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

Jerry_Sea

已于 2022-08-22 11:29:29 修改

阅读量1.7k

点赞数 2

分类专栏： SLAM 文章标签：矩阵 unity 线性代数

于 2022-08-15 20:02:01 首次发布

本文链接：https://blog.csdn.net/qq_46515446/article/details/126350026

版权

本文详细介绍了SLAM（Simultaneous Localization And Mapping）中旋转量间的转换，包括欧拉角到旋转矩阵的转换，以及旋转矩阵到欧拉角的转换。同时，讨论了如何利用欧拉角和平移向量构建变换矩阵，以及旋转矩阵和平移向量组合成变换矩阵的方法。主要使用了Eigen库和tf库，并提到了转换过程中的注意事项和可能遇到的问题。

摘要由CSDN通过智能技术生成

文章目录

0 前言

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

#include<iostream>
#include<Eigen/Core>              //核心矩阵运算库（Vector3d，Matrix3d）
#include<Eigen/Dense>          // 稠密矩阵的代数运算（逆和特征值）
#include<Eigen/Geometry>  // 引入旋转平移（旋转矩阵、旋转向量、欧拉角、四元数、平移向量）
#include<opencv2/opencv.hpp>

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