Eigen中变换矩阵Eigen::Isometry3d T的使用方法（左乘和右乘）

https://zhuanlan.zhihu.com/p/610439768?utm_id=0

 

一、基本定义

Eigen::Isometry3d T_imu_to_lidar = Eigen::Isometry3d::Identity()

转换矩阵本质是一个4*4的矩阵

二、操作方法

.translation():无参数，返回当前变换平移部分的向量表示(可修改)，可以索引[]获取各分量
.rotation():无参数，返回(只读的)当前变换的旋转部分，以旋转矩阵表示;
.matrix():返回变换对应的矩阵(可修改),包括平移部分和旋转部分;
.linear():返回变换的线性部分，对于Isometry而言就是旋转对应的旋转矩阵，Eigen::Block类型;
.Identity()函数的作用：在定义变量时使用
Eigen::Matrix4f x = Eigen::Matrix4f::Identity();即用单位矩阵对x变量进行了初始化
.inverse():求变换矩阵的逆

三、与四元素和平移向量的转换

Eigen::Matrix4d M_lidar_to_imu;
Eigen::Isometry3d T_lidar_to_imu(M_lidar_to_imu); 
Eigen::Vector3d enu_position = T_lidar_to_imu.translation();
Eigen::Quaterniond enu_attitude =Eigen::Quaterniond(T_lidar_to_imu.rotation());//rotion()的结果是3*3的Matrix3d矩阵
Eigen::Quaterniond q_delta; 
Eigen::Vector3d p_delta;  
Eigen::Isometry3d T_delta;
T_delta.linear() = q_delta.toRotationMatrix();
T_delta.translation() = p_delta;

四、左乘与右乘

旋转矩阵左乘是相对固定（世界）坐标系，右乘是相对当前坐标系

1、求取变换矩阵的相对关系
Eigen::Isometry3d T_lidar_to_enu = T_imu_to_enu * system_config.T_lidar_to_imu;//求取雷达坐标系到enu世界坐标系的转换矩阵，以enu坐标系为基准   （左乘）
2、求取点的坐标从坐标系A转到坐标系B
 Eigen::Vector3d position_ecef;
 position_ecef = system_config.T_enu_to_ecef * position_enu;  // 得到当前时刻雷达在地心地固坐标系的位置       

基于全局坐标系的旋转变换左乘旋转矩阵，基于自身坐标系的旋转变换右乘旋转矩阵。

 

pretranslate、prerotate表示左乘，即以世界坐标为参考
translate、rotate表示右乘，即以局部坐标系为参考
下面以直线L做45度旋转和向X方向移动200像素两次线性变换为例子：

 

isometry1.rotate(axisd);//右乘（局部坐标参考）
isometry1.translate(Eigen::Vector3d(200,0,0));

isometry2.prerotate(axisd);//左乘（世界坐标参考）
isometry2.pretranslate(Eigen::Vector3d(200,0,0));