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));