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

0 前言

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

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

using namespace cv;
using namespace std;

#define DEG2RAD(x) ((x)*0.017453293)

1 旋转向量间变换求解

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

1.1 欧拉角

1.1.1 欧拉角 -> 旋转矩阵

1）Eigen库将欧拉角转换为旋转矩阵

  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
  */

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

2）自定义函数将欧拉角转为旋转矩阵

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

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

    Eigen::Matrix3d R_z;    // 计算旋转矩阵的Z分量
    R_z <<
            cos(theta[2]), -sin(theta[2]), 0,
            sin(theta[2]),  cos(theta[2]), 0,
            0,              0,             1;
    Eigen::Matrix3d R = R_z * R_y * R_x;
    return R;
}

注意：

  Eigen::Vector3d point3d(p_x,p_y,p_z);
  Eigen::Vector3d theta(fixroll,fixpitch,fixyaw);
  Eigen::Matrix3d R_2 = eulerAnglesToRotationMatrix(theta);
  std::cout<<"R_2_P: "<<std::endl<<R_2*point3d<<std::endl;
  
  //上述矩阵乘以点坐标进行变换，等价于下述变换
  
  float x =p_x * cos(fixyaw) - p_y * sin(fixyaw);
  float y = p_y * cos(fixyaw) + p_x * sin(fixyaw);
  p_x = x;
  p_y = y;
  //再绕y轴
  float temp_x = p_x;
  p_x =  p_x * cos(fixpitch) + p_z * sin(fixpitch);
  p_z = -temp_x * sin(fixpitch) + p_z * cos(fixpitch);
  //最后绕x轴
  float temp_y = p_y;
  float temp_z = p_z;
  p_y = temp_y * cos(fixroll) - temp_z * sin(fixroll);
  p_z = temp_y * sin(fixroll) + temp_z * cos(fixroll);
  cout<<"pxpypz: "<<p_x<<" "<<p_y<<" "<<p_z<<endl;

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

1.2 旋转矩阵

1.2.1 旋转矩阵 -> 欧拉角

1）tf库

  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
  */

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

2）公式法

Eigen::Vector3d rotationMatrixToEulerAngles(Eigen::Matrix3d &R)
{
    assert(isRotationMatirx(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};
}

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.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] https://blog.csdn.net/hero_cjn/article/details/105229484