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