前言

发表一些自己对LIO-SAM的理解，仅作笔记。LIO-SAM说白了就五个文件：一个头文件：utility.h，四个cpp文件：imageProjection.cpp、featureExtraction.cpp、imuPreintergration.cpp、mapOptimization.cpp，这里我们着重讲讲最长也是最复杂的mapOptimization.cpp

mapOptimization.cpp

首先我们会看到它会发布一大堆乱七八糟的东西，但是我们看懂这个cpp文件的话是不需要看它的发布的东西，我们只需要看它订阅了什么，然后里面是什么逻辑的，很幸运，我们这个cpp文件只订阅了三个：当前激光帧的点云信息、GPS里程计以及来自外部闭环检测程序所提供的闭环数据；更加幸运的是，在LIO-SAM里面我们最后一个订阅的信息也没有用到，相当于我们接收了点云和GPS信息，当然我们这里订阅的点云不是简单的点云，而是作者自定义的一个特殊的msg类型：cloud_info，里面包含了imu等信息。

laserCloudInfoHandler

这个回调函数会处理来自featureExtraction的点云信息，首先它会先将当前帧的时间戳和角点以及平面点集合保存下来，同时会进行频率上的约束，不会让两次执行的间隔低于0.15s。然后就是执行的过程了，在这个里面会调用一大堆函数，这个我们一个个说。

updateInitialGuess()

该函数的作用是进行当前帧的初始化，如果是第一帧的话，我们需要初始化，我们直接将cloud_info中的imuRollInit等信息作为该帧的位姿，也就是赋予imu的原始数据（关于这一点，大家可以在imageProjection.cpp中看，这里不再做过多赘述），并且用lastImuTransformation保存当前帧的imu数据的imuRollInit等信息。

如果是第二帧及以后，并且cloudInfo.odomAvailable == true，我们会利用imu里程计计算得到当前帧的初始估计位姿，这里有一个变量：lastImuPreTransAvailable，这个变量是用来存储上一帧通过imu里程计计算得到的位姿，利用这两个我们可以得到两帧之间的增量，然后根据上一帧的位姿和这个增量，我们就可以得到这一帧的位姿了，那么看到现在的你可能很疑惑，我为啥不能用利用imu里程计估计出来的位姿呢，这是因为：订阅imu数据的是imageProjection.cpp，在它里面，imu数据在接收的时候就会转到Lidar坐标系，但是只是旋转，和真正的Lidar坐标系还差一个平移，所以不能直接使用，但是不管是imu还是Lidar两帧之间的增量是一样啊的，所以我们需要这么处理，那么为什么第一帧是直接用的imu原始数据呢？那当然是我们以第一帧开始啊，它的平移是0。

还有一种情况是，当我们的系统出了一点小问题，有某个地方没有拟合衔接好，cloudInfo.odomAvailable不为true了，不能使用imu里程计了，但是我们又需要imu提供一个初始估计位姿，我们只能使用lastImuTransformation中保存的上一帧的imu.muRollInit等信息和当前帧的imu.imuRollInit等信息进行计算得到增量。

extractSurroundingKeyFrames()&& extractNearby()&&extractCloud()

extractSurroundingKeyFrames()函数非常简单，它里面就是判定了一下现在有没有历史关键帧，如果有的话就会调用extractNearby()
而在extractNearby()中我们会在指定的半径范围内查找历史关键帧，然后把它们组成一个集合，然后我们进行下采样得到集合X，因为我们进行了下采样，所以很有可能几个类似的历史关键帧就抽取了一个，所以我们再去KD树中找到和这个最相似的关键帧，获得其intensity；紧接着我们把当前帧时间前10秒内的关键帧也加入到X中，然后调用extractCloud(X)得到局部地图
在extractCloud中我们首先将laserCloudCornerFromMap和laserCloudSurfFromMap清空，再次遍历集合，将距离超过阈值的再次剔除，其实这一步只是在检验当前帧时间10秒内的关键帧的有效性，因为其他的关键帧已经检验过一次了；如果符合条件的关键帧在laserCloudMapContainer里面那么直接角点和平面点加入laserCloudCornerFromMap和laserCloudSurfFromMap中，如果没有的话还需要进行一次位姿变换，变换到世界坐标系下，然后将角点、平面点同时存入laserCloudCornerFromMap、laserCloudSurfFromMap、laserCloudMapContainer，当然对于得到的局部点云地图我们要进行降采样，得到laserCloudCornerFromMapDS、laserCloudSurfFromMapDS

downsampleCurrentScan()

这个函数很简单，就是对当前帧的角点、平面点降采样，没什么好说的。

scan2MapOptimization()

这个函数采用的是scan to map的匹配方式，首先利用laserCloudCornerFromMapDS、laserCloudSurfFromMapDS构建KD树，然后迭代30次，每次都是进行：
角点：计算点到线的距离&&获得参数
平面点：计算点到面的距离&&获得参数
组合：提取当前帧中与局部map匹配上了的角点、平面点以及相关参数，加入同一集合
调用LMOptimization：对匹配特征点计算Jacobian矩阵，观测值为特征点到直线、平面的距离，构建高斯牛顿方程，迭代优化当前位姿，存transformTobeMapped，然后判定：如果旋转和平移量足够小就返回true，结束此次匹配。

transformUpdate

transformUpdate的作用是：使用IMU的原始数据和当前帧优化后的位姿进行加权融合，来更新roll pitch 约束yaw

saveKeyFramesAndFactor

函数的功能和名字一样，就是保存关键帧并进行因子图优化，首先判断当前帧是否是关键帧：计算和前一帧的位姿，如果位姿变化足够大那么就判定为关键帧，然后添加激光里程计、回环、GPS因子，然后使用isam进行优化，如果存在回环的话再次执行五次优化，在优化结束之后，cloudKeyPoses3D和cloudKeyPoses6D中会加入该帧的位姿，同时 transformTobeMapped也会更新，上述工作进行完之后，继续对当前帧进行降采样，然后更新里程计轨迹

performLoopClosure

下面我们来看一下关于回环检测的操作，我们首先会执行detectLoopClosureDistance，在这个函数里面我们会从历史关键帧中寻找和当前帧距离最近的关键帧，把它列为闭环候选帧，然后根据索引，我们将其周围的关键帧的角点、平面点都组合起来并降采样得到闭环帧局部地图，同时对当前帧进行降采样，然后进行icp匹配，得到优化后的位姿，同时将闭环因子加入到loopIndexQueue、loopPoseQueue、loopNoiseQueue中作为因子图优化因子