SLAM——之Eigen函数库

0. Eigen/四元数/欧拉角/旋转矩阵 相关系列文章

1. SLAM——之Eigen入门（矩阵运算及几何模块）
2. SLAM——之Eigen函数库，一个相对复杂的EIgen使用实例
3. SLAM——Eigen函数库:矩阵块运算，block操作
4. 欧拉角和旋转矩阵相互转换
5. 四元数与三维向量相乘运算
6. 四元数求导

1. Eigen示例

相较于Ceres而言，Eigen函数库相对较为简单，我们上一篇文章详细描述了Ceres的使用以及注意事项，由于Ceres能够使用ceres::AutoDiffCostFunction这一类的自动求导函数，相对而言更加轻松，所以Eigen更多的是做矩阵运算。这里我们给出上一篇文章最后的Ceres求解的Eigen版本。我们可以看到本质上差别不大，只是Eigen需要自己求解雅克比矩阵J，并在用GN构建增量方程后，使用ldlt求解线性方程HX=g。

#include 
#include 
#include 
#include 
#include 

using namespace std;
using namespace Eigen;

int main(int argc, char **argv) {
    double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值
    double ae = 2.0, be = -1.0, ce = 5.0;        // 估计参数值
    int N = 100;                                 // 数据点
    double w_sigma = 1.0;                        // 噪声Sigma值
    cv::RNG rng;                                 // OpenCV随机数产生器

    vector<double> x_data, y_data;      // 数据
    for (int i = 0; i < N; i++) {
        double x = i / 100.0;
        x_data.push_back(x);
        y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma));
    }

    // 开始Gauss-Newton迭代
    int iterations = 100;    // 迭代次数
    double cost = 0, lastCost = 0;  // 本次迭代的cost和上一次迭代的cost

    for (int iter = 0; iter < iterations; iter++) {

        Matrix3d H = Matrix3d::Zero();             // Hessian = J^T J in Gauss-Newton
        Vector3d b = Vector3d::Zero();             // bias
        cost = 0;
//-----------------用GN构建增量方程，HX=g---------------------------------//
        for (int i = 0; i < N; i++) {
            double xi = x_data[i], yi = y_data[i];  // 第i个数据点
            // start your code here
            // double error = 0; // 填写计算error的表达式
            double error = yi-exp(ae * xi * xi + be * xi + ce);   // 第i个数据点的计算误差
            
            Vector3d J; // 雅可比矩阵,3x1
            J[0] = -xi*xi*exp(ae * xi * xi + be * xi + ce);  // de/da,函数求倒数，-df/da
            J[1] = -xi*exp(ae * xi * xi + be * xi + ce);;  // de/db
            J[2] = -exp(ae * xi * xi + be * xi + ce);;  // de/dc

            H += J * J.transpose(); // GN近似的H
            b += -error * J;
            // end your code here

            cost += error * error;
        }

        // 求解线性方程 Hx=b，建议用ldlt
 	// start your code here
        Vector3d dx;

        //LDL^T Cholesky求解
        // clock_t time_stt2 = clock();
        dx = H.ldlt().solve(b);//Hx=b,,,H.ldlt().solve(b)
        // cout<<"LDL^T分解，耗时:\n"<<(clock()-time_stt2)/(double)
        //     CLOCKS_PER_SEC<<"ms"<
        cout<<"\n dx:"<<dx.transpose()<<endl;
        // return 0;//一写就死

	// end your code here

        if (isnan(dx[0])) {
            cout << "result is nan!" << endl;
            break;
        }

        if (iter > 0 && cost > lastCost) {
            // 误差增长了，说明近似的不够好
            cout << "cost: " << cost << ", last cost: " << lastCost << endl;
            break;
        }

        // 更新abc估计值
        ae += dx[0];
        be += dx[1];
        ce += dx[2];

        lastCost = cost;

        cout << "total cost: " << cost << endl;
    }

    cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
    return 0;
}

值得注意的是Eigen函数库是不存在动态链接库的，我们在CMakeList.txt编译时候只需要引入include即可。

2. Eigen模块和头文件归纳

Core： #include<Eigen/Core>，包含Matrix和Array类，基础的线性代数运算和数组操作。
Geometry： #include<Eigen/Geometry>，包含旋转，平移，缩放，2维和3维的各种变换。
LU ：#include<Eigen/LU>，包含求逆，行列式，LU分解。
Cholesky： #include<Eigen/Cholesky>，包含LLT和LDLT Cholesky分解。
SVD： #include<Eigen/SVD>，包含SVD分解。
QR： #include<Eigen/QR>，包含QR分解。
Eigenvalues ：#include<Eigen/Eigenvalues>，包含特征值，特征向量分解。
Sparse： #include<Eigen/Sparse>，包含稀疏矩阵的存储和运算。
Dense ：#include<Eigen/Dense>，包含了 Core/Geometry/LU/Cholesky/SVD/QR/Eigenvalues模块。
Eigen： #include<Eigen/Eigen>，包含Dense和Sparse。

上面的代码我们即包含了上述的两个函数库。

#include 
#include 

3.Eigen基础函数总结

$矩阵(Matrix)类的函数介绍$

在Eigen中，所有矩阵和向量均为Matrix模板类的对象，向量是矩阵的行（或列）为1是的特殊情况。
$Matrix类有6个模板参数，主要使用前三个，剩下的使用默认值。$

Matrix<typename Scalar, 
       int RowsAtCompileTime, 
       int ColsAtCompileTime,
       int Options = 0,
       int MaxRowsAtCompileTime = RowsAtCompileTime,
       int MaxColsAtCompileTime = ColsAtCompileTime>
# Scalar 元素类型
# RowsAtCompileTime 行
# ColsAtCompileTime 列
# 例 typedef Matrix<int, 3, 3> Matrix3i;
# Options 比特标志位
# MaxRowsAtCompileTime和MaxColsAtCompileTime表示在编译阶段矩阵的上限。

3.1 矩阵的三参数模板

强制性的三参数模板的原型 (三个参数分别表示：标量的类型，编译时的行，编译时的列)

///tips:用typedef定义了很多模板
///例如：Matrix4f 表示 4×4 的floats 矩阵
typedef Matrix<float, 4, 4> Matrix4f;

3.2 向量（Vectors）:向量是矩阵的特殊情况，也是用矩阵定义的。

# 列向量
typedef Matrix<double, 3, 1> Vector3d;
# 行向量
typedef Matrix<float, 1, 3> RowVector3f;

3.3 特殊动态值（special value Dynamic）

Eigen的矩阵不仅能够在编译是确定大小（fixed size），也可以在运行时确定大小，就是所说的动态矩阵（dynamic size）。

typedef Matrix<double, Dynamic, Dynamic> MatrixXd;  
typedef Matrix<int, Dynamic, 1> VectorXi;  

/* 也可使用‘行’固定‘列’动态的矩阵 */
Matrix<float, 3, Dynamic>

3.4 构造函数（Constructors）

可以使用默认的构造函数，不执行动态分配内存，也没有初始化矩阵参数：

Matrix3f a;   // a是3-by-3矩阵，包含未初始化的 float[9] 数组
Eigen::Matrix3d      //旋转矩阵（3*3）
Eigen::AngleAxisd    //旋转向量（3*1）
Eigen::Vector3d      //欧拉角（3*1）
Eigen::Quaterniond   //四元数（4*1）
Eigen::Isometry3d    //欧式变换矩阵（4*4）
Eigen::Affine3d      //放射变换矩阵（4*4）
Eigen::Projective3d  //射影变换矩阵（4*4）
MatrixXf b;   // b是动态矩阵，当前大小为 0-by-0, 没有为数组的系数分配内存

/* 矩阵的第一个参数表示“行”，数组只有一个参数。根据跟定的大小分配内存，但不初始化 */
MatrixXf a(10,15);    // a 是10-by-15阵，分配了内存，没有初始化
VectorXf b(30);       // b是动态矩阵，当前大小为 30, 分配了内存，没有初始化

/* 对于给定的矩阵，传递的参数无效 */
Matrix3f a(3,3); 

/* 对于维数最大为4的向量，可以直接初始化 */
Vector2d a(5.0, 6.0);  
Vector3d b(5.0, 6.0, 7.0);  
Vector4d c(5.0, 6.0, 7.0, 8.0);

3.5 系数访问

系数都是从0开始，矩阵默认按列存储

#include 
#include 
using namespace std; using namespace Eigen;
int main()
{
    MatrixXd m(2, 2);
    m(0, 0) = 3;   m(1, 0) = 2.5;    m(0, 1) = -1;
    m(1, 1) = m(1, 0) + m(0, 1);
    cout << "Here is the matrix m:" << endl;
    cout << m << endl;

    VectorXd v(2);
    v(0) = 4;
    v[1] = v[0] - 1;     //operator[] 在 vectors 中重载，意义和()相同
    cout << "Here is the vector v:" << endl;
    cout << v << endl;
    getchar();
}

3.6 逗号分隔的初始化

Matrix3f m;
m << 1, 2, 3,   4, 5, 6,   7, 8, 9;
cout << m;

3.7、获取vector&matrix数据大小

3.7.1 默认构造时，指定大小的矩阵，只分配相应大小的空间，不进行初始化。动态大小的矩阵，则未分配空间。

[]操作符可以用于向量元素的获取，但不能用于matrix。Eigen支持以下的读/写元素语法。

matrix(i,j);
vector(i)
vector[i]
vector.x() // 第一个系数
vector.y() // 第二个系数
vector.z() // 第三个系数
vector.w() // 第四个系数

3.7.2 matrix的大小可以通过rows(), cols(), size()获取，resize()可以重新调整矩阵大小。

Matrix<double, 3, 3> A;               // 固定行列。Matrix3d一样。
Matrix<double, 3, Dynamic> B;         // 固定行，动态列。
A.resize(4, 4);   // Runtime error if assertions are on.
B.resize(4, 9);   // Runtime error if assertions are on.
A.resize(3, 3);   // Ok; size didn't change.
B.resize(3, 9);   // Ok; only dynamic cols changed.

上述的元素访问方法都通过断言检查范围，代价比较大。

通过定义EIGEN_NO_DEBUG 或 NDEBUG，取消断言。
通过使用coeff()和coeffRef()，来取消检查。比如，MatrixBase::coeff(int,int) const, MatrixBase::coeffRef(int,int)等。
$矩阵(Matrix)类的运算$
Eigen不支持类型自动转化，因此矩阵元素类型必须相同。

3.7.3 支持+, -, +=, -=, _, /, _=, /=基础四则运算。

3.7.4 转置和共轭

MatrixXcf a = MatrixXcf::Random(3,3);
a.transpose();  # 转置
a.conjugate();  # 共轭
a.adjoint();    # 共轭转置（伴随矩阵）
// 对于实数矩阵，conjugate不执行任何操作，adjoint等价于transpose
a.transposeInPlace() #原地转置

Vector3d v(1,2,3);
Vector3d w(4,5,6);
v.dot(w);    # 点积
v.cross(w);  # 叉积

Matrix2d a;
a << 1, 2, 3, 4;
a.sum();      # 所有元素求和
a.prod();     # 所有元素乘积
a.mean();     # 所有元素求平均
a.minCoeff();     # 所有元素中最小元素
a.maxCoeff();     # 所有元素中最大元素
a.trace();        # 迹，对角元素的和
// minCoeff和maxCoeff还可以返回结果元素的位置信息
int i, j;
a.minCoeff(&i, &j);

3.7.5 $Array类$

Array是个类模板，前三个参数必须指定，后三个参数可选。和Matrix对比，Matrix的运算遵守矩阵运算规则，Array则提供更加灵活的运算，比如对应系数相乘，向量加数量等。

Array<typename Scalar,
      int RowsAtCompileTime,
      int ColsAtCompileTime>
# 常见类定义
typedef Array<float, Dynamic, 1> ArrayXf
typedef Array<float, 3, 1> Array3f
typedef Array<double, Dynamic, Dynamic> ArrayXXd
typedef Array<double, 3, 3> Array33d

ArrayXf a = ArrayXf::Random(5);
a.abs();    # 绝对值
a.sqrt();    # 平方根
a.min(a.abs().sqrt());  # 两个array相应元素的最小值

当执行array_array时，执行的是相应元素的乘积，所以两个array必须具有相同的尺寸。
Matrix对象——>Array对象：.array() 函数
Array对象——>Matrix对象：_*.matrix()__ 函数

3.7.6 $块操作$

块是matrix或array中的矩形子块。

// 方法1
.block(i, j, p, q)    //起点(i, j)，块大小(p, q)，构建一个动态尺寸的block
.block<p, q>(i, j)  // 构建一个固定尺寸的block

matrix.row(i): 矩阵第i行
matrix.col(j): 矩阵第j列

角相关操作

Vector的块操作

Matrix3f P;                     // 3x3 float matrix.
Vector3f x;                     // 3x1 float matrix.

x.head(n)                          // x(1:n)
x.head<n>()                        // x(1:n)
x.tail(n)                          // x(end - n + 1: end)
x.tail<n>()                        // x(end - n + 1: end)
x.segment(i, n)                    // x(i+1 : i+n)
x.segment<n>(i)                    // x(i+1 : i+n)


P.block(i, j, rows, cols)          // P(i+1 : i+rows, j+1 : j+cols)
P.block<rows, cols>(i, j)          // P(i+1 : i+rows, j+1 : j+cols)
P.row(i)                           // P(i+1, :)
P.col(j)                           // P(:, j+1)
P.leftCols<cols>()                 // P(:, 1:cols)
P.leftCols(cols)                   // P(:, 1:cols)
P.middleCols<cols>(j)              // P(:, j+1:j+cols)
P.middleCols(j, cols)              // P(:, j+1:j+cols)
P.rightCols<cols>()                // P(:, end-cols+1:end)
P.rightCols(cols)                  // P(:, end-cols+1:end)
P.topRows<rows>()                  // P(1:rows, :)
P.topRows(rows)                    // P(1:rows, :)
P.middleRows<rows>(i)              // P(i+1:i+rows, :)
P.middleRows(i, rows)              // P(i+1:i+rows, :)
P.bottomRows<rows>()               // P(end-rows+1:end, :)
P.bottomRows(rows)                 // P(end-rows+1:end, :)
P.topLeftCorner(rows, cols)        // P(1:rows, 1:cols)
P.topRightCorner(rows, cols)       // P(1:rows, end-cols+1:end)
P.bottomLeftCorner(rows, cols)     // P(end-rows+1:end, 1:cols)
P.bottomRightCorner(rows, cols)    // P(end-rows+1:end, end-cols+1:end)
P.topLeftCorner<rows,cols>()       // P(1:rows, 1:cols)
P.topRightCorner<rows,cols>()      // P(1:rows, end-cols+1:end)
P.bottomLeftCorner<rows,cols>()    // P(end-rows+1:end, 1:cols)
P.bottomRightCorner<rows,cols>()   // P(end-rows+1:end, end-cols+1:end)

3.7.7 $特殊矩阵$

特殊矩阵

零阵：类静态成员函数Zero()
常量矩阵：Constant(rows, cols, value)
随机矩阵：Random()
单位矩阵：Identity()
构建从low到high等间距的size长度的序列，适用于vector和一维数组:LinSpaced(size, low, high)
功能函数

setZero()
setIdentity()

Matrix<double, dynamic,="" dynamic=""> C; // Full dynamic. Same as MatrixXd.
MatrixXd::Identity(rows,cols) // eye(rows,cols)
C.setIdentity(rows,cols) // C = eye(rows,cols)
MatrixXd::Zero(rows,cols) // zeros(rows,cols)
C.setZero(rows,cols) // C = zeros(rows,cols)
MatrixXd::Ones(rows,cols) // ones(rows,cols)
C.setOnes(rows,cols) // C = ones(rows,cols)
MatrixXd::Random(rows,cols) // rand(rows,cols)_2-1 // MatrixXd::Random returns uniform random numbers in (-1, 1).
C.setRandom(rows,cols) // C = rand(rows,cols)_2-1
VectorXd::LinSpaced(size,low,high) // linspace(low,high,size)’
v.setLinSpaced(size,low,high) // v = linspace(low,high,size)’
VectorXi::LinSpaced(((hi-low)/step)+1, // low:step:hi
low,low+step*(size-1)) //

3.7.8 $归约，迭代器，广播$

• 范数计算

  • squaredNorm()：L2范数，等价于计算vector自身点积
    

  • norm()：返回`squareNorm的开方根
  • .lpNorm

    ()：p范数，p可以取Infinity，表无穷范数

Vector3f x;                     // 3x1 float matrix.
x.norm()                  // norm(x).    注意norm(R)在Eigen中不起作用。
x.squaredNorm()           // dot(x, x)   注意，对于复数并不成立
x.dot(y)                  // dot(x, y)
x.cross(y)                // cross(x, y) 需要 #include 

布尔归约
all()=true: matrix或array中所有元素为true
any()=true: 到少有一个为true
count(): 返回true元素个数

// sample
ArrayXXf A(2, 2);
A << 1,2,3,4;
(A > 0).all();
(A > 0).any();
(A > 0).count();

迭代器，获取某元素位置

// Reductions.
int r, c;
// Eigen                  // Matlab
R.minCoeff()              // min(R(:))
R.maxCoeff()              // max(R(:))
s = R.minCoeff(&r, &c)    // [s, i] = min(R(:)); [r, c] = ind2sub(size(R), i);
s = R.maxCoeff(&r, &c)    // [s, i] = max(R(:)); [r, c] = ind2sub(size(R), i);
R.sum()                   // sum(R(:))
R.colwise().sum()         // sum(R)
R.rowwise().sum()         // sum(R, 2) or sum(R')'
R.prod()                  // prod(R(:))
R.colwise().prod()        // prod(R)
R.rowwise().prod()        // prod(R, 2) or prod(R')'
R.trace()                 // trace(R)
R.all()                   // all(R(:))
R.colwise().all()         // all(R)
R.rowwise().all()         // all(R, 2)
R.any()                   // any(R(:))
R.colwise().any()         // any(R)
R.rowwise().any()         // any(R, 2)

部分归约

// sample
Eigen::MatrixXf mat(2,3);
mat << 1,2,3,
       4,5,6;
std::cout << mat.colwise().maxCoeff();
// output: 4, 5, 6
// mat.rowWise() the same as before

广播，针对vector，沿行或列重复构建一个matrix

// sample
Eigen::MatrixXf mat(2,3);
Eigen::VectorXf v(2);

mat << 1,2,3,4,5,6;
v << 0,1;
mat.colwise() += v;
// output: 1, 2, 3, 5, 6, 7

Map,用于利用数据的内在，并将其转为Eigen类型。

// Eigen can map existing memory into Eigen matrices.
float array[3];
Vector3f::Map(array).fill(10);            // 数组上创建临时Map，并将vector大小设置为10
int data[4] = {1, 2, 3, 4};
Matrix2i mat2x2(data);                    // copies data into mat2x2
Matrix2i::Map(data) = 2*mat2x2;           // overwrite elements of data with 2*mat2x2
MatrixXi::Map(data, 2, 2) += mat2x2;      // adds mat2x2 to elements of data (alternative syntax if size is not know at compile time)

3.7.9 Eigen分解函数总结

x = A.ldlt().solve(b));  // A sym. p.s.d.    #include 
x = A.llt() .solve(b));  // A sym. p.d.      #include 
x = A.lu()  .solve(b));  // Stable and fast. #include 
x = A.qr()  .solve(b));  // No pivoting.     #include 
x = A.svd() .solve(b));  // Stable, slowest. #include 

3.7.10 上面是一些常用的分解函数，下面则是对这些函数的具体使用。

#include 
#include 

//g++ Linear_algebra_and_decompositions.cpp -o la -I/download/eigen

using namespace std;
using namespace Eigen;


//QR方法解线性方程组
int main()
{
   Matrix3f A;
   Vector3f b;
   A << 1,2,3,  4,5,6,  7,8,10;
   b << 3, 3, 4;
   cout << "Here is the matrix A:\n" << A << endl;
   cout << "Here is the vector b:\n" << b << endl;
   Vector3f x = A.colPivHouseholderQr().solve(b);
   cout << "The solution is:\n" << x << endl;
}


//矩阵求逆
int main()
{
   Matrix2f A, b;
   A << 2, -1, -1, 3;
   b << 1, 2, 3, 1;
   cout << "Here is the matrix A:\n" << A << endl;
   cout << "Here is the right hand side b:\n" << b << endl;
   Matrix2f x = A.ldlt().solve(b);
   cout << "The solution is:\n" << x << endl;
}


//计算数值法求解和真实值的残差
int main()
{
   MatrixXd A = MatrixXd::Random(100,100);
   MatrixXd b = MatrixXd::Random(100,50);
   MatrixXd x = A.fullPivLu().solve(b);
   double relative_error = (A*x - b).norm() / b.norm(); // norm() is L2 norm
   cout << "The relative error is:\n" << relative_error << endl;
}


//计算特征值和特征向量
int main()
{
   Matrix2f A;
   A << 1, 2, 2, 3;
   cout << "Here is the matrix A:\n" << A << endl;
   SelfAdjointEigenSolver<Matrix2f> eigensolver(A);
   if (eigensolver.info() != Success) abort();
   cout << "The eigenvalues of A are:\n" << eigensolver.eigenvalues() << endl;
   cout << "Here's a matrix whose columns are eigenvectors of A \n"
        << "corresponding to these eigenvalues:\n"
        << eigensolver.eigenvectors() << endl;
}


//计算逆行列式
int main()
{
   Matrix3f A;
   A << 1, 2, 1,
        2, 1, 0,
        -1, 1, 2;
   cout << "Here is the matrix A:\n" << A << endl;
   cout << "The determinant of A is " << A.determinant() << endl;
   cout << "The inverse of A is:\n" << A.inverse() << endl;
}


//最小二乘解
int main()
{
   MatrixXf A = MatrixXf::Random(3, 2);
   cout << "Here is the matrix A:\n" << A << endl;
   VectorXf b = VectorXf::Random(3);
   cout << "Here is the right hand side b:\n" << b << endl;
   cout << "The least-squares solution is:\n"
        << A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b) << endl;
}


//分离矩阵计算与构造(解耦合)
int main()
{
   Matrix2f A, b;
   LLT<Matrix2f> llt;
   A << 2, -1, -1, 3;
   b << 1, 2, 3, 1;
   cout << "Here is the matrix A:\n" << A << endl;
   cout << "Here is the right hand side b:\n" << b << endl;
   cout << "Computing LLT decomposition..." << endl;
   llt.compute(A);
   cout << "The solution is:\n" << llt.solve(b) << endl;
   A(1,1)++;
   cout << "The matrix A is now:\n" << A << endl;
   cout << "Computing LLT decomposition..." << endl;
   llt.compute(A);
   cout << "The solution is now:\n" << llt.solve(b) << endl;
}


// Rank-revealing分解
int main()
{
   Matrix3f A;
   A << 1, 2, 5,
        2, 1, 4,
        3, 0, 3;
   cout << "Here is the matrix A:\n" << A << endl;
   FullPivLU<Matrix3f> lu_decomp(A);
   cout << "The rank of A is " << lu_decomp.rank() << endl;
   cout << "Here is a matrix whose columns form a basis of the null-space of A:\n"
        << lu_decomp.kernel() << endl;
   cout << "Here is a matrix whose columns form a basis of the column-space of A:\n"
        << lu_decomp.image(A) << endl; // yes, have to pass the original A
}


//LU分解，设定阈值求秩
int main()
{
   Matrix2d A;
   A << 2, 1,
        2, 0.9999999999;
   FullPivLU<Matrix2d> lu(A);
   cout << "By default, the rank of A is found to be " << lu.rank() << endl;
   lu.setThreshold(1e-5);
   cout << "With threshold 1e-5, the rank of A is found to be " << lu.rank() << endl;

4. eigen example

CMakeLists.txt

# cmake最低版本号要求
cmake_minimum_required(VERSION 3.14)

# 项目名称
project(eigen_demo)

# 设置Eigen3_DIR所在目录，对应eigen安装目录下的cmake目录，目录内包含有Eigen3Config.cmake等文件
set(Eigen3_DIR "./install/share/eigen3/cmake")

# 搜索查询Eigen3
find_package(Eigen3 REQUIRED NO_MODULE)

# 添加以main.cpp文件为基础的可执行目标文件
add_executable(eigen_base eigen_base.cpp)
# 为项目可行执行文件引入eigen依赖
target_link_libraries(eigen_base Eigen3::Eigen)


add_executable(eigen_geometry eigenGeometry.cpp)
# 为项目可行执行文件引入eigen依赖
target_link_libraries(eigen_geometry Eigen3::Eigen)

eigenGeometry.cpp

#include 
#include 
using namespace std;

#include 
// Eigen 几何模块
#include 

/****************************
 * 本程序演示了 Eigen 几何模块的使用方法
 ****************************/

int main ( int argc, char** argv )
{
    // ----------  初始化 -----------//
            //角轴          罗德公式转换    eulerAngles/geometry relationship
    //旋转向量<======>旋转矩阵<======>四元数<======>欧拉角


    // 旋转向量（轴角）：沿Z轴旋转45°
    Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) );
    cout<<"rotation_vector axis = \n" << rotation_vector.axis() <<"\n rotation_vector angle = "
<< rotation_vector.angle()<<endl;
    cout << "旋转向量 to 旋转矩阵如下：\n" << rotation_vector.matrix() << endl;  //旋转向量转为旋转矩阵
    cout << ".........." << endl;
    
    Eigen::Quaterniond quat2 = Eigen::Quaterniond(-1, 0, 1, 1);
    quat2.normalize();  //四元数归一化
    // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
    cout<<"四元数归一化：quaternion = \n"<<quat2.coeffs() <<endl;   
    
    //旋转矩阵：沿Z轴旋转45°
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    rotation_matrix <<  0.707, -0.707,      0,
    0.707,  0.707,      0,
    0,      0,          1;
    cout<<"rotation matrix =\n"<<rotation_matrix <<endl;

    cout<<"-----------------由罗德公式转换-------------：\n"<<endl;

    //由罗德公式转换：from rotation_vector to quaterniond
    Eigen::Matrix<double, 3, 3> matrix_33;
    Eigen::Matrix<double, 3, 3> matrix_axis_inverse_symmetry;
    Eigen::Vector3d temp = rotation_vector.axis();
    double n1 = temp(0);
    double n2 = temp(1);
    double n3 = temp(2);
    matrix_axis_inverse_symmetry << 0, -n3, n2, n3, 0, -n1, -n2, n1, 0; //反对称阵
    Eigen::Matrix3d rotation_matrix_1 = Eigen::Matrix3d::Identity(); //单位阵
    matrix_33 = cos(rotation_vector.angle())*rotation_matrix_1 + (1-cos(rotation_vector.angle()))*
rotation_vector.axis()*rotation_vector.axis().transpose()+sin(rotation_vector.angle())*matrix_axis_inverse_symmetry;
cout << ".....罗德公式转换结果：....." << endl;
    cout << matrix_33 << endl;
    
    // 四元数：沿Z轴旋转45°
    Eigen::Quaterniond quat = Eigen::Quaterniond(cos(rotation_vector.angle()/2),n1*sin(rotation_vector.angle()/2),
n2*sin(rotation_vector.angle()/2),n3*sin(rotation_vector.angle()/2));  //实部在第一个,直接计算
     // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
    cout<<"四元数输出方法1：quaternion = \n"<<quat.coeffs() <<endl;  
    
    Eigen::Quaterniond quat1 = Eigen::Quaterniond(rotation_vector);  //直接通过旋转向量得到
    cout << "coeffs()输出四元数为：\n" << quat1.coeffs() << endl;  //注意coeffs的顺序是（x,y,z,w）
    cout<<"四元数输出方法3\n x = " << quat1.x() << "\n y = " << quat1.y() << "\n z = " << quat1.z() << "\n 实部w = " << quat1.w() << endl;
    
    Eigen::Quaterniond quat3 = Eigen::Quaterniond(rotation_matrix);  //直接通过旋转矩阵得到
    cout << "通过旋转矩阵输出四元数为：\n" << quat3.coeffs() << endl;  //注意coeffs的顺序是（x,y,z,w）

    
    // 欧拉角: ：沿Z轴旋转45°
    Eigen::Vector3d euler_angles = Eigen::Vector3d(M_PI/4, 0, 0);// ZYX顺序，即roll pitch yaw顺序
    cout<<"Euler: yaw pitch roll = "<<euler_angles.transpose()<<endl;
    Eigen::Vector3d euler_angles1 = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX顺序，即roll pitch yaw顺序
    cout<<"yaw pitch roll = "<<euler_angles1.transpose()<<endl;
    
    return 0;
}

5.参考链接

https://www.guyuehome.com/34670