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函数：

1.2.1 使用kd树，搜索当前最近的关键帧，在给定半径内（默认是50m）所有关键帧最近的位置，并把结果返回到pointSearchInd，把距离返回到pointSearchSqDis中。

1.2.2 根据索引pointSearchInd，把相邻关键帧存入到surroundingKeyPoses中。

1.2.3 下采样，装进surroundingKeyPosesDS中，并在原始的surroundingKeyPoses其中找到最近的一个点，替换掉索引。（关于这个，我的理解是，下采样后不太准确了，好几个不同的关键帧可能因为下采样的原因混成了一个，所以要用原始数据对索引进行一个修正，这样以后才方便根据索引投影点云）

1.2.4 顺手把10s内的关键帧cloudKeyPoses3D中的位置也加入到surroundingKeyPosesDS中。

1.2.5 extractCloud：提取边缘和平面点对应的localmap，把surroundingKeyPosesDS传入到函数中：

1.2.5.1 对输入的surroundingKeyPosesDS进行遍历，找到50m之内的位置，然后用transformPointCloud把对应位置的点云，进行变换到世界坐标系下。

如何变换呢？根据上面提到的cloudKeyPoses6D的位姿，然后把cornerCloudKeyFrame和surfCloudKeyFrame中根据索引找到点云，投影到世界坐标系下。

那么在这里，cornerCloudKeyFrame和surfCloudKeyFrame是什么？之前从来没有出现过。我这里同样进行剧透，它里面存放的是下采样的点云。注意总结1中的*laserCloudCornerLast和*laserCloudSurfLast这两个东西，这是瞬时点云，这个东西会在之后被下采样，然后装入cornerCloudKeyFrame中。

1.2.5.2在角点点云和平面点点云被投影到世界坐标系下后，会被加入到laserCloudCornerFromMap和laserCloudSurfFromMap等数据结构中，然后再合出一个pair类型的Container<关键帧号，<角点集合，平面点集合>>。

1.3 downsampleCurrentScan：

这部分比较简单，就是对最外层的回调函数中的laserCloudCornerLast之类的东西，进行一个下采样，保存到laserCloudCornerLastDS这些以DS结尾的数据结构中，并且把数目存到laserCloudCornerLastDSNum这种以DSNUM结尾的数据结构中。其实就是代表了当前帧点云的角点/平面点的下采样集合，和数目值。

1.4 Scan2MapOptimization：

这个函数是本cpp中第二复杂的函数。我们现在把它展开。

1.4.1 首先，没有关键帧保存，那就返回，不处理；

1.4.2 如果DSNUM这种记录角点和平面点的数据结构中，发现数目不够多，也不处理；只有在数目足够多的时候才进行处理，默认最少要10个角点，100个平面点。

迭代30次：

 1.4.2.1 边缘点匹配优化：CornerOptimization

1.4.2.2 平面点匹配优化：SurfOptimization

1.4.2.3 组合优化多项式系数：combineOptimizationCoeffs

1.4.2.4 LMOptimization判断迭代误差是否足够小，如果是true则认为迭代完成，返回；

1.4.3 transformUpdate：原始的imu的rpy，在这里和优化后的激光里程计位姿进行一个加权融合。

 接下来，我们依次展开这些函数：

 1.4.2.1 边缘点匹配优化：CornerOptimization

把系统状态transformTobeMapped做一个数据格式转换，变成transPointAssociateToMap形式
从当前角点下采样集合laserCloudCornerLastDS进行遍历，找到世界坐标系下最近的5个点，要求小于1m。
求5个样本的均值，协方差矩阵。对协方差矩阵进行特征值分解，如果最大特征值大于次大特征值的3倍，那么就认为构成线。
一旦发现构成线，那么就在均值沿着最大特征向量方向（把它看成线的方向）前后各取一个点（+-0.1 x 方向）。
X0为当前点，X1和X2为“X0附近的5个点一起算出的均值沿方向前后各取的一点”，叉乘计算三点面积a012，x1x2底边长度l12。然后再做一次叉乘，得到X0距离x1，x2连线的垂线段的单位方向向量（la，lb，lc）。并计算点到直线的距离ld2=a012/l12。
用一个鲁棒和函数，使得距离ld2越大，s越小。然后用coeff来保存“鲁棒后”的距离，和“鲁棒后”的点到线的垂线的方向向量。
如果点到直线的距离小于1m，那么存入数据结构，laserCloudOriCornerVec为原始点云，coeffSelCornerVec为鲁棒距离和鲁棒向量，laserCloudOriCornerFlag代表当前点X0 已经找到对应的线。
思考：为什么要加入方向向量呢？是因为这个在优化的偏导数中会被用到。

1.4.2.2 平面点匹配优化：SurfOptimization

和上面的同理，对系统状态量transformTobeMapped进行数据格式转换；
从当前角点下采样集合laserCloudSurfLastDS进行遍历，找到世界坐标系下最近的5个点，要求小于1m。
直接用matA0存储这个5个点，求解Ax+By+Cz+1=0的ABC系数（用QR分解）
然后对ABCD，代码中为pa，pb，pc，pd=1进行单位化。
根据点x0到平面的距离d= (分母为1）判断是否构成平面。如果有一个大于0.2m则不构成。
pd2为点到平面的距离，也用鲁棒和函数处理，并且比上两次开方（这点我不理解，我猜就是用来鲁棒的，换成1次开方可能也差不多，意义或许不大），然后和角点部分类似，得到s，存入数据结构。
 1.4.2.3 组合优化多项式系数：combineOptimizationCoeffs

这个比较简单，就是把CornerOptimization和SurfOptimization中已经确定匹配关系的点提取出来，laserCloudOri统一把角点和平面点装在一起，coeffSel统一装之前计算得到的“鲁棒优化向量”（角点就是点到直线的“鲁棒垂线”，平面点就是点到平面的“鲁棒法线”）。

优化向量会在LMOptimization中进行优化。

 1.4.2.4 LMOptimization判断迭代误差是否足够小，如果是true则认为迭代完成，返回。

这一部分大内容，主要麻烦在原理上面。

这里推荐一个阅读：LIO-SAM-scan2MapOptimization()优化原理及代码解析

这个文章中公式写的非常好。我就不照搬了。

另外在推导部分，可以仔细研究一下这篇文章：

LeGO-LOAM中的数学公式推导

虽然是Lego-loam的推导，但是Lego-loam和lio-sam在这部分的原理是一样的，因此可以通用。看完这篇文章，就能理解1.4.2.3中我提到的“优化向量”是干啥用的。

总之，照着原理，构建JtJ*delt_x=-JTf，然后构建MatAtA，matAtB，利用cv:solve提供的QR分解，得到matX，即delta_x。
当特征点缺乏时，状态估计方法会发生退化。特征值小于阈值，则视为退化方向向量。这块的理论，可以参考LOAM SLAM原理之防止非线性优化解退化
更新位姿，判断收敛与否。那么真正的雷达里程计系统状态transformTobeMapped，就是在这里被更新。
1.4.3 transformUpdate：原始的imu的rpy，在这里和优化后的激光里程计位姿进行一个加权融合。

当imuAvailable=True的时候，并且俯仰角<1.4，那么对位姿和原始imu的rpy做一个加权平均，（权重在配置文件中可以被设置为0.01）。主要是对roll，pitch仅加权平均，并且对z进行一个高度约束（也就是clip，不得超过配置文件中的z_tollerance，这个主要是一个小trick，应对不能飞起来的无人小车用的），更新transformTobeMapped。

好了，那么 现在回到回调函数的主流程：

1.5 saveKeyFramesAndFactor：之前函数二话不说就用了一些并没有出现过的数据结构，例如什么cloudKeyPoses3D，cornerCloudKeyFrame之类的东西，看完这个函数将明白这些变量是怎么来的。

1.5.1 saveFrame：计算当前帧和前一帧位姿变换，如果太小不设关键帧。默认是角度<0.2，距离<1m，两者不满足其一就不保存；

1.5.2 addOdomFactor：

这个是要加入激光里程计因子，给图优化的模型gtSAMgraph。在1.5之前别的函数里，如果没有关键帧，直接就跳过了。但是这里不能跳过。

如果暂时还没有关键帧，就把当前激光系统状态transformTobeMapped，打包成一个PriorFactor加入到gtSAMgraph里。如果目前已经有关键帧了，就把最后一个关键帧，和当前状态transformTobeMapped计算一个增量，把这个增量打包成一个BetweenFactor，加入到gtSAMgraph里头去。

initialEstimate代表变量初值，用transformTobeMapped赋值。

1.5.3 addGpsFactor：

GPS的筛选规则为：如果没有GPS信息，没有关键帧信息，首尾关键帧小于5m，或者位姿的协方差很小（x，y的协方差阈值小于25），就不添加GPS。

否则，遍历GPS列表，当前帧0.2s以前的不要，或者GPS的协方差太大了也不要，无效数据也不要…… 找到正常数据，打包成一个gps_factor，加入gtSAMgraph里面。

1.5.4 addLoopFactor：

这个其实和当前的回调函数无关，因为当前回调函数监听的是/feature/cloud_info信息，回环是由其他线程监控和检测的。那么在这里，它查询回环队列，加入回环因子，就是一个顺手的事情，反正现在要更新优化，那么查一下，如果有候选的等在那里，就顺手加入优化。如果用做饭来比喻这件事，那么另外的回环检测的线程就是相当于另一个人在备菜，这里addLoopFactor相当于是在炒菜，备好了就先炒，没有备好就算了。

1.5.5 gtsam正常更新。如果有回环那就多更新几次。

1.5.6 把cloudKeyPoses3D，cloudKeyPoses6D，分别装上信息，cloudKeyPoses3D代表关键帧的位置，cloudKeyPoses6D代表关键帧的位置+姿态，为什么要有一个3D一个6D呢？6D里不已经包含了3D信息吗？这个问题我在1.2处已经解释过了。

1.5.7 用优化结果更新transformTobeMapped。

1.5.8 cornerCloudKeyFrames，surfCloudKeyFrames装入信息，回顾一下，回调函数开头收到的点云数据为laserCloudCornerLast，laserCloudSurfLast，然后在downsampleCurrentScan处这俩信息被下采样，加上了DS后缀。在这里把它装到cornerCloudKeyFrames和surfCloudKeyFrames中。

（回顾：cornerCloudKeyFrames代表关键帧位置处的角点点云，surfCloudKeyFrames代表关键帧位置处的平面点点云。这俩东西就是上面1.2处extractSurroundingKeyFrames用到的内容，cornerCloudKeyFrames通过cloudKeyPoses6D变换到世界系下，被存到laserCloudCornerFromMap里面，这个FromMap又在scan2MapOptimization函数中被设置到kdtreeCornerFromMap这个Kd树里，在cornerOptimization函数里，就是把当前帧的激光点云依据1.1的初值transformTobeMapped，变换到世界坐标系下，再用kdtreeCornerFromMap进行kd搜索，建立匹配关系，优化transformTobeMapped。）

1.5.9 updatePath，更新里程计轨迹。把cloudKeyPoses6D传入，保存在globalPath中。不过暂时还没有进行发布。

 1.6 correctPoses：

如果发现回环的话，就把历史关键帧通通更新一遍。我们刚刚在1.5.5里面虽然更新过了，但是结果都是保存在gtsam里面的，cloudKeyPoses3D和cloudKeyPoses6D，这俩保存位置和位姿的变量仍然保留着更新前的关键帧位姿。所以就根据更新结果，把他俩更新一遍。

为什么不更新cornerCloudKeyFrames和surfCloudKeyFrames呢？因为没有必要更新，这俩存的是机器人坐标系下的点云，和机器人在世界系下的位姿是无关的。

1.7 publishOdometry：

到此为止，激光里程计部分的transformTobeMapped就不再更新了。回顾一下transformTobeMapped经历了哪些变换：在1.1部分用imu角度初值或是imu里程计初值赋值，然后在scan2mapOptimization里面根据点到线、点到面的方程进行更新，再在transformUpdate里和原始imu的rpy信息进行一个很小的加权融合（不过这一步我觉得没啥大用），最后在saveKeyFrameAndFactor里面再加入GPS因子和回环因子进行一轮优化。

最后把transformTobeMapped发布出去，其他cpp文件里，接收的“激光里程计”就是这么个东西。也就是lio_sam/mapping/odometry_incremental.

1.8 publishFrames:

这个纯粹就是把乱七八糟东西都发布出去，不管有没有用。如果用户需要就可以监听它。

1.8.1发布关键帧位姿集合，把cloudKeyPoses3D发布成lio_sam/mapping/trajectory

1.8.2发布局部降采样平面点，把laserCloudSurfFromMapDS（历史默认50m内的点，在extractCloud中被设置），发布为lio_sam/mapping/map_local

1.8.3发布当前帧的下采样角点和平面点，用优化后的激光里程计位姿transformTobeMapped投影到世界系下发布，/lio_sam/mapping/cloud_registered

1.8.4发布原始点云经过配准的点云：输入的/feature/cloud_info的cloud_deskewed字段是由featureExtraction.cpp发布的，其cloud_deskewed是源于imageProjection.cpp发布的原始去畸变点云。把它发布到世界坐标系下，然后以/lio_sam/mapping/cloud_registered_raw的形式发布。

1.8.5发布轨迹，把1.5.9里装好在globalPath里面但是还没有发布的轨迹发布出去，名为/lio_sam/mapping/path。

那么到现在，基本上mapOptimization.cpp的内容就结束了，但是还有一些尾巴：

2.gpshandle：监听GPS数据，保存到GPS队列里。

3.loopinfohandle：监听"lio_loop/loop_closure_detection",订阅来自外部闭环检测程序提供的闭环数据，本程序没有提供，这里实际没用上。

4.loopClosureThread：这个线程在主函数里单独开了一个线程，简要说一下：

4.1 读取配置文件中是否开启回环检测。

4.2开始无限循环：

4.2.1 performLoopClosure：

在历史帧中搜索距离关键帧最近，时间间各较远的关键帧（默认是30s以外，15m以内）没找到就返回，如果找到了，结果就放在loopKeyPre当中，loopKeyCur保存最近一个关键帧。
把最近一个关键帧的特征点提出来，放入cureKeyframeCloud里；回环候选帧前后各25帧也提取出来，放入prevKeyframeCloud里。
把prevKeyframeCloud发布出去，名为lio_sam/mapping/icp_loop_closure_history/cloud
调用pcl库的icp轮子，设定阈值，参数，用setInputSource，setInputTarget传入两个点云，用align对齐。成功阈值设定为0.3，成功则存在icp.getFinalTransformation里面。把当前关键帧的点云，用这个结果icp.getFinalTransformation，转换以后，以lio_sam/mapping/icp_loop_closure_corrected_cloud发布出去。
把当前帧的的位姿用icp.getFinalTransformation结果校正一下，把pair<当前，回环>，间隔位姿，噪声用队列存起来，等待addLoopFactor来调用，即上面的1.5.4部分。
4.2.2 visualizeLoopClosure:

这部分内容没啥好说的，就是用于rviz展示，把关键帧节点和二者的约束用点和线连起来，以lio_sam/mapping/loop_closure_constraints发布出去。

5. 最后一个线程，visualizeGlobalMapThread:

这个主要是两块内容：

5.1 publishGlobalmap：把当前关键帧附近1000m（默认）的关键帧找出来（其实也就是全局的了），降采样，变换到世界系下，然后发布为lio_sam/mapping/map_global.

5.2 saveMapService:这个用来保存pcd格式的点云地图。在配置文件中可以设置开启与否，和存储位置。注意，当程序结束时，ctrl+c以后，才会启动保存任务。这个部分的代码，和发布globalmap部分的核心内容基本一致，反正就是把cornerCloudKeyFrames，surfCloudKeyFrames用cloudKeyPoses6D变换到世界系下，分别保存角点pcd和平面点pcd，以及全局(合起来）的pcd文件。

个人学习使用，摘自：SLAM学习笔记（二十）LIO-SAM流程及代码详解（最全）_zkk9527的博客-CSDN博客_lio-sam