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

你可能感兴趣的:(slam,人工智能,算法,c++,自动驾驶)