作者丨yc zhang@知乎

来源丨https://zhuanlan.zhihu.com/p/111388877

编辑丨3D视觉工坊

LOAM作为比较古老的激光匹配slam方法，一直以来都霸占着KITTI的前列，近些年来，依靠LOAM框架也产出了很多文章，理解了LOAM，就可以很好的理解LOAM系列的其他文章。
因此，我决定将重新整理一下LOAM的论文和代码，方便初入门的同学更好的理解LOAM算法。
论文：https://www.ri.cmu.edu/pub_files/2014/7/Ji_LidarMapping_RSS2014_v8.pdf

代码：

https://github.com/HKUST-Aerial-Robotics/A-LOAM
文章较长，书写不易，如果觉得对您有帮助的话，希望可以点赞收藏支持一下哈~
1、论文概览

LOAM这篇论文是发表于2014年RSS的文章，全称为：LOAM: Lidar Odometry and Mapping in Real-time . LOAM是基于激光雷达而搭建的在ROS平台下的SLAM系统，一共分为四个部分：

本文的核心主要在于两个部分：特征提取（Lidar Registration）和里程计解算（Odometry and Mapping）。
当提取出特征后，通过两个高频率的里程计（Odometry）实现粗定位和低频率的里程计（Mapping）实现精定位。下面，我们将结合论文和代码，进行LOAM框架的详细讲解。
2、符号设定

一个sweep代表了一次扫描周期，记作 

一个扫描周期内获取到的所有点云记作  代表了第k个扫描周期的点云

雷达坐标系设定为  代表了第k个周期时的雷达观测坐标系，  在  中可以被表示为 

全局坐标系设定为  代表了第k个周期时的全局坐标系，  在  中可以被表示为 

因此，我们就可以将整个问题转化为：
已知一段点云序列  ，计算在前  个时期内的雷达位姿以及构建全局地图。
3、Lidar Registration
为了计算雷达的运动位姿，我们需要得到的是相邻帧间的姿态变换关系，这样才能继续往后走下去。为了获取到相邻帧的姿态变换，使用全部点云处理是不可靠的，为了减少计算的时间消耗，一般需要使用特征点来代替完整的数据帧。
常见的特征点提取方法：特征向量、直方图、旋转图片等。
这些方法虽然能很精准的涵盖一帧数据的大部分信息，但是由于计算量大，很难在激光slam的相邻帧的匹配中使用。因此，需要想一些更好的方法。
本文作者根据点的曲率来计算平面光滑度作为提取当前帧的特征信息的指标。

这种提取方法就是通过计算一个集合  中的所有点之间的关系来判断到底点  是属于平面点还是边缘点。具体到代码的实现就是使用计算当前点和前后五个点，来得到对应的平滑度数据,计算量减少了很多。

再得到平滑度这一指标后，可以将特征点划分为两大类：平面点和边缘点。

平面点：在三维空间中处于平滑平面上的点，其和周围点的大小差距不大，曲率较低，平滑度较低。

边缘点：在三维空间中处于尖锐边缘上的点，其和周围点的大小差距较大，曲率较高，平滑度较高。

我们对集合内的点进行排序，找到最小的点c作为平面点，最大的点c作为边缘点。
这样就可以在一帧中得到有效的点数了。而在论文中是对整个扫描进行化段，分四段，每段各取两个边缘点和4个平面点。而在A-LOAM的代码中则是进行了一下的实现：

同时，对所取的点也有一定的限制：

该点的周围的点尽量不要被再被取到，这样可以使整体的特征点分布更加的平均

该点不能与雷达扫描束过于平行，这样也会出现问题

具体样例如下图所示：

因此，选点就有了三要素：

1、不能超过设定的size，每个集合平面点4个，边缘点2个；

2、已选取的点周围不能有点，使得点可以分布的更加均匀；

3、选取的平面点不能与激光扫描束平行。

以上的对应关系，同样再代码中有着体现。
判断该点是否是之前选取的点的周围的点以及不能超过size：

通过上面的循环，就实现了边缘点（Corner）和平面点（Planar）的获取了。这样就可以得到一帧数据对应的特征信息了。
提取后的数据如图所示：

这样就可以在整个三维空间内，将平面点和边缘点作为特征点提取出来来代替整个数据了。
4、Lidar Odometry
再提取了特征点之后，我们需要做的就是特征匹配了。这里使用的使scan-to-scan的方法来实现帧与帧之间的特征匹配。
已知第  次扫描的点云为  ，而提取的边缘点集合记为：  ，提取的平面点记为  。
已知第  次扫描的点云为  ，而提取的边缘点集合记为：  ，提取的平面点记为  。
我们想要得到的是  和  之间的变换关系，也就是  和  以及  和  之间的关系。
由于雷达自身在  和 时刻过程中是运动的，所以，我们每个点对应的姿态变换矩阵都应该得到一定的修正。为了方便处理，我们将所有的点重投影到每一帧的初始时刻，这样在这一帧中的所有点都可以得到对应的姿态变换信息。
我们将重投影到每一帧初始时刻的平面点和边缘点记为:  和  。
这样的话就可以进行后续的优化了。
我们知道平面和边缘线是两种不同的特征，那么在LOAM的计算中，也是分开进行计算的。
4.1、边缘点匹配
已知信息：  和  。
我们知道，边缘点就是三维结构中线所构成的点，这样的话，就是求点到线的最近距离。需要在  中找到一条线来求解最近距离：

雷达扫描束

三个点在两帧之间的直观展示

构建的几何约束关系

从  中选取一个点  ，在  中选取与  最近的点  ，以及在  中选取和点  相邻扫描线中最近的点  ，这样的目的是防止  三点共线而无法构成三角形。
选取最近点的算法使用的是kd-tree的最近邻搜索，如果对kd-tree感兴趣的同学，可以阅读这篇文章：

https://zhuanlan.zhihu.com/p/112246942

因此，选取了三个点: {  }，坐标分别记为：  ，  和  。
这样，就将姿态变换转换为了，点  到线  的最短距离了。
因此，就变为了：

边缘点约束公式

我们知道，分子叉乘球出来的是  和  构成的平行四边形的面积，而分母则是  构成的底，这样的话，就可以通过基础的数学知识，得到高  。
因此，我们就构建了边缘点的优化方程。
4.2、平面点匹配
已知信息  和  。
平面点的匹配起始和边缘点的匹配类似，同样的是寻找两帧之间的对应关系，我们知道平面点的话，就是要求点到平面的距离，这样的话，就需要在  中找到一个对应的平面。

雷达扫描束

四个点在两帧之间的直观展示

构建的几何约束关系

从  中寻找一个点  ，从  中找寻与点  最近的点  ，并找到与点  相同激光扫描束的最近点  ，然后，找寻相邻帧中与点  最相近的点  ，这样的话，就可以找到一个不共线的，能构成一个平面的三个点。
因此，选取了四个点：{  }，坐标分别记为：  和  。
这样的话，就变成了点  到平面  之间的最近距离了。
因此，就变为了：

平面点约束公式

故，分子为构成的三维物体的体积，分母为地面构成的平行四边形的面积，则高  。
因此，我们就得到了平面点的优化方程。
4.3、姿态解算
当获取到了  和  之后，我们需要做的就是求解公式(2)和(3)中的右边的部分，最小化右边部分，就得到了最小化的 和 ，这样就可以使用非线性优化的方法来进行求解 了。
我们首先列出已知的信息：

由于考虑了雷达的自身运动是匀速运动，所以，我们获取了每个点的时间戳信息，并使用线性方程，得到每个时刻对应的姿态变换矩阵，这一步主要是进行了运动补偿。
我们首先求解第  帧中第个点的姿态变换信息：

这样的话，就可以使用每个点对应的姿态变换矩阵放入进去进行后续的求解了。
但是，这里论文使用的是6-DoF的表示，也就是  。
所以，我们需要想办法从6-DoF得到对应的姿态变换矩阵，进行迭代优化求解。
我们设定旋转矩阵  和平移矩阵  。
这样的话，就可以构建公式：

这样，就可以将  帧上的点投到k帧所在的集合中了。
这里可能不是很好理解。
这里就涉及到论文中所涉及到的几个符号的表示，建议可以对照论文当中来看。

核心思想就是要找到一个姿态变换使得可以将公式(2)和(3)中的右边部分进行化简计算，而我们知道{  }，所以，需要将  转化到同样的帧中，统一坐标系，进行后面的解算。

回归正文，我们获得了得到了公式之后，就需要求解  和  了。
然而，我们使用的是6-DoF的表示，就需要想办法进行欧拉角到旋转矩阵的变换，使用罗德里格斯公式进行变换：

欧拉角转旋转矩阵

PS:这里使用的是旋转向量来表示，而不是欧拉角。但是在原始的loam-velodyne版本中，我们使用的是欧拉角来进行后续的计算。而在后面的A-LOAM版本使用的是四元数来求解。也算是避开了原版的一个坑吧。
这样，就可以将欧拉角转换为旋转矩阵了，其中  。
而，平移矩阵就是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
当我们获取了若干相邻帧的姿态变换信息后，我们需要做的就是将其和全局地图进行匹配，并将其加入到全局地图之中。

mapping匹配示意图

这里设定：

第  帧之前的扫描点云在全局坐标系下的投影为  ；

第  次扫描的末位，也就是  帧的起始时的姿态变换信息  。

利用Odometry的输出  ，将  从  时刻的起始推演到  时刻的起始，得到姿态矩阵  。

通过  ，将之前第  帧的点云投影到全局坐标系下，记为  。

这样，其实就很明晰了，我们需要做的就是优化求解  。
这里的已知信息为：{  }，不精准的  。想要优化得到精准的  。
同样是两个点云，求精准的姿态变换，这里和之前的Lidar Odometry部分很接近，所以，使用的算法基本相同。
不过，我们这里是map-to-map的算法，所以，使用的  是10帧Odometry输出的数据，  是之前的地图数据。
如果使用全部的地图数据，在计算效率上会大打折扣，所以，这里使用的是一个边长为10m的立方体，用以代替全局地图，优化得到最终的姿态变换矩阵  。
选取特征点的方法是一样的，不过具体实现的方式不尽相同。
将  和  中相交的存入到KD-tree中，这里的相交部分，也就是判定是否处于这个cube中。相交的部分属于了两个map之间的重合部分，可以用来作为点云匹配的依据。
在这里，首先选取相邻点集合  ，针对平面点和边缘点又有两种处理方法：

1、平面点：  只保留平面特征点；

2、边缘点：  只保留边缘特征点。

计算  的协方差矩阵，记为  ，  的特征值记为  ，特征向量记为  。
如果  分布在一条线段上，那么  中一个特征值就会明显比其他两个大，  中与较大特征值相对应的特征向量代表边缘线的方向。（一大两小，大方向）
如果  分布在一块平面上，那么  中一个特征值就会明显比其他两个小， 中与较小特征值相对应的特征向量代表平面片的方向。（一小两大，小方向）
边缘线或平面块的位置通过穿过  的几何中心来确定。
经过和评论区的同学讨论，发现这个地方没有描述的很清楚，这里可以看一个动图（感谢CSDN博主robinvista同意我使用他的图片）：

特征向量表示整个平面

可以清楚地看到，特征向量的长度反应了点的分布，也就是说我们根据特征值和特征向量就能计算出直线的方向。平面也是同理，我们可以根据两个较长的特征向量计算平面的法向量，三个向量相交于几何中心，这样平面就确定了。
通过这种方法就可以快速的确定对应的边缘线和平面了。
这样就可以快速的找到  中的一个点  ，和  中的边缘点{  }以及平面点{  }。
这样就可以使用公式(2)和(3)，利用LM法来求解  了。
这里需要注意的是除了由于实时性的缘故，找对应的特征点更换了方法，Lidar Mapping其他的算法步骤和Lidar Odometry 的基本一致。
这里需要注意的是，Lidar Odometry中使用过运动补偿了，这里的点云就都被设置为对应帧的时间戳，就不用再考虑运动补偿的事情了。
后续可以通过VoxelFilter来进行降噪，减少点云的数量。
整个的计算流程可以表示如下：

Lidar Mapping算法更新流程

通过不断的处理，就可以得到对应的了。
对应的代码解析如下：
对cube里的数据进行处理：

接受Lidar Odometry的数据，并得到初始的姿态变换矩阵：

经过计算后，得到的新的姿态信息：

在这里，需要注意的是，我们最终传递的并不是q_wmap_odom和t_wmapodom。最终传递的是优化计算后得到的t_w_curr和q_w_curr：

Lidar Mapping节点publish的message

6、Transform Integration
这一部主要是将Lidar Odometry中得到的姿态信息和Lidar Mapping中得到的信息全部都放入到rviz中，方便观看和处理。如果是为了使用LOAM作为前端的话，到Lidar Mapping就完全够用了。
7、总结：
LOAM作为常见霸占KITTI榜的激光SLAM算法，是有着自己的独特优势的。其优缺点如下：
优点：

新颖的特征提取方式（边缘点和平面点）

运动补偿（时间戳）

融合了scan-to-scan（odometry）和map-to-map（mapping）的思想

缺点：

没有后端优化（年代）

不能处理大规模的旋转变换（旋转向量的求解）

LOAM的整理到这里就结束了。LOAM由于发表时间较早，并没有进行后端优化。不过，他的后继者：LeGO-LOAM就很好的解决了这个问题。

https://zhuanlan.zhihu.com/p/115986186

在LeGO-LOAM这篇文章中，我详细的阐述了LOAM和LeGO-LOAM的区别与联系，大家可以根据自己的实际需求选择对应的算法来实现想要的功能。

一路看下来，想必也不容易，如果觉得本文对您有帮助的话，希望可以点赞收藏关注支持一下哈~

本文仅做学术分享，如有侵权，请联系删文。

下载1

在「3D视觉工坊」公众号后台回复：3D视觉，即可下载 3D视觉相关资料干货，涉及相机标定、三维重建、立体视觉、SLAM、深度学习、点云后处理、多视图几何等方向。

下载2

在「3D视觉工坊」公众号后台回复：3D视觉github资源汇总，即可下载包括结构光、标定源码、缺陷检测源码、深度估计与深度补全源码、点云处理相关源码、立体匹配源码、单目、双目3D检测、基于点云的3D检测、6D姿态估计源码汇总等。

下载3

在「3D视觉工坊」公众号后台回复：相机标定，即可下载独家相机标定学习课件与视频网址；后台回复：立体匹配，即可下载独家立体匹配学习课件与视频网址。

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、orb-slam3等视频课程）、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近2000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

圈里有高质量教程资料、可答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~