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

原创

已于 2022-09-24 15:25:22 修改 · 1.2k 阅读

11 ·

CC 4.0 BY-SA版权

文章标签：

#slam

于 2020-11-05 21:32:39 首次发布

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()