以transformAssociateToMap函数为例，分析LeGO-LOAM的坐标系统

最新推荐文章于 2024-02-18 20:20:29 发布

翡翠之幻

最新推荐文章于 2024-02-18 20:20:29 发布

阅读量2.6k

点赞数 3

本文链接：https://blog.csdn.net/qq_40268190/article/details/112425180

版权

文章目录

LeGO-LOAM采用的坐标轴体系
transformAssociateToMap函数剖析
- 公式推导
LeGO-LOAM坐标变换解析

LeGO-LOAM采用的坐标轴体系

LeGO-LOAM的旋转顺序是固定轴ZXY而LeGO-LOAM坐标的选取在第一帧下的几何意义为Z朝前，Y朝上，那根据右手定则，X朝左。在RPY体系下就是滚转（Roll）——俯仰（Pitch）——偏航（Yaw）。在ROS和Eigen采用的坐标定义下就是sXYZ顺序。

transformAssociateToMap函数剖析

公式推导

相对位姿和绝对位姿关系的推导
$P_w=T_{wa}P_a$
$P_w=T_{wb}P_b$
$P_a=T_{ab}P_b$
$T_{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}$
认为后端优化轨迹和里程计轨迹相对位姿在短时间内相同，估计绝对位姿
$T_a^{-1}T_b=P_a^{-1}P_b$
$P_b=P_aT_a^{-1}T_b$
代码简化
transformAssociateToMap使用Eigen简化之后的代码，可以看到就是上面推导出来的公式：

    vector<double> 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<double> &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<double> &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<double> 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<<endl;

    Eigen::AngleAxisd XAngle(Eigen::AngleAxisd(transformMapped[0],Eigen::Vector3d::UnitX()));
    Eigen::AngleAxisd YAngle(Eigen::AngleAxisd(transformMapped[1],Eigen::Vector3d::UnitY()));
    Eigen::AngleAxisd ZAngle(Eigen::AngleAxisd(transformMapped[2],Eigen::Vector3d::UnitZ()));

    Eigen::Quaterniond Tran=YAngle*XAngle*ZAngle;

    cout<< Tran.x()<<" "<< Tran.y()<<" "<< Tran.z()<<" "<< Tran.w()<<endl;

输出为：

0.680418 -0.0885445 0.122661 0.717039
0.680418 -0.0885445 0.122661 0.717039