LeGo-LOAM源碼篇:源碼分析(3)

src/mapOptmization.cpp

整體功能分為回環檢測、可視化以及位姿全局優化，核心是位姿優化。主體流程：訂閱特征提取後的點雲、裡程計資料->計算目前姿态優化的初始值->提取局部關鍵幀->降采樣->scan-to-map地圖優化（線特征、面特征、L-M）->儲存關鍵幀與因子圖優化->閉環檢測->釋出TF變換、點雲

int main(int argc, char** argv)
{
    ros::init(argc, argv, "lego_loam");
    ROS_INFO("\033[1;32m---->\033[0m Map Optimization Started.");
    mapOptimization MO;//訂閱點雲、裡程計資訊
    
    std::thread loopthread(&mapOptimization::loopClosureThread, &MO);//閉環檢測線程
    std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);//可視化線程

    ros::Rate rate(200);
    while (ros::ok())
    {
        ros::spinOnce();
        MO.run();//全局位姿優化
        rate.sleep();
    }
    
    loopthread.join();
    visualizeMapThread.join();
    
    return 0;
}

...
void run(){
    //三種點雲、裡程計資訊時間戳一緻
    if (newLaserCloudCornerLast  && std::abs(timeLaserCloudCornerLast  - timeLaserOdometry) < 0.005 &&
        newLaserCloudSurfLast    && std::abs(timeLaserCloudSurfLast    - timeLaserOdometry) < 0.005 &&
        newLaserCloudOutlierLast && std::abs(timeLaserCloudOutlierLast - timeLaserOdometry) < 0.005 &&
        newLaserOdometry)
    {

        newLaserCloudCornerLast = false; newLaserCloudSurfLast = false; newLaserCloudOutlierLast = false; newLaserOdometry = false;

        std::lock_guard<std::mutex> lock(mtx);
        //設定優化的間隔為0.3秒
        if (timeLaserOdometry - timeLastProcessing >= mappingProcessInterval) {

            timeLastProcessing = timeLaserOdometry;
            //将點雲轉換到世界坐标系
            //根據目前（transformSum）與上次全局優化時的裡程計（transformBefMapped）、上次全局優化的結果（transformAftMapped），計算目前姿态優化（transformTobeMapped）的初始值
            transformAssociateToMap();                
            //根據目前位置，提取局部關鍵幀集合以及對應的點雲集合
            extractSurroundingKeyFrames();
            //降采樣
            downsampleCurrentScan();
            //scan-to-map配準優化，獲得transformTobeMapped，參考IMU姿态獲得最終位姿 transformAftMapped
            scan2MapOptimization();
            //儲存關鍵幀與因子圖優化
            saveKeyFramesAndFactor();
            //閉環檢測
            correctPoses();
            //發送tf變換
            publishTF();
            //釋出關鍵幀點雲
            publishKeyPosesAndFrames();

            clearCloud();
        }
    }
}
...
           

scan2MapOptimization()

邊特征、面特征優化

void scan2MapOptimization(){

    if (laserCloudCornerFromMapDSNum > 10 && laserCloudSurfFromMapDSNum > 100) {
        //來源于recent（閉環）或surrounding（無閉環）
        kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
        kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);

        for (int iterCount = 0; iterCount < 10; iterCount++) {

            laserCloudOri->clear();
            coeffSel->clear();

            cornerOptimization(iterCount);//邊特征優化
            surfOptimization(iterCount);//面特征優化
            //LM優化
            if (LMOptimization(iterCount) == true)
                break;              
        }

        transformUpdate();
    }
}
           

saveKeyFrameAndFactor()

儲存關鍵幀和因子圖優化

void saveKeyFramesAndFactor(){

    currentRobotPosPoint.x = transformAftMapped[3];
    currentRobotPosPoint.y = transformAftMapped[4];
    currentRobotPosPoint.z = transformAftMapped[5];

    bool saveThisKeyFrame = true;
    if (sqrt((previousRobotPosPoint.x-currentRobotPosPoint.x)*(previousRobotPosPoint.x-currentRobotPosPoint.x)
             +(previousRobotPosPoint.y-currentRobotPosPoint.y)*(previousRobotPosPoint.y-currentRobotPosPoint.y)
             +(previousRobotPosPoint.z-currentRobotPosPoint.z)*(previousRobotPosPoint.z-currentRobotPosPoint.z)) < 0.3){
        saveThisKeyFrame = false;
    }

    if (saveThisKeyFrame == false && !cloudKeyPoses3D->points.empty())
        return;

    previousRobotPosPoint = currentRobotPosPoint;
    /**
         * update grsam graph
         */
    if (cloudKeyPoses3D->points.empty()){
        //剛剛初始化，增加PriorFactor因子
        gtSAMgraph.add(PriorFactor<Pose3>(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], transformTobeMapped[1]),
                                                   Point3(transformTobeMapped[5], transformTobeMapped[3], transformTobeMapped[4])), priorNoise));
        initialEstimate.insert(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], transformTobeMapped[1]),
                                        Point3(transformTobeMapped[5], transformTobeMapped[3], transformTobeMapped[4])));
        for (int i = 0; i < 6; ++i)
            transformLast[i] = transformTobeMapped[i];
    }
    else{
        //非剛剛初始化，從transformLast獲得上一次位姿
        gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(transformLast[2], transformLast[0], transformLast[1]),
                                      Point3(transformLast[5], transformLast[3], transformLast[4]));
        gtsam::Pose3 poseTo   = Pose3(Rot3::RzRyRx(transformAftMapped[2], transformAftMapped[0], transformAftMapped[1]),
                                      Point3(transformAftMapped[5], transformAftMapped[3], transformAftMapped[4]));
        gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->points.size()-1, cloudKeyPoses3D->points.size(), poseFrom.between(poseTo), odometryNoise));
        initialEstimate.insert(cloudKeyPoses3D->points.size(), Pose3(Rot3::RzRyRx(transformAftMapped[2], transformAftMapped[0], transformAftMapped[1]),
                                                                     Point3(transformAftMapped[5], transformAftMapped[3], transformAftMapped[4])));
    }
    /**
         * update iSAM
         */
    isam->update(gtSAMgraph, initialEstimate);//第一個參數是新加到系統中的因子，第二個參數是新變量的初始點
    isam->update();

    gtSAMgraph.resize(0);
    initialEstimate.clear();

    /**
         * save key poses
         */
    PointType thisPose3D;
    PointTypePose thisPose6D;
    Pose3 latestEstimate;

    isamCurrentEstimate = isam->calculateEstimate();
    latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size()-1);

    thisPose3D.x = latestEstimate.translation().y();
    thisPose3D.y = latestEstimate.translation().z();
    thisPose3D.z = latestEstimate.translation().x();
    thisPose3D.intensity = cloudKeyPoses3D->points.size(); // this can be used as index
    cloudKeyPoses3D->push_back(thisPose3D);

    thisPose6D.x = thisPose3D.x;
    thisPose6D.y = thisPose3D.y;
    thisPose6D.z = thisPose3D.z;
    thisPose6D.intensity = thisPose3D.intensity; // this can be used as index
    thisPose6D.roll  = latestEstimate.rotation().pitch();
    thisPose6D.pitch = latestEstimate.rotation().yaw();
    thisPose6D.yaw   = latestEstimate.rotation().roll(); // in camera frame
    thisPose6D.time = timeLaserOdometry;
    cloudKeyPoses6D->push_back(thisPose6D);
    /**
         * save updated transform
         */
    if (cloudKeyPoses3D->points.size() > 1){
        transformAftMapped[0] = latestEstimate.rotation().pitch();
        transformAftMapped[1] = latestEstimate.rotation().yaw();
        transformAftMapped[2] = latestEstimate.rotation().roll();
        transformAftMapped[3] = latestEstimate.translation().y();
        transformAftMapped[4] = latestEstimate.translation().z();
        transformAftMapped[5] = latestEstimate.translation().x();

        for (int i = 0; i < 6; ++i){
            transformLast[i] = transformAftMapped[i];
            transformTobeMapped[i] = transformAftMapped[i];
        }
    }

    pcl::PointCloud<PointType>::Ptr thisCornerKeyFrame(new pcl::PointCloud<PointType>());
    pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame(new pcl::PointCloud<PointType>());
    pcl::PointCloud<PointType>::Ptr thisOutlierKeyFrame(new pcl::PointCloud<PointType>());

    pcl::copyPointCloud(*laserCloudCornerLastDS,  *thisCornerKeyFrame);
    pcl::copyPointCloud(*laserCloudSurfLastDS,    *thisSurfKeyFrame);
    pcl::copyPointCloud(*laserCloudOutlierLastDS, *thisOutlierKeyFrame);

    cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
    surfCloudKeyFrames.push_back(thisSurfKeyFrame);
    outlierCloudKeyFrames.push_back(thisOutlierKeyFrame);
}
           

參考資料：

[1]. LeGo-LOAM源碼閱讀筆記(四)—mapOptmization.cpp

[2]. LeGO-LOAM LoopClosure