SC-Lego-LOAM解析（中）

上回说到经过连续帧间匹配，激光odo给出来一个位姿估计，
但是是存在不断的误差的积累的，需要与绝对的参考（地图）进行匹配，
以及进行回环检测和全局位姿优化。这也是正是mapOptmization这个node所做的事情。

mapOptmization

还是直接来到main函数：

int main(int argc, char **argv) {
    ros::init(argc, argv, "lego_loam");

    ROS_INFO("\033[1;32m---->\033[0m Map Optimization Started.");

    lego_loam::mapOptimization MO;

    //  两个线程分别进行回环检测和可视化
    std::thread loopthread(&lego_loam::mapOptimization::loopClosureThread, &MO);
    std::thread visualizeMapThread(&lego_loam::mapOptimization::visualizeGlobalMapThread, &MO);

    ros::Rate rate(200);
    while (ros::ok())
        // while ( 1 )
    {
        ros::spinOnce();

        // 循环进行scan-to-map的优化,保存关键帧，计算回环检测所需要的ScanContext
        MO.run();

        rate.sleep();
    }

    loopthread.join();
    visualizeMapThread.join();

    return 0;
}

首先又是构造了一个lego_loam::mapOptimization MO，然后开了两个新线程分别进行ScanContext回环检测和可视化，接下来又是一个死循环执行MO.run()。

先来看lego_loam::mapOptimization的构造函数：

mapOptimization::mapOptimization() : nh("~") {
        //  用于闭环图优化的参数设置，使用gtsam库
        ISAM2Params parameters;
        parameters.relinearizeThreshold = 0.01;
        parameters.relinearizeSkip = 1;
        isam = new ISAM2(parameters);

        InitParams();
        //  subscriber
        // jin: 以下几个回调函数,接收原始点云,面点,角点,离群点,以及里程计位姿保存在成员变量中
        // 这里的原始的点云，是没有经过分割和提取的，雷达直接发出来的
        subLaserCloudRaw = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 2,
                                                                  &mapOptimization::laserCloudRawHandler, this);
        subLaserCloudCornerLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2,
                                                                         &mapOptimization::laserCloudCornerLastHandler,
                                                                         this);
        subLaserCloudSurfLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2,
                                                                       &mapOptimization::laserCloudSurfLastHandler,
                                                                       this);
        subOutlierCloudLast = nh.subscribe<sensor_msgs::PointCloud2>("/outlier_cloud_last", 2,
                                                                     &mapOptimization::laserCloudOutlierLastHandler,
                                                                     this);
        // odo输出的位姿保存到transformSum中
        subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("/laser_odom_to_init", 5,
                                                            &mapOptimization::laserOdometryHandler, this);
        // 保存IMU输出的时间/roll/pitch
        subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 50, &mapOptimization::imuHandler, this);

        //  publisher
        pubKeyPoses = nh.advertise<sensor_msgs::PointCloud2>("/key_pose_origin", 2);
        pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surround", 2);
        pubOdomAftMapped = nh.advertise<nav_msgs::Odometry>("/aft_mapped_to_init", 5);

        pubHistoryKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("/history_cloud", 2);
        pubIcpKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("/corrected_cloud", 2);
        pubRecentKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("/recent_cloud", 2);
        pubRegisteredCloud = nh.advertise<sensor_msgs::PointCloud2>("/registered_cloud", 2);
        
        。。。。。。
 
    }

后端使用gtsam进行后端优化，首先进行参数的初始化，然后，请注意，定义了6个subscriber！！！
订阅的分别是最最原始的激光雷达的数据，featureAssociation提取并发布的角点，面点和离群点，激光里程计位姿以及IMU消息，回调函数基本都只是进行了格式转换和保存，注意对于IMU消息，只保存了俯仰和滚转，后面可以看出在这两个维度上只由IMU确定。

然后来看main函数中死循环执行的函数MO.run()：

void mapOptimization::run() {
        // 有新数据进来，才执行后续
        // jin: 如果成员变量里接收到了新的数据
        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;

            // jin: 互斥锁
            std::lock_guard<std::mutex> lock(mtx);// jin: 和回环检测不同时进行
            // jin: 距离上一次进行scan-to-map优化足够久了
            if (timeLaserOdometry - timeLastProcessing >= mappingProcessInterval) {
                timeLastProcessing = timeLaserOdometry;

                // jin: 应该是根据当前的odo pose,以及上一次进行map_optimation前后的pose(即漂移),计算目前最优的位姿估计
                // 保存到transformTobeMapped
                transformAssociateToMap(); 

                // jin: 确定周围的关键帧的索引,点云保存到recentCorner等，地图拼接保存到laserCloudCornerFromMap等
                extractSurroundingKeyFrames();

                // jin: 对当前帧原始点云,角点,面点,离群点进行降采样
                downsampleCurrentScan();

                // jin: 进行scan-to-map位姿优化,并为下一次做准备
                // 最优位姿保存在和transformAftMapped中，同时transformBfeMapped中保存了优化前的位姿，两者的差距就是激光odo和最优位姿之间偏移量的估计
                scan2MapOptimization();
                // 到这里，虽然在scan-to-scan之后，又进行了scan-to-map的匹配，但是并未出现回环检测和优化，
                // 所以依然是一个误差不断积累的里程计的概念

                // jin: 如果距离上一次保存的关键帧欧式距离最够大，需要保存当前关键帧
                // 计算与上一关键帧之间的约束，这种约束可以理解为局部的小回环，加入后端进行优化，
                // 将优化的结果保存作为关键帧位姿，同步到scan-to-map优化环节
                // 为了检测全局的大回环,还需要生成当前关键帧的ScanContext
                saveKeyFramesAndFactor();

                // jin: 如果另一个线程中isam完成了一次全局位姿优化,那么对关键帧中cloudKeyPoses3D/6D的位姿进行修正
                correctPoses();

                // jin: 发布优化后的位姿,及tf变换
                publishTF();

                // 发布所有关键帧位姿,当前的局部面点地图及当前帧中的面点/角点
                publishKeyPosesAndFrames();

                clearCloud();
            }
        }
    }

如果接收到了所有订阅的话题所发布的消息，且距离上一次进行scan-to-map优化足够久了，那么需要进行新的一次scan-to-map优化：

1. transformAssociateToMap：根据odo与map之间的漂移（由上一次scan-to-map的解雇确定），对该帧的odo位姿进行补偿，保存到transformTobeMapped，这个变量也是后面一直被优化的变量; extractSurroundingKeyFrames：以时间顺序，选择最近的一定数量的关键帧点云，组成局部地图laserCloudCornerFromMap等，并进行一定的降采样;

2. downsampleCurrentScan：对当前帧原始点云,角点,面点,离群点进行降采样;

3. scan2MapOptimization：将步骤2提取出来的局部地图构建kd-tree，方便用来搜索最近邻，基于当前的最优位姿估计transformTobeMapped，对于当前帧的每一个点在 kd-tree中查找最近邻，建立约束，对transformTobeMapped的位姿进行优化，使得总体残差最小，连续循环优化多次，再使用IMU消息回调所确定的roll和pitch对该位姿进行修正。最终，最优位姿保存在和transformAftMapped中，同时transformBfeMapped中保存了优化前的位姿即激光里程计位姿，两者的差距就是激光odo和最优位姿之间偏移量的估计，这也是步骤1所需要的；

4. saveKeyFramesAndFactor：如果距离上一次保存的关键帧欧式距离最够大，需要保存当前关键帧，计算与上一关键帧之间的约束，这种约束可以理解为局部的小回环，加入后端进行优化。将优化的结果保存作为关键帧位姿（保存当前关键帧的3维和6维位姿）和点云，同步到scan-to-map优化环节（即修改transformAftMapped和transformTobeMapped）。为了检测全局的大回环,还需要生成当前关键帧的ScanContext，即scManager.makeAndSaveScancontextAndKeys(*thisRawCloudKeyFrame)，这里暂且不表;

5. correctPoses：如果ScanContext回环检测（即std::thread loopthread）对全局位姿进行了优化，需要同步下来优化后的关键帧所在的位姿; publishTF：发布优化后的位姿和tf变换；

6. publishKeyPosesAndFrames：发布所有关键帧位姿,当前的局部面点地图及当前帧中的面点/角点。

简单总结一下，这个函数对回调函数所保存的各种数据进行了处理，主要是进行scan-to-map匹配，如果需要还增加了新的关键帧进行优化，同时，对该关键帧生成ScanContext信息。另外的一些内容就是，如果后端回环检测后对关键帧的位姿进行了优化，这里需要做同步，以及发布了地图，所有关键帧位姿和当前点云等信息。

那么接下来的重点就是，ScanContext是怎么生成的？以及怎么用的？（肯定是在std::thread loopthread用的）

参考：
SC-Lego-LOAM解析（上）
SC-Lego-LOAM解析（中）
SC-Lego-LOAM解析（下）

本文经允许后转自知乎：https://zhuanlan.zhihu.com/p/348281520