SLAM知识点——Eigen旋转量间变换求解、变换矩阵求解

0 前言

下面内容包含头文件如下：

#include
#include              //核心矩阵运算库（Vector3d，Matrix3d）
#include          // 稠密矩阵的代数运算（逆和特征值）
#include  // 引入旋转平移（旋转矩阵、旋转向量、欧拉角、四元数、平移向量）
#include

using namespace cv;
using namespace std;

#define DEG2RAD(x) ((x)*0.017453293)
1
2
3
4
5
6
7
8
9
10

1 旋转向量间变换求解

旋转量包括：欧拉角、旋转矩阵、旋转向量、四元数

1.1 欧拉角

1.1.1 欧拉角 -> 旋转矩阵

  float init_roll = 1.4162955503546129e+00,init_pitch=1.9927596853157299e+00,init_yaw = -5.5358076219140663e+01;

  Eigen::AngleAxisf init_rotation_x(DEG2RAD(init_roll), Eigen::Vector3f::UnitX());
  Eigen::AngleAxisf init_rotation_y(DEG2RAD(init_pitch), Eigen::Vector3f::UnitY());
  Eigen::AngleAxisf init_rotation_z(DEG2RAD(init_yaw), Eigen::Vector3f::UnitZ());

  Eigen::Matrix3f R_M;
  R_M=init_rotation_z*init_rotation_y*init_rotation_x;
  std::cout<<"R_M: "<<std::endl<<R_M<<std::endl;

  /*
      R_M: 
          0.568102     0.822958 -0.000574131
        -0.822223     0.567565   -0.0426499
        -0.0347732    0.0247016      0.99909
  */
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

欧拉角转换成旋转矩阵（相对于世界坐标系的旋转矩阵）通常是按外旋方式（绕固定轴），X-Y-Z顺序。 外旋是左乘，旋转顺序x-y-z（rpy），所以是先R(x)，再R(y)*R(x)，最后R(z)*R(y)*R(x);

参考：欧拉角和旋转矩阵之间的转换

1.2 旋转矩阵

1.2.1 旋转矩阵 -> 欧拉角

  tf::Matrix3x3 M;
  M.setValue(5.6810212543969762e-01, 8.2295789186479229e-01,  -5.7413088648496612e-04, -8.2222312468457914e-01,  5.6756515720693990e-01, -4.2649893058088473e-02,  -3.4773207542891066e-02, 2.4701553819839669e-02,  9.9908995263011369e-01);
  double roll, pitch, yaw;
  M.getRPY(roll, pitch, yaw);
  std::cout<<"eulerAngle:  "<<roll* 180 / M_PI<<" "<<pitch* 180 / M_PI<<" "<<yaw* 180 / M_PI<<std::endl;
    /*
      eulerAngle:  1.4163 1.99276 -55.3581
  */
1
2
3
4
5
6
7
8

• 使用tf最有效！#include
• 使用eigen库，该方法行不通！！eigen将matrix转欧拉角会出错。
  ypr = R_AB.eulerAngles(2, 1, 0);
• 使用公式法计算，但是准确性不够:

  //公式法
  float T[9] = {5.6810212543969762e-01, 8.2295789186479229e-01,  -5.7413088648496612e-04, -8.2222312468457914e-01, 5.6756515720693990e-01, -4.2649893058088473e-02,  -3.4773207542891066e-02, 2.4701553819839669e-02,   9.9908995263011369e-01};
  float E[3];
  E[0] = atan2(T[1],T[0]);//Ny/Nx
  E[1] = atan2(-1*T[2],(T[0]*cos(E[0])+T[1]*sin(E[0])));//Nz/(Nx+Ny)
  E[2] = atan2((T[6]*sin(E[0])-T[7]*cos(E[0])),(-1*T[3]*sin(E[0])+T[4]*cos(E[0])));//Ax-Ay/-Ox+Oy
  std::cout<<E[0]* 180 / M_PI<<" "<<E[1]* 180 / M_PI<<" "<<E[2]* 180 / M_PI<<std::endl;
1
2
3
4
5
6
7

2 变换矩阵求解

2.1 欧拉角+平移向量 -> 变换矩阵

  float init_x=-1.0393021697572675e+00, init_y=2.5139219579635164e-03, init_z = -0.177126;
  float init_roll = 1.4162955503546129e+00,init_pitch=1.9927596853157299e+00,init_yaw = -5.5358076219140663e+01;
  
  Eigen::Translation3f init_translation(init_x, init_y, init_z);

  Eigen::AngleAxisf init_rotation_x(DEG2RAD(init_roll), Eigen::Vector3f::UnitX());
  Eigen::AngleAxisf init_rotation_y(DEG2RAD(init_pitch), Eigen::Vector3f::UnitY());
  Eigen::AngleAxisf init_rotation_z(DEG2RAD(init_yaw), Eigen::Vector3f::UnitZ());

  Eigen::Matrix4f  T_M =  Eigen::Matrix4f::Identity();
  T_M = (init_translation * init_rotation_z * init_rotation_y * init_rotation_x).matrix();  //current_guess_为ndt计算的初始换变换估计位置，4*4矩阵
  std::cout<<"T_M:  "<<std::endl<<T_M<<std::endl;

  /*
      T_M:  
          0.568102     0.822958 -0.000574132      -1.0393
        -0.822223     0.567565   -0.0426499   0.00251392
        -0.0347732    0.0247016      0.99909    -0.177126
                0            0            0            1
  */
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

对比1.1.1部分，可以看到R_M是T_M的左上角3x3矩阵。

2.2 旋转矩阵+平移向量 -> 变换矩阵

包含头文件：#include

  Eigen::Matrix3d R_M;
  R_M<<2.7137982845306396e-01, -9.6243983507156372e-01,   7.9119475558400154e-03, 9.6094810962677002e-01,   2.7047842741012573e-01, -5.8482807129621506e-02,   5.4146166890859604e-02, 2.3474026471376419e-02,    9.9825710058212280e-01;
  Eigen::Vector3d t(-6.2644982337951660e-01, 9.2981266975402832e-01,   5.9500701725482941e-02);
  Eigen::Isometry3d  T_M3=Eigen::Isometry3d::Identity();   // T_M3是一个4x4的矩阵
  T_M3.rotate (R_M);
  T_M3.pretranslate (t);
  std::cout<<"T_M3: "<<std::endl<<T_M3.matrix()<<std::endl;
  Eigen::Matrix4d T_M4 =  Eigen::Matrix4d::Identity();
  T_M4 = T_M3.matrix(); 
  std::cout<<"T_M4: "<<std::endl<<T_M4<<std::endl;
  /*
    T_M3:
        0.27138   -0.96244 0.00791195   -0.62645
        0.960948   0.270478 -0.0584828   0.929813
        0.0541462   0.023474   0.998257  0.0595007
        0          0          0          1
  T_M4: 
        0.27138   -0.96244 0.00791195   -0.62645
        0.960948   0.270478 -0.0584828   0.929813
        0.0541462   0.023474   0.998257  0.0595007
        0          0          0          1
  */
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

更多变换参考：
[1] https://blog.csdn.net/hero_cjn/article/details/105229484