Eigen库函数实现欧拉角与旋转矩阵转换

一、简介

Eigen库是一个开源的C++线性代数库，它提供了快速的有关矩阵的线性代数运算，还包括解方程等功能。Eigen是一个用纯头文件搭建起来的库，这意味这你只要能找到它的头文件，就能使用它。Eigen头文件的默认位置是“/usr/include/eigen3”.

由于Eigen库相较于OpenCV中的Mat等库而言更加高效，许多上层的软件库也使用Eigen进行矩阵运算，比如SLAM中常用的g2o,Sophus等。此外Eigen库还被被用于Caffe，Tensorflow等许多深度学习相关的框架中。

Eigen申明变量时有点类似于c语言，类型在变量的前面，而opencv中Mat申明变量时是c++中直接制定获取是构造函数中构造类型。

二、Eigen的学习

1. 向量矩阵基本操作

• 向量

Eigen::Vector3d v3d;  //向量
Eigen::VectorXd v(6);  //需要指定大小(6)
v3d << 1,2,3;
cout << "向量 v3d = "<< endl << v3d << endl << endl;
v << 1,2,3,4,5,6;
cout << "向量 v = "<< endl << v << endl << endl;

输出：

向量 v3d = 
1
2
3

向量 v = 
1
2
3
4
5
6
 

• 矩阵基本操作

Eigen::Matrix m_f23;  //行、列
Eigen::MatrixXd m_d24(2,4);
Eigen::Matrix3d m_d33;
m_f23 << 1,2,3,
         4,5,6;
cout << "矩阵 m_f23 = "<< endl << m_f23 << endl << endl;
m_d24 << 1,2,3,4,
         5,6,7,8;
cout << "矩阵 m_d24 = "<< endl << m_d24 << endl << endl;
m_d33 << 1,2,3,
         4,5,6,
         7,8,9;
cout << "矩阵 m_d33 = "<< endl << m_d33 << endl << endl;

输出：

矩阵 m_f23 = 
1 2 3
4 5 6

矩阵 m_d24 = 
1 2 3 4
5 6 7 8

矩阵 m_d33 = 
1 2 3
4 5 6
7 8 9

• 矩阵利用函数进行赋值

Eigen::Matrix4d m_d44 = Eigen::Matrix4d::Zero();
cout << "Zero 矩阵 m_d44 = " << endl << m_d44 << endl << endl;
m_d44 = Eigen::Matrix4d::Random();
cout << "矩阵 m_d44 = "<< endl << m_d44 << endl << endl;
Eigen::MatrixXd m_dnn(5,5);
m_dnn = Eigen::MatrixXd::Random(5, 5);
cout << "Random 矩阵 m_dnn = "<< endl << m_dnn << endl << endl;

输出：

Zero 矩阵 m_d44 = 
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0

矩阵 m_d44 = 
  0.680375   0.823295  -0.444451  -0.270431
 -0.211234  -0.604897    0.10794  0.0268018
  0.566198  -0.329554 -0.0452059   0.904459
   0.59688   0.536459   0.257742    0.83239

Random 矩阵 m_dnn = 
 0.271423 -0.514226 -0.740419  0.678224 -0.012834
 0.434594 -0.725537 -0.782382   0.22528   0.94555
-0.716795  0.608354  0.997849 -0.407937 -0.414966
 0.213938 -0.686642 -0.563486  0.275105  0.542715
 

• 获取指定元素的值

for(int i = 0 ;i < 5; i++){
    for(int j = 0; j < 5; j++){
        cout << m_dnn(i,j) << "," ;
    }
    cout << endl << endl;
}

• 获取矩阵的子矩阵（block）

Eigen::MatrixXf m_block(5,5);
m_block <<1,2,3,4,5,
    6,7,8,9,10,
    11,12,13,14,15,
    16,17,18,19,20,
    21,22,23,24,25;
Eigen::MatrixXf m_sun1 = m_block.block<2,2>(1,1);//从1行1列开始，获取2*2的子矩阵矩阵
cout << "block 矩阵 m_block.block<2,2>(1,1) = "<< endl << m_sun1 << endl << endl;
Eigen::MatrixXf m_sun2 = m_block.block(1,1,2,2);//同上
cout << "block 矩阵 m_block.block(1,1,2,2) = "<< endl << m_sun2 << endl << endl;

cout << "矩阵 m_block m_block.row(0) = "<< endl << m_block.row(0) << endl << endl;
cout << "矩阵 m_block m_block.col(0) = "<< endl << m_block.col(0) << endl << endl;

输出：

block 矩阵 m_block.block<2,2>(1,1) = 
 7  8
12 13

block 矩阵 m_block.block(1,1,2,2) = 
 7  8
12 13

矩阵 m_block m_block.row(0) = 
1 2 3 4 5

矩阵 m_block m_block.col(0) = 
 1
 6
11
16
21

• 矩阵组合操作

Eigen::Matrix3f R;
R << 1,2,3,4,5,6,7,8,9;
Eigen::Matrix T;
T << 11,11,11;
Eigen::Matrix B;
B << 22,22,22,22;
Eigen::Matrix H;
H << R,T,B;
cout << "其次矩阵 H = "<< endl << H << endl << endl;
Eigen::Matrix newH;
newH << H.block(0,0,1,4),B;
cout << "其次矩阵 newH = "<< endl << newH << endl << endl;

输出：

其次矩阵 H = 
 1  2  3 11
 4  5  6 11
 7  8  9 11
22 22 22 22

其次矩阵 newH = 
 1  2  3 11
22 22 22 22
 

2.矩阵进行运算

• 矩阵相乘

Eigen::Matrix3d m1,m2;
m1 << 1,0,0,
        0,1,0,
        0,0,2;
m2 << 1,2,3,
        4,5,6,
        7,8,9;
cout << "m1 * m2 = "<< endl << m1 * m2 << endl << endl;

• 矩阵转置

cout << "转置  m2.transpose() = "<< endl << m2.transpose() << endl << endl;

• 矩阵求和

cout << "求和  m2.sum() = "<< endl << m2.sum() << endl << endl;

• 矩阵求逆

cout << "求逆  m2.inverse() = "<< endl << m2.inverse() << endl << endl;

• 矩阵迹

cout << "迹  m2.trace() = "<< endl << m2.trace() << endl << endl;

• 矩阵数乘

cout << "数乘  10*m2 = "<< endl << 10 * m2 << endl << endl;

• 矩阵行列式

cout << "行列式  m2.determinant() = "<< endl << m2.determinant() << endl << endl;

m1 * m2 = 
 1  2  3
 4  5  6
14 16 18

转置  m2.transpose() = 
1 4 7
2 5 8
3 6 9

求和  m2.sum() = 
45

求逆  m2.inverse() = 
-inf  inf -inf
 inf -inf  inf
-inf  inf -inf

迹  m2.trace() = 
15

数乘  10*m2 = 
10 20 30
40 50 60
70 80 90

行列式  m2.determinant() = 
0

3、代码

#include 
#include 
#include 

using namespace std;

int main()
{
    /*+++++++++++++++++++++++++++              一、向量矩阵基本操作            ++++++++++++++++**/
    //1、向量
    Eigen::Vector3d v3d;  //向量
    Eigen::VectorXd v(6);  //需要指定大小(6)

    v3d << 1,2,3;
    cout << "向量 v3d = "<< endl << v3d << endl << endl;
    v << 1,2,3,4,5,6;
    cout << "向量 v = "<< endl << v << endl << endl;

    //2、矩阵基本操作
    Eigen::Matrix m_f23;  //行、列
    Eigen::MatrixXd m_d24(2,4);
    Eigen::Matrix3d m_d33;

    m_f23 << 1,2,3,
             4,5,6;
    cout << "矩阵 m_f23 = "<< endl << m_f23 << endl << endl;
    m_d24 << 1,2,3,4,
             5,6,7,8;
    cout << "矩阵 m_d24 = "<< endl << m_d24 << endl << endl;
    m_d33 << 1,2,3,
             4,5,6,
             7,8,9;
    cout << "矩阵 m_d33 = "<< endl << m_d33 << endl << endl;


    //3、矩阵利用函数进行赋值
    Eigen::Matrix4d m_d44 = Eigen::Matrix4d::Zero();
    cout << "Zero 矩阵 m_d44 = " << endl << m_d44 << endl << endl;
    m_d44 = Eigen::Matrix4d::Random();
    cout << "矩阵 m_d44 = "<< endl << m_d44 << endl << endl;
    Eigen::MatrixXd m_dnn(5,5);
    m_dnn = Eigen::MatrixXd::Random(5, 5);
    cout << "Random 矩阵 m_dnn = "<< endl << m_dnn << endl << endl;

    //4、获取指定元素的值
    for(int i = 0 ;i < 5; i++){
        for(int j = 0; j < 5; j++){
            cout << m_dnn(i,j) << "," ;
        }
        cout << endl << endl;
    }
    //5、获取矩阵的子矩阵（block）
    Eigen::MatrixXf m_block(5,5);
    m_block <<1,2,3,4,5,
        6,7,8,9,10,
        11,12,13,14,15,
        16,17,18,19,20,
        21,22,23,24,25;
    Eigen::MatrixXf m_sun1 = m_block.block<2,2>(1,1);//从1行1列开始，获取2*2的子矩阵矩阵
    cout << "block 矩阵 m_block.block<2,2>(1,1) = "<< endl << m_sun1 << endl << endl;
    Eigen::MatrixXf m_sun2 = m_block.block(1,1,2,2);//同上
    cout << "block 矩阵 m_block.block(1,1,2,2) = "<< endl << m_sun2 << endl << endl;

    cout << "矩阵 m_block m_block.row(0) = "<< endl << m_block.row(0) << endl << endl;
    cout << "矩阵 m_block m_block.col(0) = "<< endl << m_block.col(0) << endl << endl;
    //6、矩阵组合操作
    Eigen::Matrix3f R;
    R << 1,2,3,4,5,6,7,8,9;
    Eigen::Matrix T;
    T << 11,11,11;
    Eigen::Matrix B;
    B << 22,22,22,22;
    Eigen::Matrix H;
    H << R,T,B;
    cout << "其次矩阵 H = "<< endl << H << endl << endl;
    Eigen::Matrix newH;
    newH << H.block(0,0,1,4),B;
    cout << "其次矩阵 newH = "<< endl << newH << endl << endl;
   /*------------------             end          -----------------**/

   /*+++++++++++++++++++++++++++            二、矩阵进行运算          ++++++++++++++++**/


    // 1、矩阵相乘
    Eigen::Matrix3d m1,m2;
    m1 << 1,0,0,
            0,1,0,
            0,0,2;
    m2 << 1,2,3,
            4,5,6,
            7,8,9;
    cout << "m1 * m2 = "<< endl << m1 * m2 << endl << endl;

    //2、矩阵转置
    cout << "转置  m2.transpose() = "<< endl << m2.transpose() << endl << endl;

    //3、矩阵求和
    cout << "求和  m2.sum() = "<< endl << m2.sum() << endl << endl;

    //4、矩阵求逆
    cout << "求逆  m2.inverse() = "<< endl << m2.inverse() << endl << endl;

    //5、矩阵迹
    cout << "迹  m2.trace() = "<< endl << m2.trace() << endl << endl;

    //6、矩阵数乘
    cout << "数乘  10*m2 = "<< endl << 10 * m2 << endl << endl;

    //7、矩阵行列式
    cout << "行列式  m2.determinant() = "<< endl << m2.determinant() << endl << endl;


    return 0;
}

三、矩阵之间的转换

1、欧拉角转旋转矩阵

//这里theta也是弧度制
static Eigen::Matrix3d eul2rotm(Eigen::Vector3d &theta,string m_sSeq)
{
    Eigen::Matrix3d rotX;    // 计算旋转矩阵的X分量
    rotX <<
            1,              0,               0,
            0,  cos(theta[0]),  -sin(theta[0]),
            0,  sin(theta[0]),   cos(theta[0]);

    Eigen::Matrix3d rotY;    // 计算旋转矩阵的Y分量
    rotY <<
            cos(theta[1]),   0, sin(theta[1]),
            0,   1,             0,
            -sin(theta[1]),  0, cos(theta[1]);

    Eigen::Matrix3d rotZ;    // 计算旋转矩阵的Z分量
    rotZ <<
            cos(theta[2]), -sin(theta[2]), 0,
            sin(theta[2]),  cos(theta[2]), 0,
            0,              0,             1;

    Eigen::Matrix3d R;
    if (m_sSeq == "zyx")
    {
        R = rotX * rotY * rotZ;
    }
    else if (m_sSeq == "yzx")
    {
        R = rotX * rotZ * rotY;
    }
    else if (m_sSeq == "zxy")
    {
        R = rotY * rotX * rotZ;
    }
    else if (m_sSeq == "xzy")
    {
        R = rotY * rotZ * rotX;
    }
    else if (m_sSeq == "yxz")
    {
        R = rotZ * rotX * rotY;
    }
    else if (m_sSeq == "xyz")
    {
        R = rotZ * rotY * rotX;
    }

    return R;
}

2、旋转矩阵转欧拉角

static Eigen::Vector3d rotm2eul(Eigen::Matrix3d &R)
{
    double sy = sqrt(R(0,0) * R(0,0) + R(1,0) * R(1,0));
    bool singular = sy < 1e-6;
    double x, y, z;
    if (!singular)
    {
        x = atan2( R(2,1), R(2,2));
        y = atan2(-R(2,0), sy);
        z = atan2( R(1,0), R(0,0));
    }
    else
    {
        x = atan2(-R(1,2), R(1,1));
        y = atan2(-R(2,0), sy);
        z = 0;
    }
    return {x, y, z};
}