文章目录
- LeGO-LOAM采用的坐标轴体系
- transformAssociateToMap函数剖析
- 公式推导
- LeGO-LOAM坐标变换解析
LeGO-LOAM采用的坐标轴体系
LeGO-LOAM的旋转顺序是固定轴ZXY而LeGO-LOAM坐标的选取在第一帧下的几何意义为Z朝前,Y朝上,那根据右手定则,X朝左。在RPY体系下就是滚转(Roll)——俯仰(Pitch)——偏航(Yaw)。在ROS和Eigen采用的坐标定义下就是sXYZ顺序。
transformAssociateToMap函数剖析
公式推导
- 相对位姿和绝对位姿关系的推导
Pw=TwaPaP_w=T_{wa}P_aPw=TwaPa
Pw=TwbPbP_w=T_{wb}P_bPw=TwbPb
Pa=TabPbP_a=T_{ab}P_bPa=TabPb
Tab=PaPb−1=Twa−1PwTwb−1Pw−1=Twa−1PwPw−1∗Twb=Twa−1TwbT_{ab}=P_aP_b^{-1}=T_{wa}^{-1}P_w{T_{wb}^{-1}P_w}^{-1}=T_{wa}^{-1}P_wP_w^{-1}*T_{wb}=T_{wa}^{-1}T_{wb}Tab=PaPb−1=Twa−1PwTwb−1Pw−1=Twa−1PwPw−1∗Twb=Twa−1Twb - 认为后端优化轨迹和里程计轨迹相对位姿在短时间内相同,估计绝对位姿
Ta−1Tb=Pa−1PbT_a^{-1}T_b=P_a^{-1}P_bTa−1Tb=Pa−1Pb
Pb=PaTa−1TbP_b=P_aT_a^{-1}T_bPb=PaTa−1Tb - 代码简化
transformAssociateToMap使用Eigen简化之后的代码,可以看到就是上面推导出来的公式:
vector Trans_eigen(6,0);Eigen::Matrix4d Tran_sum,Tran_BefMapped,Tran_AftMapped,Tran_final;RpyxyzToMatrix4d_LeGO(transformSum,Tran_sum);RpyxyzToMatrix4d_LeGO(transformBefMapped,Tran_BefMapped);RpyxyzToMatrix4d_LeGO(transformAftMapped,Tran_AftMapped);Tran_final=Tran_AftMapped*Tran_BefMapped.inverse()*Tran_sum;Matrix4dToRpyxyz_LeGO(Tran_final,Trans_eigen);
所用工具代码,即按照LeGO-LOAM坐标系来转换欧拉角和旋转矩阵,如下:
void RpyxyzToMatrix4d_LeGO(const vector &transformSum,Eigen::Matrix4d &Tran){Eigen::AngleAxisd XAngle(Eigen::AngleAxisd(transformSum[0],Eigen::Vector3d::UnitX()));Eigen::AngleAxisd YAngle(Eigen::AngleAxisd(transformSum[1],Eigen::Vector3d::UnitY()));Eigen::AngleAxisd ZAngle(Eigen::AngleAxisd(transformSum[2],Eigen::Vector3d::UnitZ()));Eigen::Matrix3d Rotate(YAngle*XAngle*ZAngle);Eigen::Vector3d tran(transformSum[3],transformSum[4],transformSum[5]);Tran=Eigen::Matrix4d::Identity();Tran.block<3,3>(0,0)=Rotate;Tran.block<3,1>(0,3)=tran;
}void Matrix4dToRpyxyz_LeGO(const Eigen::Matrix4d &Tran,vector &transformSum){Eigen::Vector3d rpy_transformSum=Tran.block<3,3>(0,0).eulerAngles(1,0,2);transformSum[0]=rpy_transformSum(1);transformSum[1]=rpy_transformSum(0);transformSum[2]=rpy_transformSum(2);transformSum[3]=Tran(0,3);transformSum[4]=Tran(1,3);transformSum[5]=Tran(2,3);}
LeGO-LOAM坐标变换解析
LeGO-LOAM中有一段非常常见的,只改变旋转角不改变三维位姿的坐标变换。
geoQuat = tf::createQuaternionMsgFromRollPitchYaw(transformMapped[2], -transformMapped[0], -transformMapped[1]);laserOdometry2.header.stamp = laserOdometry->header.stamp;laserOdometry2.pose.pose.orientation.x = -geoQuat.y;laserOdometry2.pose.pose.orientation.y = -geoQuat.z;laserOdometry2.pose.pose.orientation.z = geoQuat.x;laserOdometry2.pose.pose.orientation.w = geoQuat.w;laserOdometry2.pose.pose.position.x = transformMapped[3];laserOdometry2.pose.pose.position.y = transformMapped[4];laserOdometry2.pose.pose.position.z = transformMapped[5];
这段代码的作用就是按照sZXY的顺序将欧拉角转化为四元数,验证代码如下:
vector transformMapped{1.5,0.6,0.9};geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(transformMapped[2], -transformMapped[0], -transformMapped[1]);cout<< -geoQuat.y<<" "<< -geoQuat.z<<" "<< geoQuat.x<<" "<< geoQuat.w<
输出为:
0.680418 -0.0885445 0.122661 0.717039
0.680418 -0.0885445 0.122661 0.717039










