无人驾驶学习笔记-LeGO-LOAM 算法源码学习总结

与loam 类似，整体分四个部分，分别执行四个节点。不同的是，lego-loam将loam中特征提取sacnRegistration节点，与帧间匹配的laserOdometry合并为featureAssociation节点，增加了imageProjection节点，同时在mapOptimization 节点开闭闭环检测与全局优化线程。各节点具体功能如下：

imageProjection节点负责将原始点云投影为深度图，并利用深度图进行快速的地面点分割和目标点聚类，有效的对点云降维。
featureAssociation节点负责从分割点中提取线面特征，并进行scan-to-scan的特征匹配，使用两次LM优化估计LiDAR在 $t_{k-1}$ 到 $t_{k}$ 的相对运动变换 $T_{k-1}^{k}$ 。
mapOptimization节点一方面负责利用 $T_{k-1}^{k}$ 预测LiDAR位姿 $T_{k}^{w}$ ，由scan-to-map的特征点云匹配对预测的 $T_{k}^{w}$ 进一步优化；另一方面，在并行线程中根据关键帧位置点间的距离检测回环，并采用ICP算法匹配构成回环的关键帧点云，最后进行全局位姿图优化。
transformFusion节点主要负责将高频运行的featureAssociation节点和低频运行的mapOptimization节点估计的位姿进行整合输出，保证高频的位姿输出频率。

6、各部分方法原理及代码注释

6.1、点云投影与目标分割

1、总结概述

1）首先将收到的一帧点云投影到一副分辨率为1800*16的图像上（水平精度0.2°，16线lidar），每一个收到的点代表图像的一个像素，像素的值r{i} 表示相应的点p{i} 到sensor的欧式距离。

1800 ：由于VLP-16的水平方向的角度分辨率为 0.2°，所以，投影图像的水平方向分辨率为 360/2 =1800。
16：由于VLP-16的竖直方向是16根扫描线，所以，投影图像的竖直方向的分辨率就变成了 16 .

点云投影实际上是一种将无序点云有序化的过程，我们可以直接从投影的点云深度图中索引到某一点的上下或两侧的点。这也将极大提高目标分割的效率。

2）完成图像化之后得到一个矩阵，对矩阵的每一列进行估计（即完成对地面的估计），在分割之前进行地面点的提取。

由于距离的竖直方向为 16，这个也代表了原始三维空间中的竖直维度的特性。所以，通过判断其竖直维度的特性，可以很好的标记出地面点和非地面点。
通过VLP-16的激光扫描束的范围为 [-15°，15°] ，地面点必然出现在 [-15°，-1°] 扫描线上。

在这个过程中被标记为地面点不用于进行后续分割。

3）将基于图像的分割方法应用于距离图像以将点分组为多个聚类。同一类的点具有相同的一个标签（地面是较为特殊的一类）；

随风飘动的树叶，草等，在实际的实验中会经常遇，可能会造成出现在前一帧而不出现在后一帧的情况。
去除不能聚成类的点云，可以在保留当前帧特征信息的基础上，减少噪音点的干扰，提升特征提取精度。

为了提高处理效率，本文将低于30个数据点的类别都当作噪点处理，这样保存下来的就是一些相对来说比较静态的物体了（例如，树干，楼房等）。这样的话，整个距离图像就可以被分成若干个比较大的类别。加上前面设定的一个类别——地面点，这样就可以进行后续的处理了。

基于这一步的处理，一帧点云中的点可以被小区很多噪音点，结果如下所示：

完成这一步之后，图像里只保留了可能代表大物体的点，然后记录下点的三个值：

分割类的标签（地面点或分割点）;
在距离图像中的行和列的索引也就是点在矩阵中的坐标(x,y);
与传感器的距离值即 r_{i} 在接下来的模块里使用。

2、代码注释

imageProjection.cpp 主要完成图像的映射，将得到的激光数据分割，并在得到的激光数据上进行坐标转换。

直接进入核心算法的函数

2.1、copyPointCloud()

将 ros 消息转换为 pcl 点云，这里对velodyne的lidar做了区分处理，以16线雷达为例，激光雷达通过16根雷达光束进行旋转扫描，从而得到一周360°的点云。velodyne的lidar对点云属于16线中的哪一线做了标记，这个标记被称为Ring index，其中的Ring表示环的意思。如果不是velodyne的lidar后面会通过计算得出点云属于哪个线束。因此保存点云的时候分别保存为"laserCloudIn"和"laserCloudInRing"中。

2.2、findStartEndAngle()

查找起始角度和终止角度，这里的角度是激光雷达旋转一周的起始角度和终止角度，选取的起点是points[0]，选取的终点是points[laserCloudIn->points.size() - 1]，是否是说点云是按照顺序排列的呢？另外这里最后一个点并不一定是最大角度。

此外还对x,y进行了坐标转换，关于这一步的结果会在特征提取中用到。关于雷达的坐标系定义如下图

引自LeGO-LOAM 源码阅读笔记(imageProjecion.cpp)_wykxwyc的博客-CSDN博客

VLP雷达坐标系

在计算角度的时候，应用atan2()函数取负数的原因：雷达旋转方向和坐标系定义下的右手定则正方向不一致。如下所示：

2.3、projectPointCloud();

这一步把点云投影到RangeImage上，也就是说得到的原始点云是一个球形，我们把球形投影到一个圆柱体上然后把它展开，下图是一个简单的示例。

引自LeGO-LOAM分析之点云分割（一） - 知乎

以16线激光为例，转换的range图，长为16，宽为旋转一周的采样次数，这里为1800，因此最后得到的是16*1800的图像。这个图像可以用数组表示，其中的坐标为x,y，而z会转换为深度信息，最后转换好的rangeImage效果如下图，类似一个深度图。

将激光雷达得到的1800*16的点云阵列。根据每个点云返回的xyz数据将他们对应到这个阵列中去。

代码流程：

计算竖直角度，用atan2函数进行计算。
通过计算的竖直角度得到对应的行的序号rowIdn，rowIdn计算出该点激光雷达是水平方向上第几线的。从下往上计数，-15度记为初始线，第0线，一共16线(N_SCAN=16)。
求水平方向上的角度horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;。
根据水平方向上的角度计算列向量columnIdn。这里对数据的处理比较巧妙，一开始觉得很奇怪，但后来发现这样做其实让数据更不容易失真。columnIdn函数计算

先把columnIdn从horizonAngle:(-PI,PI]转换到columnIdn:[H/4,5H/4],然后判断columnIdn大小，再讲它的范围转换到了[0,H] (H:Horizon_SCAN)。

这样就把扫描开始的地方角度为0与角度为360的连在了一起，非常巧妙。

接着在thisPoint.intensity中保存一个点的位置信息rowIdn+columnIdn / 10000.0，fullInfoCloud的点保存点的距离信息；

具体的转换过程可以看下面这个两张图：

2.4、groundRemoval()

由三个部分的程序组成。

接下来的处理都是基于rangeImage，去除地面的过程如下，对rangeImage中行Ringindex小于7的点进行地面检测，如果是32线雷达，则是20以下的，所以这里是利用了Lidar的先验知识。

地面检测的原理如上图所示，通过判断rangeImage中A和相邻点B之间的夹角ɑ是否足够小，如果角度足够小则判断为地面。这里选择的角度阈值为10°，如果小于10°则为地面。

代码流程：

由上下两线之间点的XYZ位置得到两线之间的俯仰角，如果俯仰角在10度以内，则判定(i,j)和(i,j+1)为地面点,groundMati=1，否则，则不是地面点，进行后续操作；
找到所有点中的地面点，并将他们的labelMat标记为-1，rangeMati==FLT_MAX 判定为无效的另一个条件。
如果有节点订阅groundCloud，那么就需要把地面点发布出来。具体实现过程：把点放到groundCloud队列中去。这样就把地面点和非地面点标记并且区分开来了。

2.5、cloudSegmentation()

引自LeGO-LOAM分析之点云分割（一） - 知乎

在去除地面之后，对接下来的点进行分割。这里通过BFS（深度优先遍历）递归进行查找，从[0,0]点开始，遍历它前、后、左、右的4个点，分别进行对比，如果相对角度大于60°，则认为是同一个点云集群。最后分割出来的点云数量大于30个则认为分割有效（实际上大于5个可能也行）。

这里对cloudSegmentation拆分成2部分进行介绍，先进行标记labelComponents，也就是递归找到特征比较明显的点。

作者说用vector和deque会变慢速度，不知道是不是数组的动态扩展导致的（当预先分配的vector大小不够时候，会重新malloc2倍的大小，然后把原先的数组拷贝过去，系统自动完成），如果设置固定大小，我认为不会导致明显的性能下降。这里作者用4个数组来代替了queue的操作，看起来有点别扭。

点云分割的原理如下图所示，A和B是Lidar扫描的连续2个点云，如果A和B的夹角β比较小，例如下图中红色的线（代码中为60°）那么证明这2个点不属于同一个物体，反之，如果这2个点的夹角β比较大，例如下图中绿色的线，则可以认为是同一个物体，通过这个方法对点云进行分割。

具体程序步骤：

首先判断点云标签，这个点云没有进行过分类（在原先的处理中没有被分到地面点中去或没有分到平面中），则通过labelComponents(i, j);对点云进行分类。进行分类的过程在labelComponents中进行介绍。
分类完成后，找到可用的特征点或者地面点(不选择labelMati=0的点)，按照它的标签值进行判断，将部分界外点放进outlierCloud中。continue继续处理下一个点。
然后将大部分地面点去掉，省下的那些点进行信息的拷贝与保存操作。
最后如果有节点订阅SegmentedCloudPure，那么把点云数据保存到segmentedCloudPure中去。

labelComponents对点云进行标记。通过标准的BFS算法对点进行标记：以(row，col)为中心向外面扩散，判断(row,col)是否属于平面中一点。

BFS过程： 1.用queueIndX，queueIndY保存进行分割的点云行列值，用queueStartInd作为索引。 2.求这个点的4个邻接点，求其中离原点距离的最大值d1最小值d2。根据下面这部分代码来评价这两点之间是否具有平面特征。注意因为两个点上下或者水平对应的分辨率不一样，所以alpha是用来选择分辨率的。
在这之后通过判断角度是否大于60度来决定是否要将这个点加入保存的队列。加入的话则假设这个点是个平面点。
然后进行聚类，聚类的规则是：

1.如果聚类超过30个点，直接标记为一个可用聚类，labelCount需要递增；

2.如果聚类点数小于30大于等于5，统计竖直方向上的聚类点数

3.竖直方向上超过3个也将它标记为有效聚类

4.标记为999999的是需要舍弃的聚类的点，因为他们的数量小于30个

2.6、 publishCloud() 发布各类点云数据

2.7、resetParameters() 初始化，重置各类参数内容

6.2、特征提取与激光里程计

1、总结概述

1.1、特征提取

特征提取过程和loam中类似，不同的是从地面点和线点中提取特征；

代码实现参考featureAssociation.cpp的calculateSmoothness()、markOccludedPints()和extractFeatures()等函数。

引自 LeGO-LOAM论文翻译（内容精简)_wykxwyc的博客-CSDN博客_loam论文翻译

图2

通过上述操作，就可以得到一帧点云中的四个特征点集合：F_e、F_me、F_p、F_mp。

纵观整个过程，lego-loam的特征提取和loam的特征提取的区别在于

使用的点云类型不同
计算公式不同
平面点和边缘点的选择标准不同

这里虽然特征点集合看上去比LOAM多，但是其实和LOAM的是一样的，不过LOAM在论文中没有点明的。

下面依次进行对比：

图片引自LeGO-LOAM和LOAM的区别与联系 - 知乎

1.激光里程计

帧间匹配主要根据上述提取出的面特征和线特征进行匹配，然后根据最小二乘法，得出前后帧之间的坐标转换，具体方法和loam中一致。相对于loam不同的是：lego-loam根据标签分别对线和面进行匹配；两步LM 优化：根据线特征匹配得到tx ty yaw，根据面特征匹配得到tz roll pitch。代码实现见featureAssociation.cpp的updateTransformation()函数：

引自LeGO-LOAM和LOAM的区别与联系 - 知乎第4章节

但是，LeGO-LOAM还做了些许的改进来提高匹配的准确性和效率，主要在以下两个方面：

标签匹配

两步LM优化

标签匹配

这一步的操作其实非常巧妙，有以下两个优点：

在相邻帧中地面信息基本保持不变
聚类后点云被分为若干块，缩小了对应点的候选范围

不过，这一步的处理也不是没有问题的，在实际的实验中，如果地面不是特别的平坦，也就是说相邻帧间的地面有了一定的变动，这个时候，LeGO-LOAM就不能很好的运行。

再获取了点到线和点到面的对应关系后，需要使用列文伯格-马夸特法（LM）优化方法来进行优化求解。

两步LM优化

关于分两步优化的原理解释可以参考LeGO-LOAM分析之特征提取（二） - 知乎

线特征匹配与面特征匹配的原理可以参考之前总结的笔记无人驾驶学习笔记 - LOAM 算法论文核心关键点总结_ppipp1109的博客-CSDN博客

2、代码注释

2.1、featureAssociation.cpp

特征关联，包括特征提取与帧间匹配

2.2、imuHandler函数

通过接收到的imuIn里面的四元素得到roll,pitch,yaw三个角；
对加速度进行坐标变换(坐标变换可以参考下面这张图)；
将欧拉角，加速度，速度保存到循环队列中；
对速度，角速度，加速度进行积分，得到位移，角度和速度(AccumulateIMUShiftAndRotation())；

加速度坐标交换的示意图：

进行加速度坐标交换时将重力加速度去除，然后再进行x 到z ,y 到x ,z 到y 的变换。
去除重力加速度的影响时，需要把重力加速度分解到三个坐标轴上，然后分别去除他们分量的影响，在去除的过程中需要注意加减号（默认右手坐标系的旋转方向来看）。
在上面示意图中，可以简单理解为红色箭头实线分解到红色箭头虚线上（根据pitch进行分解）,然后再按找roll角进行分解。

2.3、runFeatureAssociation

该函数是featureAssociation.cpp中最主要的函数，它调用这个cpp文件中的其他函数。关于特征提取，运动补偿，曲率计算，去除异常遮挡点等知识点参考之前写的总结无人驾驶学习笔记 - LOAM 算法论文核心关键点总结_ppipp1109的博客-CSDN博客

算法步骤如下：

如果有新数据进来则执行，否则不执行任何操作；
将点云数据进行坐标变换，进行插补等工作；
进行光滑性计算，并保存结果；
标记阻塞点(阻塞点：点云中可能出现的互相遮挡的点)；
特征提取，然后分别保存到cornerPointsSharp等等队列中去；
发布cornerPointsSharp等4种类型的点云数据；
预测位姿；
更新变换；
积分总变换；
发布里程计信息及上一次点云信息；

2.4、adjustDistortion

将点云数据进行坐标转换，进行运动补偿等工作。

对每个点进行处理，首先进行坐标轴转换
针对每个点计算偏航角yaw，然后根据不同的偏航角，可以知道激光雷达扫过的位置有没有超过一半，计算的时候有一部分需要注意一下：

这里分四种情况
- 没有转过一半，但是start-ori>M_PI/2
- 没有转过一半，但是ori-start>3/2*M_PI,说明ori太大，不合理（正常情况在前半圈的话，ori-start范围[0,M_PI]）
- 转过一半，end-ori>3/2*PI,ori太小
- 转过一半，ori-end>M_PI/2,太大
然后进行imu数据与激光数据的时间轴对齐操作。对齐时候有两种情况，一种不能用插补来优化，一种可以通过插补进行优化。
- 不能通过插补进行优化：imu数据比激光数据早，但是没有更后面的数据(打个比方,激光在9点时出现，imu现在只有8点的) 这种情况下while循环是以imuPointerFront == imuPointerLast结束的：
- 可以通过插补来进行数据的优化：这种情况只有在imu数据充足的情况下才会发生，进行插补时，当前timeScanCur + pointTime < imuTime[imuPointerFront]，而且imuPointerFront是最早一个时间大于timeScanCur + pointTime的imu数据指针。 imuPointerBack是imuPointerFront的前一个imu数据指针。
通过上面计算的ratioFront以及ratioBack进行插补, 因为imuRollCur和imuPitchCur通常都在0度左右，变化不会很大，因此不需要考虑超过2 π 2\pi2π的情况, imuYaw转的角度比较大，需要考虑超过2 π 2\pi2π的情况。
后面再进行imu的速度插补与位置插补。

另外，针对 i = 0 i=0i=0 的情况（另一个不同的点云），每次都要用和上面相同的方法判断是否进行插补并且更新 imu 的数据。更新的数据用途：后面将速度坐标投影过来会用到 i = 0 i=0i=0 时刻的值。

2.5、calculateSmoothness

用于计算光滑性，这里的计算没有完全按照公式LOAM论文中的进行。此处的公式计算中没有除以总点数 i 及 r[i] .

2.6、MarkOccludedPoints

选择了距离比较远的那些点，并将他们标记为1，还选择了距离变换大的点，并将他们标记为1。

2.7、extractFeatures 特征提取

接下来就进行提取特征的操作了，提取的特征分为2类，一类是曲率比较大的线特征，一类是曲率比较小的面特征。

提取的方法是把平面划分为6等分，也就是6个方向（LOAM中为前、后、左、右4个方向），根据上述计算好的曲率进行排序，然后每个方向最多选择2个线特征和4个面特征，如果超过则跳过，在下一个方向上继续选择。如果一个点已经被选择为特征点，那么它的相邻点会被标记，不允许被选为特征了。

最后总结一下，特征点的选择满足以下3个条件。

选择的边缘点或平面点的数量不能超过每个方向上的最大值，一共有6个方向，每个方向上最多2个线特征，4个面特征。
线特征和面特征周围相邻的点不能被选中。
不能是平行于激光雷达光束的点或者被遮挡的点。

然后接着总结下代码中的输出

cornerPointsSharp 线特征（不为地面），每个方向上最多2个
cornerPointsLessSharp 比cornerPointsSharp平滑的线特征（不为地面），每个方向上20个
surfPointsFlat 面特征（为地面），每个方向上最多4个
surfPointsLessFlat 面特征（地面或者分割点云中不为线特征的点）

至此，特征提取就介绍完毕了

2.8、updateTransformation

其中主要是两个部分，一个是找特征平面，通过面之间的对应关系计算出变换矩阵。另一个部分是通过角、边特征的匹配，计算变换矩阵。该函数主要由其他四个部分组成：findCorrespondingSurfFeatures,calculateTransformationSurf findCorrespondingCornerFeatures,calculateTransformationCorner 这四个函数分别是对应于寻找对应面、通过面对应计算变换矩阵、寻找对应角/边特征、通过角/边特征计算变换矩阵。

2.9、integrateTransformation

计算了旋转角的累积变化量。这个函数首先通过AccumulateRotation()将局部旋转左边切换至全局旋转坐标。然后同坐变换转移到世界坐标系下。再通过PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart, imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz);插入imu旋转，更新姿态。

6.3、地图优化

1、总结概述

引自LeGO-LOAM分析之建图（三） - 知乎

基于传感器视域

基于凸优化

下面详细介绍两种方法

1）基于传感器视域

根据传感器的有效探测距离，选取一个cube，将里面的特征融合成 $Q^{t-1}$ 就得到当前位置附近的全局点云地图。

VLP-16的有效探测距离是100m，所以，本文就选用100m，来获取 $Q^{t-1}$ 。

此方法和LOAM的方法基本一致。

2）通过凸优化建立姿态图

这一部分和loam的最大区别在于加入了位姿图和回环检测，解决了loam没有后端优化的弊端，提升了建图效率。

2、代码注释

mapOptmization.cpp 主要功能就是地图优化，将得到的局部地图信息融合到全局地图中。

2.0、loopthread 回环检测

可以消除漂移，通过ICP算法对比前后帧是否匹配，如果匹配则进行凸优化

2.1、loopClosureThread()

主要函数performLoopClosure 函数流程：

先进行闭环检测detectLoopClosure()，如果返回true,则可能可以进行闭环，否则直接返回，程序结束。判断条件是首尾之间的距离小于7米，并且时间相差30s以上。
接着使用icp迭代进行对齐。作者同时提醒回环检测的ICP算法当里程计漂移太大时经常失败。对于更高级的闭环方法，建议采样 SC-LeGO-LOAM ，它的特征采用的是点云描述符
对齐之后判断迭代是否收敛以及噪声是否太大，是则返回并直接结束函数。否则进行迭代后的数据发布处理。
接下来得到latestSurfKeyFrameCloud和nearHistorySurfKeyFrameCloudDS之间的位置平移和旋转。
然后进行图优化过程。

RANSAC（Random Sample Consensus）是根据一组包含异常数据的样本数据集，计算出数据的数学模型参数，得到有效样本数据的算法。

2.2、visualizeMapThread

主要函数publishGlobalMap 三个步骤：

通过KDTree进行最近邻搜索;
通过搜索得到的索引放进队列; 3.通过两次下采样，减小数据量;

2.3、run

run()是mapOptimization类的一个成员变量

run()的运行流程： 1.判断是否有新的数据到来并且时间差值小于0.005； 2.如果timeLaserOdometry - timeLastProcessing >= mappingProcessInterval，则进行以下操作： 2.1.将坐标转移到世界坐标系下，得到可用于建图的Lidar坐标，即修改transformTobeMapped的值； 2.2.提取周围的关键帧； 2.3.下采样当前scan； 2.4.当前scan进行图优化过程； 2.5.保存关键帧和因子； 2.6.校正位姿； 2.7.发布Tf； 2.8.发布关键位姿和帧数据

2.4、mapOptimization

负责订阅数据的话题，发布点云位姿等的画图

2.5、transformAssociateToMap函数

将坐标转移到世界坐标系下，得到可用于建图的Lidar坐标，即修改了transformTobeMapped的值。

2.6、extractSurroundingKeyFrames 提取周围关键帧

如果使能了回环检测，则添加过去50个关键帧，如果没有使能回环检测，则添加离当前欧式距离最近的50个关键帧，然后拼接成点云。

自然语言描述

extractSurroundingKeyFrames(){
if(cloudKeyPoses3D为空) return；
if(进行闭环过程){
1.若recentCornerCloudKeyFrames中的点云数量不够， 清空后重新塞入新的点云直至数量够。
2.否则pop队列recentCornerCloudKeyFrames最前端的一个，再往队列尾部push一个；
        *laserCloudCornerFromMap += *recentCornerCloudKeyFrames[i];
        *laserCloudSurfFromMap   += *recentSurfCloudKeyFrames[i];
        *laserCloudSurfFromMap   += *recentOutlierCloudKeyFrames[i];
}else{
/*这里不进行闭环过程*/
1.进行半径surroundingKeyframeSearchRadius内的邻域搜索
2.双重循环，不断对比surroundingExistingKeyPosesID和surroundingKeyPosesDS中点的index,
如果能够找到一样，说明存在关键帧。然后在队列中去掉找不到的元素，留下可以找到的。
3.再来一次双重循环，这部分比较有技巧，
这里把surroundingExistingKeyPosesID内没有对应的点放进一个队列里，
这个队列专门存放周围存在的关键帧，
但是和surroundingExistingKeyPosesID的点不在同一行。
关于行，需要参考intensity数据的存放格式，
整数部分和小数部分代表不同意义。
}
不管是否进行闭环过程，最后的输出都要进行一次下采样减小数据量的过程。
最后的输出结果是laserCloudCornerFromMapDS和laserCloudSurfFromMapDS。
}

2.7、downsampleCurrentScan

1.下采样laserCloudCornerLast得到laserCloudCornerLastDS；
2.下采样laserCloudSurfLast得到laserCloudSurfLastDS;
3.下采样laserCloudOutlierLast得到laserCloudOutlierLastDS;
4.laserCloudSurfLastDS和laserCloudOutlierLastDS相加，得到laserCloudSurfTotalLast；
5.下采样得到laserCloudSurfTotalLast，得到得到laserCloudSurfTotalLastDS;

2.8、scan2MapOptimization

是一个对代码进行优化控制的函数，主要在里面调用面优化，边缘优化以及L-M优化。该函数控制了进行优化的最大次数为10次

上面laserCloudCornerFromMapDSNum和laserCloudSurfFromMapDSNum是我们在函数extractSurroundingKeyFrames()中刚刚更新的。

关于上面的三个优化函数，在下文中有对优化函数的详细分析： 1.关于特征边缘的优化：cornerOptimization; 2.关于特征平面的优化：surfOptimization; 3.关于特征边缘和特征平面的联合L-M优化方法：LMOptimization。

2.9、saveKeyFramesAndFactor

保存关键帧和进行优化功能。

运行流程如下：

程序开始：      
saveKeyFramesAndFactor(){      
1.把上次优化得到的transformAftMapped(3:5)坐标点作为当前的位置，      
计算和再之前的位置的欧拉距离，距离太小并且cloudKeyPoses3D不为空(初始化时为空)，则结束；      
2.如果是刚刚初始化，cloudKeyPoses3D为空，      
那么NonlinearFactorGraph增加一个PriorFactor因子，      
initialEstimate的数据类型是Values（其实就是一个map），这里在0对应的值下面保存一个Pose3，      
本次的transformTobeMapped参数保存到transformLast中去。      
3.如果本次不是刚刚初始化，从transformLast得到上一次位姿，      
从transformAftMapped得到本次位姿，      
gtSAMgraph.add(BetweenFactor),到它的约束中去，      
initialEstimate.insert(序号，位姿)。      
4.不管是否是初始化，都进行优化，isam->update(gtSAMgraph, initialEstimate);      
得到优化的结果：latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size()-1),      
将结果保存，cloudKeyPoses3D->push_back(thisPose3D);      
cloudKeyPoses6D->push_back(thisPose6D);      
5.对transformAftMapped进行更新;      
6.最后保存最终的结果：      
cornerCloudKeyFrames.push_back(thisCornerKeyFrame);      
surfCloudKeyFrames.push_back(thisSurfKeyFrame);      
outlierCloudKeyFrames.push_back(thisOutlierKeyFrame);      
}      
程序结束

2.10、correctPoses

矫正位姿只在回环结束时进行(aLoopIsClosed == true)才会进行这一步，将isam优化后的位姿替换之前的位姿。校正位姿的过程主要是将isamCurrentEstimate的x，y，z平移坐标更新到cloudKeyPoses3D中，另外还需要更新cloudKeyPoses6D的姿态角。

2.11、cornerOptimization

基本都是数学公式转化成代码。

该函数分成了几个部分：

进行坐标变换,转换到全局坐标中去；
进行5邻域搜索，得到结果后对搜索得到的5点求平均值；
求矩阵matA1=[ax,ay,az]t*[ax,ay,az]，例如ax代表的是x-cx,表示均值与每个实际值的差值，求取5个之后再次取平均，得到matA1；
求正交阵的特征值和特征向量，特征值：matD1，特征向量：保存在矩阵matV1中

因为求取的特征值是按照降序排列的，所以根据论文里面提到的：

如果这是一个边缘特征，则它的一个特征值远大于其余两个；

如果这是一个平面特征，那么其中一个特征值远小于其余两个特征值；根据上面两个原则进行判断要不要进行优化。如果没有满足条件1，就不进行优化过程，因为这不是一个边缘特征。

5.如果进行优化，进行优化的过程是这样的：先定义3组变量

接着求面积的绝对值，最后再求一次叉乘，得到底边高上的单位向量

2.12、surfOptimization

函数进行面优化，内容和函数cornerOptimization(int)的内容基本相同。步骤如下：

1.进行坐标变换,转换到全局坐标中去；

2.进行5邻域搜索，得到结果后判断搜索结果是否满足条件(pointSearchSqDis[4] < 1.0)，不满足条件就不需要进行优化；

3.将搜索结果全部保存到matA0中，形成一个5x3的矩阵；

4.解这个矩阵cv::solve(matA0, matB0, matX0, cv::DECOMP_QR);。关于cv::solve函数，参考官网。 matB0是一个5x1的矩阵，需要求解的matX0是3x1的矩阵；

matA_0*matX_0=matB_0$ ，最后求得 $matX_0$。这个公式其实是在求由matA0中的点构成的平面的法向量matX0`。

5.求解得到的matX0=[pa,pb,pc,pd]，对[pa,pb,pc,pd]进行单位化， matB0=[-1,-1,-1,-1,-1]^t，关于matB0为什么全是-1而不是0的问题：因为pd=1，所以在求解的时候设置了matB0全为-1。这里再次判断求解的方向向量和每个点相乘，最后结果是不是在误差范围内。如果误差太大就不把当前点pointSel放到点云中去了。

6.误差在允许的范围内的话把这个点放到点云laserCloudOri中去，把对应的向量coeff放到coeffSel中。

2.13、LMOptimization

该函数是代码中最重要的一个函数，实现的功能是高斯牛顿优化(虽然写了是LMOptimization，但其实是用的高斯牛顿的方法)。首先是对laserCloudOri中数据的处理，将它放到matA中，matA就是误差对旋转和平移变量的雅克比矩阵。

求完matA之后，再计算matAtA，matAtB，matX，方便后面的计算

根据opencv文档，cv::transpose(matA,matAt)将矩阵由matA转置生成matAt。

初次优化时，特征值门限设置为100，小于这个值认为是退化了，修改matX，matX=matP*matX2

最后将matX作为6个量复制到transformTobeMapped中去。在判断是否是有效的优化时，要求旋转部分的模长小于0.05m，平移部分的模长也小于0.05度。

7、总结

8、参考链接

《Fast Range Image-Based Segmentation of Sparse 3D Laser Scans for Online Operation》 2016 IROS
《Efficient Online Segmentation for Sparse 3D Laser Scans》 2017

LeGO-LOAM和LOAM的区别与联系 - 知乎

激光里程计系列（二）：LeGO-LOAM - 知乎

LeGO-LOAM论文翻译（内容精简)_wykxwyc的博客-CSDN博客_loam论文翻译

LeGO-LOAM中的数学公式推导_wykxwyc的博客-CSDN博客_lego loam 数学推导

LeGO-LOAM 源码阅读笔记(imageProjecion.cpp)_wykxwyc的博客-CSDN博客

LeGO-LOAM 源码阅读笔记(featureAssociation.cpp)_wykxwyc的博客-CSDN博客_featureassociation

LeGO-LOAM 源码阅读笔记（mapOptmization.cpp）_wykxwyc的博客-CSDN博客

LeGO-LOAM分析之点云分割（一） - 知乎

LeGO-LOAM分析之特征提取（二） - 知乎

LeGO-LOAM分析之建图（三） - 知乎

无人驾驶算法学习（九）：LeGo-LOAM激光雷达定位算法_su扬帆启航的博客-CSDN博客_lego loam定位