多传感器融合SLAM --- 13.LIO-SAM地图优化代码分析 mapOptmization.cpp（地图优化部分）

目录

0 流程图

1 地图优化

1.1 构造函数分析 mapOptimization()

1.2  laserCloudInfoHandler 回调函数解析

1.3  updateInitialGuess函数解析

1.4  提取当前帧的相关关键帧并且构建点云局部地图extractSurroundingKeyFrames

1.5  对当前帧进行下采样downsampleCurrentScan

1.6 scan2MapOptimization 求得一个Rt使得当前帧能最好的匹配到局部地图上面去

1.7 判断是否插入关键帧 saveKeyFramesAndFactor

1.8 调整全局轨迹 correctPoses

1.9  发布lidar里程计信息 publishOdometry

1.10 发布点云关键帧信息

---

0 流程图

1 地图优化

1.1 构造函数分析 mapOptimization()

    mapOptimization()
    {
        ISAM2Params parameters;  // 创建ISAM2参数对象
        parameters.relinearizeThreshold = 0.1;  // 设置ISAM2的重线性化阈值
        parameters.relinearizeSkip = 1;  // 设置ISAM2的重线性化间隔
        isam = new ISAM2(parameters);  // 创建ISAM2对象

        // 订阅信息
        pubKeyPoses = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/trajectory", 1);  // 发布关键位姿信息
        pubLaserCloudSurround = nh.advertise<sensor_msgs::P