日期：
来源：新机器视觉收集编辑：
d点击下方卡片，关注“新机器视觉”公众号
重磅干货，第一时间送达
因此，我决定将重新整理一下LOAM的论文和代码，方便初入门的同学更好的理解LOAM算法。
论文：https://www.ri.cmu.edu/pub_files/2014/7/Ji_LidarMapping_RSS2014_v8.pdf
文章较长，书写不易，如果觉得对您有帮助的话，希望可以点赞收藏支持一下哈~
1、论文概览
当提取出特征后，通过两个高频率的里程计（Odometry）实现粗定位和低频率的里程计（Mapping）实现精定位。下面，我们将结合论文和代码，进行LOAM框架的详细讲解。
2、符号设定
已知一段点云序列 ，计算在前 个时期内的雷达位姿以及构建全局地图。
3、Lidar Registration
为了计算雷达的运动位姿，我们需要得到的是相邻帧间的姿态变换关系，这样才能继续往后走下去。为了获取到相邻帧的姿态变换，使用全部点云处理是不可靠的，为了减少计算的时间消耗，一般需要使用特征点来代替完整的数据帧。
常见的特征点提取方法：特征向量、直方图、旋转图片等。
这些方法虽然能很精准的涵盖一帧数据的大部分信息，但是由于计算量大，很难在激光slam的相邻帧的匹配中使用。因此，需要想一些更好的方法。
本文作者根据点的曲率来计算平面光滑度作为提取当前帧的特征信息的指标。
这样就可以在一帧中得到有效的点数了。而在论文中是对整个扫描进行化段，分四段，每段各取两个边缘点和4个平面点。而在A-LOAM的代码中则是进行了一下的实现：
判断该点是否是之前选取的点的周围的点以及不能超过size：
提取后的数据如图所示：
4、Lidar Odometry
再提取了特征点之后，我们需要做的就是特征匹配了。这里使用的使scan-to-scan的方法来实现帧与帧之间的特征匹配。
已知第 次扫描的点云为 ，而提取的边缘点集合记为： ，提取的平面点记为 。
已知第 次扫描的点云为 ，而提取的边缘点集合记为： ，提取的平面点记为 。
我们想要得到的是 和 之间的变换关系，也就是 和 以及 和 之间的关系。
由于雷达自身在 和 时刻过程中是运动的，所以，我们每个点对应的姿态变换矩阵都应该得到一定的修正。为了方便处理，我们将所有的点重投影到每一帧的初始时刻，这样在这一帧中的所有点都可以得到对应的姿态变换信息。
我们将重投影到每一帧初始时刻的平面点和边缘点记为: 和 。
这样的话就可以进行后续的优化了。
我们知道平面和边缘线是两种不同的特征，那么在LOAM的计算中，也是分开进行计算的。
4.1、边缘点匹配
已知信息： 和 。
我们知道，边缘点就是三维结构中线所构成的点，这样的话，就是求点到线的最近距离。需要在 中找到一条线来求解最近距离：
选取最近点的算法使用的是kd-tree的最近邻搜索，如果对kd-tree感兴趣的同学，可以阅读这篇文章：
这样，就将姿态变换转换为了，点 到线 的最短距离了。
因此，就变为了：
因此，我们就构建了边缘点的优化方程。
4.2、平面点匹配
已知信息 和 。
平面点的匹配起始和边缘点的匹配类似，同样的是寻找两帧之间的对应关系，我们知道平面点的话，就是要求点到平面的距离，这样的话，就需要在 中找到一个对应的平面。
因此，选取了四个点：{ }，坐标分别记为： 和 。
这样的话，就变成了点 到平面 之间的最近距离了。
因此，就变为了：
因此，我们就得到了平面点的优化方程。
4.3、姿态解算
当获取到了 和 之后，我们需要做的就是求解公式(2)和(3)中的右边的部分，最小化右边部分，就得到了最小化的 和 ，这样就可以使用非线性优化的方法来进行求解 了。
我们首先列出已知的信息：
由于考虑了雷达的自身运动是匀速运动，所以，我们获取了每个点的时间戳信息，并使用线性方程，得到每个时刻对应的姿态变换矩阵，这一步主要是进行了运动补偿。
我们首先求解第 帧中第个点的姿态变换信息：
这样的话，就可以使用每个点对应的姿态变换矩阵放入进去进行后续的求解了。
但是，这里论文使用的是6-DoF的表示，也就是 。
所以，我们需要想办法从6-DoF得到对应的姿态变换矩阵，进行迭代优化求解。
我们设定旋转矩阵 和平移矩阵 。
这样的话，就可以构建公式：
这样，就可以将 帧上的点投到k帧所在的集合中了。
这里可能不是很好理解。
这里就涉及到论文中所涉及到的几个符号的表示，建议可以对照论文当中来看。
然而，我们使用的是6-DoF的表示，就需要想办法进行欧拉角到旋转矩阵的变换，使用罗德里格斯公式进行变换：
这样，就可以将欧拉角转换为旋转矩阵了，其中 。
而，平移矩阵就是6-DoF的前三位，也就是说 。
这样，就可以实现6-DoF和旋转矩阵的变动了。
那么我们可以知道在公式(2)和公式(3)中，只有 和 是未知的，其他都是已知的。那么就可以得到一个约束公式：
因此，可以统一为一个公式：
因此，我们就需要求解优化这个非线性优化问题就可以了。
那么使用常规的列文伯格-马夸特法(LM)来进行求解:
如果对最小二乘的求法感兴趣的话，我在这里总结了一些SLAM中最小二乘的求解方法：https://zhuanlan.zhihu.com/p/113946848
构建拉格朗日函数， 是系数因子：
这样的话，化简后求导就可以得到：
我们化简后得到：
而
故，我们可以得到导数为：
我们可以看到，和论文中略有不同，这个和初始雅可比矩阵 以及系数矩阵 有关，实际使用中，系数矩阵D，通常是用 来表示的，这样的话，我们就得到了其微分量 。
代入梯度下降的公式为：
和论文中基本一致，就是表现的方式有一点点的区别。
不断求解上面这个式子，直到收敛即可。
代码使用的是ceres solver来进行求解。
首先设定损失函数：
A-LOAM的代码相比于LOAM-Velodyne是使用了现有的优化求解库，这样可以使代码更加简介和简单，只需要输入边缘点对应关系的{ }和平面点的{ }，这样的话，就可以优化求解了。
然而，需要注意的是：
这里求解的还是局部雷达观测坐标系下的结果，是为了求解相邻帧之间的变换，也就是 ，而为了定位和建图，需要求解的是全局坐标系下的变换，也就是 。因此，需要进入到下一个章节Lidar Mapping中。
5、Lidar Mapping
当我们获取了若干相邻帧的姿态变换信息后，我们需要做的就是将其和全局地图进行匹配，并将其加入到全局地图之中。
这里的已知信息为：{ }，不精准的 。想要优化得到精准的 。
同样是两个点云，求精准的姿态变换，这里和之前的Lidar Odometry部分很接近，所以，使用的算法基本相同。
不过，我们这里是map-to-map的算法，所以，使用的 是10帧Odometry输出的数据， 是之前的地图数据。
如果使用全部的地图数据，在计算效率上会大打折扣，所以，这里使用的是一个边长为10m的立方体，用以代替全局地图，优化得到最终的姿态变换矩阵 。
选取特征点的方法是一样的，不过具体实现的方式不尽相同。
将 和 中相交的存入到KD-tree中，这里的相交部分，也就是判定是否处于这个cube中。相交的部分属于了两个map之间的重合部分，可以用来作为点云匹配的依据。
在这里，首先选取相邻点集合 ，针对平面点和边缘点又有两种处理方法：
如果 分布在一条线段上，那么 中一个特征值就会明显比其他两个大， 中与较大特征值相对应的特征向量代表边缘线的方向。（一大两小，大方向）
如果 分布在一块平面上，那么 中一个特征值就会明显比其他两个小， 中与较小特征值相对应的特征向量代表平面片的方向。（一小两大，小方向）
边缘线或平面块的位置通过穿过 的几何中心来确定。
经过和评论区的同学讨论，发现这个地方没有描述的很清楚，这里可以看一个动图（感谢CSDN博主robinvista同意我使用他的图片）：
通过这种方法就可以快速的确定对应的边缘线和平面了。
这样就可以快速的找到 中的一个点 ，和 中的边缘点{ }以及平面点{ }。
这样就可以使用公式(2)和(3)，利用LM法来求解 了。
这里需要注意的是除了由于实时性的缘故，找对应的特征点更换了方法，Lidar Mapping其他的算法步骤和Lidar Odometry 的基本一致。
这里需要注意的是，Lidar Odometry中使用过运动补偿了，这里的点云就都被设置为对应帧的时间戳，就不用再考虑运动补偿的事情了。
后续可以通过VoxelFilter来进行降噪，减少点云的数量。
整个的计算流程可以表示如下：
对应的代码解析如下：
对cube里的数据进行处理：
这一部主要是将Lidar Odometry中得到的姿态信息和Lidar Mapping中得到的信息全部都放入到rviz中，方便观看和处理。如果是为了使用LOAM作为前端的话，到Lidar Mapping就完全够用了。
7、总结：
LOAM作为常见霸占KITTI榜的激光SLAM算法，是有着自己的独特优势的。其优缺点如下：
优点：
https://zhuanlan.zhihu.com/p/115986186