Eigen::Isometry3d变换矩阵的常用方法
文章目录
- Eigen::Isometry3d变换矩阵的常用方法
- 一、基本定义
- 二、操作方法
- 三、与四元数和平移向量的转换
- 四、左乘与右乘
Eigen::Isometry3d变换矩阵的常用方法
一、基本定义
变换矩阵本质是一个4*4的矩阵
Eigen::Isometry3d T_imu_to_lidar = Eigen::Isometry3d::Identity()二、操作方法
-
.translation():无参数，返回当前变换平移部分的向量表示(可修改)，可以索引[]获取各分量
-
.rotation():无参数，返回(只读的)当前变换的旋转部分，以旋转矩阵表示;
-
.matrix():返回变换对应的矩阵(可修改),包括平移部分和旋转部分;
-
.linear():返回变换的线性部分，对于Isometry而言就是旋转对应的旋转矩阵，Eigen::Block类型;
-
.Identity()函数的作用：在定义变量时使用Eigen::Matrix4f x =
Eigen::Matrix4f::Identity();即用单位矩阵对x变量进行了初始化 -
.inverse():求变换矩阵的逆
三、与四元数和平移向量的转换
// Eigen::Isometry3d --> Eigen::Quaterniond
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();
//rotation()的结果是3*3的Matrix3d矩阵. rotation：旋转
Eigen::Quaterniond enu_attitude =Eigen::Quaterniond(T_lidar_to_imu.rotation());// Eigen::Quaterniond --> Eigen::Isometry3d
Eigen::Quaterniond q_delta;
Eigen::Vector3d p_delta;
Eigen::Isometry3d T_delta;
T_delta.linear() = q_delta.toRotationMatrix();
T_delta.translation() = p_delta;四、左乘与右乘
旋转矩阵左乘是相对固定坐标系，右乘是相对当前坐标系。
1、求取变换矩阵的相对关系
//求取雷达坐标系到enu世界坐标系的转换矩阵，以enu坐标系为基准
Eigen::Isometry3d T_lidar_to_enu = T_imu_to_enu * system_config.T_lidar_to_imu;
2、求取点的坐标从坐标系A转到坐标系B
Eigen::Vector3d position_ecef;// 得到当前时刻雷达在地心地固坐标系的位置position_ecef = system_config.T_enu_to_ecef * position_enu;