文章目錄

• 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 w a P a P_w=T_{wa}P_a
  Pw​=Twa​Pa​
  P w = T w b P b P_w=T_{wb}P_b Pw​=Twb​Pb​
  P a = T a b P b P_a=T_{ab}P_b Pa​=Tab​Pb​
  T a b = P a P b − 1 = T w a − 1 P w T w b − 1 P w − 1 = T w a − 1 P w P w − 1 ∗ T w b = T w a − 1 T w 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}
  Tab​=Pa​Pb−1​=Twa−1​Pw​Twb−1​Pw​−1=Twa−1​Pw​Pw−1​∗Twb​=Twa−1​Twb​
• 認為後端優化軌跡和里程計軌跡相對位姿在短時間內相同，估計絕對位姿
  T a − 1 T b = P a − 1 P b T_a^{-1}T_b=P_a^{-1}P_b Ta−1​Tb​=Pa−1​Pb​
  P b = P a T a − 1 T b P_b=P_aT_a^{-1}T_b Pb​=Pa​Ta−1​Tb​
• 程式碼簡化
     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