LIO-SAM mapOptimization.cpp 函数的核心注释

1.第一个巨大的回调函数：lasercloudinfoHandle：

这个函数监听的是/feature/cloud_info，关于这个话题里面包含的内容，我已经在featureExtraction.cpp的总结部分说过了。这里回忆一下，cloud_info是作者自定义的一个特殊的msg类型，包含了当前激光帧的角点集合，平面点集合，一维数组，每根线开始的点和结束点在一维数组中的索引……

那么收到数据，先保存时间戳，然后提取已经在featureExtraction.cpp中被提取的角点和平面点信息，保存到*laserCloudCornerLast和*laserCloudSurfLast中。请记住这两个命名。

频率控制：当前时刻-上一时刻>=0.15s时，才开始下列循环：

1.1  UpdateInitGuess：当前帧初始化。

1.1.1 当关键帧集合为空时，用激光帧的imu原始数据角度数据初始化，并且把数据用lastImuTransformation保存（记住这个命名），返回。

此时推荐回顾一下imageProjection.cpp部分的总结3.2

1.1.2 假如cloudInfo.odomAvailable=true时，那么就用一个transBack来记录cloudInfo.initialGuessX等信息，（这个信息其实来自于imupreintegration.cpp中发布的imu里程计数据），然后记录增量，在之前系统状态transformTobeMapped的基础上，加上这个增量，再次存入transformTobeMapped。 注意这个transformTobeMapped，这个数据结构，在这个cpp里，我们可以理解为就是它在存储着激光里程计部分的系统状态，包括位置与姿态。

注意，这里有一个lastImuPreTransformation，这个用来保存上一时刻imu里程计的数据，根据它和当前的imu里程计来算增量。不要和lastImuTransformation变量混起来，虽然这俩变量名字长的很像。

然后覆盖lastImuTransformation（在这个case里没用到），返回；

1.1.3 假如cloudInfo.imuAvailable=true，那么进入这个case：

注意，lastImuTransformation在1.1.1和1.1.2中并未用到，只是不断的在替换成最新数据。当cloudInfo.odomAvailable一直是true的时候，程序压根也不会进入到这个case。

但是，凡事总有例外，万一哪里没有衔接好，imu里程计没有及时算出来，那么就导致此时激光帧的位姿没有一个初始化的数据（我们之后是要在初始化的基础上进行优化的），那么之后的优化就无从进行。因此，就要用到这个case。

这里主要思路是用imuRollInit数据来初始化，如果你回顾过imageProjection.cpp部分的总结3.2 那么你应该就会懂，这里的数据来源是原始imu数据的角度信息。那么如果这里有数据，就用lastImuTransformation当成最新的上一时刻数据，当前数据transBack和它算一个增量，然后累积到系统值transformTobeMapped上面去。最后更新覆盖lastImuTransformation，返回。

 1.2 extractSurroundingKeyFrames：这个是比较复杂的一个函数，以下的内容希望读者可以心平气和的，每个字都依次念一遍。

如果没有关键帧，那就算了，返回；

如果有，就调用extractNearby函数。

关键帧是啥？cloudKeyPoses3D，我们要记住这个变量，虽然到现在为止，我们还不知道它是怎么来的，但是这个东西是怎么获取的，我们在后续必须弄明白。

在这里我先剧透一下：它里面装的是历史的“关键帧”的位置，即x，y，z三个值。需要明确：这里装的绝不是历史的关键帧位置处的点云。而是历史关键帧记录时刻机器人所在的位置。

同理还有一个cloudKeyPoses6D，它比这个3D还多了三个角度信息。之所以要用一个6D一个3D分别来装关键帧，我现在直接揭晓答案：用3D装，是因为我们要根据这个来构建KD树，搜索历史上最近的节点。“最近”指的是距离上最近，即xyz空间坐标最近，和角度无关。而cloudKeyPoses6D，是用来投影点云的，把当前帧点云投影到世界坐标系下，那么投影就必须要用角度信息了，所以作者分别用了一个3D和一个6D来装数据。

Kd树的原理我这里不写，随便放一个链接：机器学习——详解KD-Tree原理 - 知乎 ，实际上代码里也只是调库，所以这里我不写。

那么接下来，我介绍extractNearby函数：<