LIO-SAM中的mapOptmization

前言

最近在学习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，以便后面进行优化

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

1. 第 0 帧的时候没有里程计的输入，所以第0帧的位姿初始化是直接初始化为IMU的纯旋转
2. 第 1 帧的时候有里程计的输入，但是没有上一帧的里程计输入，所以第1帧的位姿初始化是通过IMU获得第0帧的第1帧之间的增量变换，然后叠加到第0帧的位姿上
3. 从第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、执行因子图优化

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

实际上它只优化了当前添加到因子图中的变量。例如：

第k次优化：

第k+1次优化

correctPoses()

这个函数在回环触发的时候才会起作用。主要作用就是更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿，更新里程计轨迹。

publishOdometry()

发布激光里程计

publishFrames()

1、发布历史关键帧位姿集合

2、发布局部map的降采样平面点集合

3、发布历史帧（累加的）的角点、平面点降采样集合

4、发布里程计轨迹

gpsHandler()

loopInfoHandler()

主要订阅来自外部闭环检测程序提供的闭环数据

loopClosureThread()

performLoopClosure()

1、以当前帧为中心，选择15米以内的历史帧作为回环检测帧候选帧集合，然后选择距离当前帧时间最远的帧作为回环关键帧

2、提取当前帧特征点构造一个点云集合，提取回环帧前后25帧特征点构造一个点云集合，两个做ICP匹配，获得当前关键帧与闭环关键帧之间的位姿变换

3、构造因子图优化需要的数据，在因子图优化环节会调用这些数据，会优化回环帧到当前帧的所有位姿

visualizeLoopClosure()

主要做闭环检测的可视化，激光雷达运行过程中的那些绿色的点和黄色的线就是从这里发布出去的。

visualizeGlobalMapThread()

1、发布局部关键帧map的特征点云

2、保存全局关键帧特征点集合