LIO-SAM中的mapOptmization

最新推荐文章于 2025-05-12 17:49:48 发布

原创最新推荐文章于 2025-05-12 17:49:48 发布 · 1.6k 阅读

8 ·

CC 4.0 BY-SA版权

文章标签：

#算法 #SLAM #LIO-SAM #因子图优化

SLAM 专栏收录该内容

15 篇文章

订阅专栏

本文深入剖析LIO-SAM源码，重点解读mapOptmization.cpp文件，涵盖残差构建原理、scan-to-map匹配及因子图优化等内容。通过特征匹配提高定位准确性。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

前言

最近在学习LIO-SAM源码的时候，发现LIO-SAM这套代码调用了比较多库的内置API，里面涉及的一些细节也比较多，整个工程还是比较清晰的，值得学习！

LIO-SAM这个框架主要由四个大的模块组成，其中这个图优化模块的内容最多，里面也涉及最多细节，这里主要记录下mapOptmization.cpp这个文件的解读，包括：残差构建原理、scan-to-map、因子图优化等。scan-to-map的原理部分参考我的另外一篇博客：LIO-SAM中的scan_to_map原理剖析

学习这份代码的时候参考了很多LIO-SAM-DetailedNote这位大哥的注释，我在他的基础上修正了一些我个人感觉不太对的地方，增加了一些细节的注释。对于整个工程的详细注释放在了我的个人Github：LIO-SAM-note，欢迎大家批评指正，一起交流学习！

模块功能简述:

1、scan-to-map匹配：提取当前激光帧特征点（角点、平面点），局部关键帧map的特征点，执行scan-to-map迭代优化，更新当前帧位姿；

2、关键帧因子图优化：关键帧加入因子图，添加激光里程计因子、GPS因子、闭环因子，执行因子图优化，更新所有关键帧位姿；

3、闭环检测：在历史关键帧中找距离相近，时间相隔较远的帧设为匹配帧，匹配帧周围提取局部关键帧map，同样执行scan-to-map匹配，得到位姿变换，构建闭环因子数据，加入因子图优化。

mapOptmization 类中通过构造函数对订阅的话题消息用回调函数进行处理，主要的工作在回调函数 laserCloudInfoHandler() 中实现，另外开了一个线程做回环检测因子的构建loopClsureThread()，以及另外开了一个可视化线程visualizeGlobalMapThread()。

laserCloudInfoHandler()

updateInitialGuess()

这个函数主要通过IMU里程计来的信息对当前帧位姿进行初始化，每一帧都获得一个粗糙的 T_wr，以便后面进行优化

这里有一个细节是，对不同的帧分了三种处理方式

第 0 帧的时候没有里程计的输入，所以第0帧的位姿初始化是直接初始化为IMU的纯旋转
第 1 帧的时候有里程计的输入，但是没有上一帧的里程计输入，所以第1帧的位姿初始化是通过IMU获得第0帧的第1帧之间的增量变换，然后叠加到第0帧的位姿上
从第2帧开始，有了上一帧和当前帧的里程计结果，此后都是通过里程计获得当前帧和前一帧的增量变换，然后把增量变换叠加到上一帧的位姿作为当前帧的初始化位姿，以便后续进行优化

void updateInitialGuess()
    {
        // 前一帧的lidar位姿
        incrementalOdometryAffineFront = trans2Affine3f(transformTobeMapped);
        static Eigen::Affine3f lastImuTransformation;
        
        // 1. 第0帧的处理方法，只有IMU旋转变换，没有里程计输入，所以第0帧位姿初始化为IMU的纯旋转
        if (cloudKeyPoses3D->points.empty())
        {
            // 当前帧位姿(map坐标系下)，用激光帧信息中的RPY（来自imu原始数据）初始化
            transformTobeMapped[0] = cloudInfo.imuRollInit;
            transformTobeMapped[1] = cloudInfo.imuPitchInit;
            transformTobeMapped[2] = cloudInfo.imuYawInit;

            if (!useImuHeadingInitialization)
                transformTobeMapped[2] = 0;
            lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); 
            return;
        }

        static bool lastImuPreTransAvailable = false;
        static Eigen::Affine3f lastImuPreTransformation;
        // 2. 第1帧开始有里程计输入了，用当前帧和前一帧对应的imu里程计计算相对位姿变换，再用前一帧的位姿与相对变换，计算当前帧的位姿，存transformTobeMapped
        if (cloudInfo.odomAvailable == true)
        {
            // 当前帧的初始估计位姿（来自imu里程计），后面用来计算增量位姿变换
            Eigen::Affine3f transBack = pcl::getTransformation(cloudInfo.initialGuessX,    cloudInfo.initialGuessY,     cloudInfo.initialGuessZ, 
                                                              cloudInfo.initialGuessRoll, cloudInfo.initialGuessPitch, cloudInfo.initialGuessYaw);
            // 第1帧的处理方法
            if (lastImuPreTransAvailable == false)  
            {
                lastImuPreTransformation = transBack;
                lastImuPreTransAvailable = true;
            }
            // 第2帧包括之后的帧的处理方法 
            else {
                // 当前帧相对于前一帧的位姿变换，imu里程计计算得到
                Eigen::Affine3f transIncre = lastImuPreTransformation.inverse() * transBack;
                // 前一帧的位姿
                Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
                // 当前帧的位姿
                Eigen::Affine3f transFinal = transTobe * transIncre;
                // 更新当前帧位姿transformTobeMapped
                pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], 
                                                              transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
                // 递归
                lastImuPreTransformation = transBack;
                lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
                return;
            }
        }

        // 只在处理第1帧调用，因为此时还没有上一帧的里程计输入，所以第0帧和第1帧的位姿变换通过IMU获得，然后把增量位姿叠加到上一帧的位姿获得当前帧到map坐标系的变换
        if (cloudInfo.imuAvailable == true)
        {
            // 当前帧的姿态角（来自原始imu数据）
            Eigen::Affine3f transBack = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit);
            // 当前帧相对于前一帧的姿态变换
            Eigen::Affine3f transIncre = lastImuTransformation.inverse() * transBack;  // T_r(i)_r(i+1) = T_wr(i).inv() * T_wr(i+1)

            // 前0帧的位姿(map坐标系下) Twri
            Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
            // 当前帧的位姿 T_wr(i+1) = T_wr(i) * T_r(i)r(i+1)
            Eigen::Affine3f transFinal = transTobe * transIncre;
            // 更新当前帧位姿transformTobeMapped
            pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], 
                                                          transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
            lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
            return;
        }
    }

extractSurroundingKeyFrames()

这个函数里面主要是调用了 extractNearby() 把相邻（注意：是空间上的）帧的特征点提取出来构造一个局部点云地图，以便后面进行 scan-to-map 。这里主要得到两个数据结构，它把角点和平面点分开保存了：

downSizeFilterCorner

downSizeFilterSurf

    void extractNearby()
    {
        pcl::PointCloud<PointType>::Ptr surroundingKeyPoses(new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType>::Ptr surroundingKeyPosesDS(new pcl::PointCloud<PointType>());
        std::vector<int> pointSearchInd;
        std::vector<float> pointSearchSqDis;

        // kdtree的输入，全局关键帧位姿集合（历史所有关键帧集合）
        kdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D); 
        // 对最近的一帧关键帧，在半径区域内搜索空间区域上相邻的关键帧集合
        kdtreeSurroundingKeyPoses->radiusSearch(cloudKeyPoses3D->back(), (double)surroundingKeyframeSearchRadius, pointSearchInd, pointSearchSqDis);
        // 遍历搜索结果，pointSearchInd存的是结果在cloudKeyPoses3D下面的索引
        for (int i = 0; i < (int)pointSearchInd.size(); ++i)
        {
            int id = pointSearchInd[i];
            // 加入相邻关键帧位姿集合中
            surroundingKeyPoses->push_back(cloudKeyPoses3D->points[id]);
        }
        // 降采样一下
        downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);
        downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);
        // 加入时间上相邻的一些关键帧，比如当载体在原地转圈，这些帧加进来是合理的
        int numPoses = cloudKeyPoses3D->size();
        for (int i = numPoses-1; i >= 0; --i)
        {
            if (timeLaserInfoCur - cloudKeyPoses6D->points[i].time < 10.0)
                surroundingKeyPosesDS->push_back(cloudKeyPoses3D->points[i]);
            else
                break;
        }
        // 将相邻关键帧集合对应的角点、平面点，加入到局部map中，作为scan-to-map匹配的局部点云地图
        extractCloud(surroundingKeyPosesDS);
    }

downsampleCurrentScan()

对当前激光帧进行降采样，这里同样划分为边角点集合以及平面点集合

void downsampleCurrentScan(){
    // 当前激光帧角点集合降采样
    laserCloudCornerLastDS->clear();
    downSizeFilterCorner.setInputCloud(laserCloudCornerLast);
    downSizeFilterCorner.filter(*laserCloudCornerLastDS);
    laserCloudCornerLastDSNum = laserCloudCornerLastDS->size();
    // 当前激光帧平面点集合降采样
    laserCloudSurfLastDS->clear();
    downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
    downSizeFilterSurf.filter(*laserCloudSurfLastDS);
    laserCloudSurfLastDSNum = laserCloudSurfLastDS->size();
}

scan2MapOptimization();

这个函数的作用是把两拨点云匹配，求出相对位姿变换，然后作用到当前位姿上，实现位姿矫正

为什么要做scan-to-map，ICP不好吗?

答：scan-to-map是特征匹配，ICP是点与点之间的匹配；ICP匹配对初始化位姿和点云质量要求比较高，但是特征匹配利用的是点与线、点与面之间的匹配，可以提高鲁棒性

这个函数主要是用scan-to-map的方式优化当前帧的位姿, 流程如下:

1、要求当前帧特征点数量足够多，且匹配的点数够多，才执行优化

2、以下内容迭代30次（上限）优化

1) 当前激光帧角点寻找局部map匹配点

这里使用OpenMP的一个多线程加速的技巧，因为根据当前帧的点去找近邻点是完全可以并行化处理的，他们相互之间没有干涉。

a.更新当前帧位姿，将当前帧角点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成直线（注意：并不是直接用这五个点拟合一个曲线，而是用距离中心点的协方差矩阵，特征值进行判断），则认为匹配上了

b.计算当前帧角点到直线的距离、垂线的单位向量，存储为角点参数

2) 当前激光帧平面点寻找局部map匹配点

a.更新当前帧位姿，将当前帧平面点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成平面（最小二乘拟合平面），则认为匹配上了

b.计算当前帧平面点到平面的距离、垂线的单位向量，存储为平面点参数

3) 提取当前帧中与局部map匹配上了的角点、平面点，加入同一集合。存入到laserCloudOri coeffSel

4) 对匹配特征点计算Jacobian矩阵，观测值为特征点到直线、平面的距离，构建高斯牛顿方程，迭代优化当前位姿

因为这个模块涉及的原理比较多，参考我的另外一篇博客：

https://blog.csdn.net/weixin_40599145/article/details/127398733

LIO-SAM在优化结束之后和IMU进行了加权融合

3、用imu原始RPY数据与scan-to-map优化后的位姿进行加权融合，修正当前帧位姿的roll、pitch，此外还对roll、pitch以及z坐标做了一个幅值约束，防止优化出错。

saveKeyFramesAndFactor()

这个函数主要是做因子图优化方面的工作。

1、计算当前帧与前一帧位姿变换，判断旋转和平移增量的大小，如果变化太小，不设为关键帧，反之设为关键帧

2、给因子图添加激光里程计因子

void addOdomFactor()
    {
        if (cloudKeyPoses3D->points.empty())
        {
            // 第一帧初始化先验因子。这里他把yaw和平移分量的方差设置得很大，因为我们的系统是四自由度不可观的。
            noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-2, 1e-2, M_PI*M_PI, 1e8, 1e8, 1e8).finished()); // rad*rad, meter*meter
            gtSAMgraph.add(PriorFactor<Pose3>(0, trans2gtsamPose(transformTobeMapped), priorNoise));
            // 变量节点设置初始值
            initialEstimate.insert(0, trans2gtsamPose(transformTobeMapped));
        }else{
            // 添加激光里程计因子
            noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
            // cloudKeyPoses6D这个数据结构是存储经因子图优化之后的位姿的，所以在此时它最后一个还是上一帧的位姿
            gtsam::Pose3 poseFrom = pclPointTogtsamPose3(cloudKeyPoses6D->points.back());  
            gtsam::Pose3 poseTo   = trans2gtsamPose(transformTobeMapped);
            // 参数：前一帧id，当前帧id，前一帧与当前帧的位姿变换（作为观测值），噪声协方差
            gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->size()-1, cloudKeyPoses3D->size(), poseFrom.between(poseTo), odometryNoise));
            // 变量节点设置初始值
            initialEstimate.insert(cloudKeyPoses3D->size(), poseTo);
        }
    }

3、添加GPS约束因子、添加回环检测约束因子

4、执行因子图优化

这里值得注意的是，它不会对历史所有位姿进行优化，他每次优化完成之后会把因子图清空