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;