LOAM是Ji Zhang提出的使用激光雷达进行实时建图的算法，即Lidar Odometry and Mapping in Real-time。

Abstract

提出了一个用移动在6自由度的两轴激光雷达实现实时的里程计和建图的算法。由于距离测量值的不同时性和运动估计时产生的畸变导致点云错误匹配。这里作者提出一个很好的想法，即将同时定位和建图一分为二，这样就可以在不需要高精度测距或惯性测量的情况下实现两低——低漂移和低计算复杂度。核心由两个算法构成：一个算法是执行高频率（10Hz）但低精度的里程计去进行雷达的运动估计；另一个算法以比第一个低一个数量级的频率（1Hz）进行点云的匹配和信息注册（即建图和校正里程计）。LOAM算法就是把2这两个算法结合。

一、INTRODUCTION

3D激光雷达优点：测距频率高，准确；无论测量距离如何，误差都相对恒定。

问题：我们使用激光雷达建图，一般激光雷达相对于环境都会移动，我们在建图过程中就需要获取激光雷达的位姿。通常使用 GPS/INS 或者车轮编码器/视觉里程计系统，但是不可避免会出现累计误差和漂移。

低漂移的两轴激光雷达的优点：对光线和物体纹理不敏感；体积和成本不断下降。故可以手持或者固定在小型移动设备上即可。

不涉及回环检测：因为此方法的目的是最小化里程计漂移。

具体如何实现低漂移和低计算量？

作者提出了提取边缘点和平面点作为特征点，去和边缘线段和平面块进行匹配。这种方法只需要计算一个点前后的5个点就得到该点的曲率，大大减少了计算量。

作者采用高低频率结合的方法，执行高频率（10Hz）但低精度的里程计去进行雷达的运动估计（odometry算法），同时进行低频率（1Hz）的点云匹配（mapping算法）。因为采用了高频率去进行运动估计，故建图的过程就有足够的时间去保证准确性。以较低频率运行时，建图算法可以合并大量特征点并进行足够次数的迭代以达到收敛。

二、RELATED WORK

如果激光雷达的转速相对于其本身的运动来说很高，那么运动造成的运动畸变可以被忽略。这种情况下，标准的ICP方法就可以用来进行两帧间的点云匹配。

但对于两轴激光雷达，因为它的其中一个轴的速度相对比较慢，即激光雷达的旋转速度相对较慢时会产生严重的运动畸变。通常用其他传感器获得速度信息去去除运动畸变。如使用带有IMU的视觉里程计或在可以用多传感器融合时使用卡尔曼滤波or粒子滤波器去除运动畸变。

出现问题：如果两轴激光雷达没有其他传感器辅助，那么运动估计和运动畸变的校正is a hard problem。列举了一些其他人的方法。

三、NOTATIONS AND TASK DESCRIPTION

假设激光雷达是预先标定好的；雷达的角速度和线速度光滑并连续（靠IMU实现）

定义了两个坐标系：

1.雷达坐标系：{L}，x轴指向左边，y轴指向上，z轴指向前2，即右手系。

2.世界坐标系：世界坐标在初始时刻与雷达坐标系相同。

sweep:对于多线lidar，所有线束旋转一周称之为sweep。

scan：其中某一线束旋转一周则称之为scan。

四、SYSTEM OVERVIEW

1.硬件：激光扫描仪的视场为180°，分辨率为0.25°，扫描速度为40线/秒。激光扫描仪连接到电机，该电机被控制为在−90°和90°之间以180°/ s的角速度旋转。连续旋转激光雷达，扫描只是一个半球形的旋转。

2.软件系统：

如图中的结构框架，此系统主要由四部分构成：Point Cloud Registration;Lidar Odometry;Lidar Mapping;Transform Integration。

$\hat{P}$ ：激光雷达扫描得到的点集； $P_{k}$ :第k次扫描得到的点集。

先计算连续两次扫描之间激光雷达的运动，并将估计得到的运动去修正 $P_{k}$ 中的畸变点（执行频率10Hz）；然后把上一步中去畸变的点云给了Lidar Mapping节点进行地图匹配（执行频率1Hz）；最后由Transform Integration节点接收前面两个节点输出的transform信息，进行融合处理生成一个10Hz的transform信息？。

五、激光雷达里程计

1.特征点提取：

因为点云会出现非均匀性，采取从一次扫描中获得的点。选取边缘点和平面点作为特征点。

方法：提取点i周围的几个连续的点集S，用来求该点曲率，为了避免雷达顺时针和逆时针的影响，S中一半的点要在点i的一侧并且每两个点间隔0.25°。曲率公式如下：

每个点取前后相邻 5个点进⾏曲率计算，并将点及其曲率进⾏排序。我们把曲率大于阈值的点叫做边缘点，曲率小于阈值的叫做平面点。为了防止特征点聚集不均匀，我们把一次扫描分为4个部分，每个部分提供两个边缘点和四个平面点。

在提取特征点的时候需要避免提取一些不好的点，这些点通常被认为是不可靠的，如下图：

（1）所选边缘点或平面点的数量不能超过子区域的最大值；

（2）它周围的点都没有被选中；

（3）不能在大致平行于激光束的平面上，如上图（a）中的点A，也不能在遮挡区域的边界上，如上图（b）中的点A。

2.特征点匹配：

$t_{k}$ ：表示第k次扫描开始的时间；一次扫描结束时的时间为 $t_{k+1}$ ，并且点云 $P_{k}$ 进行重投影，把投影后的点云记为 $\bar{P_{k}}$ ，在第k+1次sweep的时候使用 $\bar{P_{k}}$ 和新一帧点云 $P_{k+1}$ 去估计雷达的运动。然后要寻找对应关系即匹配两帧点云的特征点。 $P_{k+1}$ 中的边缘点和 $\bar{P_{k}}$ 中的边缘线进行对应， $P_{k+1}$ 中的平面点和 $\bar{P_{k}}$ 中的平面线进行对应。 $\varepsilon _{k+1}$ 和 $H_{k+1}$ 分别代表 $P_{k+1}$ 中边缘点和平面点的集合，在每一次迭代中， $\varepsilon _{k+1}$ 和 $H_{k+1}$ 都会被投影到起始点的坐标系下，记为 $\bar{\varepsilon _{k+1}}$ 和 $\bar{H_{k+1}}$ ，用于和上一帧点云进行匹配。 $\bar{P_{k}}$ 中的曲率大和小2的点被保存在KD树里用于寻找对应点。

上图（a）表示寻找边缘点对应的边缘线的过程。设前⼀帧的基准点为i，在当前帧下，先寻找与点i处于同⼀条线上的最近点j，（i，j两点表示边缘线，）再寻找相邻雷达线上的最近点l （j和l来自不同的扫描），计算点i与直线(l，j )的距离，并以此构建约束⽅程。

分子是两个向量的叉乘，叉乘后求模就变成了求两个向量构成的三角形的面积。分母是向量的模，相当于三角形底边的长。即S/底边=高，即de。

上图（b）表示寻找平面点对应的平面的过程。在当前帧下，先寻找与点i处于同一条线上的最近点j和l。l是和j在同一scan中与i最近的点，而m表示在与j相邻的scan中与i最近的点，保证三个点不共线。计算点i与平面（j，l，m）的距离，并以此构建约束⽅程。

分子有两部分，上是一个向量，下是两个向量叉乘再取模即求三角形的面积，再乘以上面的高，即立方体的体积。分母表示立方体的底面积，相除得到de。

3.运动估计：

激光雷达运动在每次扫描过程中以恒定的角速度和线速度进行建模，这样可以进行线性插值。比如我们知道了一帧数据终止点相对于起始点的转换矩阵就可以对这一帧数据中的任意点相对于起始点的时间进行插值，求得每个点的位姿。差值公式：

在之前特征点匹配时， $\varepsilon _{k+1}$ 和 $H_{k+1}$ 分别代表 $P_{k+1}$ 中边缘点和平面点的集合， $\bar{\varepsilon _{k+1}}$ 和 $\bar{H_{k+1}}$ 是重投影到sweep开始时刻的点集。为了求解激光雷达运动，我们需要在 $\varepsilon _{k+1}$ 和 $\bar{\varepsilon _{k+1}}$ 之间或 $H_{k+1}$ 和 $\bar{\varepsilon _{k+1}}$ 之间建立几何关系。

$X_{\left ( k+1 i\right )}^{L}$ 是点 i在 $\varepsilon _{k+1}$ 或 $H_{k+1}$ 中的坐标， $\tilde{X_{\left ( k +1 i\right )}^{L}}$ 是 $\bar{\varepsilon _{k+1}}$ 和 $\bar{H_{k+1}}$ 中的对应点

$T_{\left ( k+1 i \right )}^{L}$ （a：b）是 $T_{\left ( k+1 i \right )}^{L}$ 的第a至第b个条目；R是由罗德里格斯公式定义的旋转矩阵：

其中 $\theta$ 为旋转角：

$\omega$ ：单位向量，方向为旋转方向。

$\hat{\omega }$ ： $\omega$ 的斜对称矩阵。

我们根据上面的式子得到 $\varepsilon _{k+1}$ 中边缘点和对应边缘线之间的几何关系：

同理我们可以得到在 $H_{k+1}$ 中的平面点和对应平面之间的几何关系：

$\lambda$

最后我们用LM方法求解雷达的运动。把 $\varepsilon _{k+1}$ 和 $H_{k+1}$ 中每个特征点的距离函数叠加，得到一个非线性函数：

f的每一行对应于一个特征点，d包含了其相应的距离。计算J，为f相对于 $T_{k+1}^{L}$ 的雅可比矩阵。

J =

然后再进行非线性迭代求解，把d进行最小化：（ $\lambda$ 是LM方法确定的重要因子）

4.雷达里程计算法：

输入：最新扫描的点云 $\bar{P_{k}}$ ，当前扫描的增长点云 $P_{k+1}$ ，最新递归的姿态变换 $T_{k+1}^{L}$

4-6行：当每次开启新扫描时，把 $T_{k+1}^{L}$ 的值置0；

7行：在 $P_{k+1}$ 中提取边缘点和平面点，分别构造 $\varepsilon _{k+1}$ 和 $H_{k+1}$ ；

9-19行：对于每个特征点，在 $\bar{P_{k}}$ 中找其对应关系。运动估计适用于鲁棒拟合。

第15行给每个特征点分配一个bisquare（双平方）权重，和其对应关系距离较大的特征点被赋予更小的权重，距离大于阈值的特征点被视为异常值并赋予权重为0。

第16行姿态变换更新一次非线性迭代，如果找到收敛或满足最大迭代次数，非线性优化终止。如果扫描结束，则把 $P_{k+1}$ 重新投影到时间戳 $t_{k+2}$ ，否则下一轮递归只返回变换 $T_{k+1}^{L}$ 。

六、建图

建图算法的运行频率比里程计算法低，每次sweep只调用一次。

如上图 $Q_{k}$ 定义为累积到第k次sweep的地图点云， $T_{k}^{W}$ 是第k次sweep结束时雷达相对于地图的位姿（蓝线），算法把 $T_{k}^{W}$ 在【 $t_{k+1}$ ， $t_{k+2}$ 】拓展，获得 $T_{k+1}^{W}$ ，并且把 $\bar{P_{k+1}}$ 投影到世界坐标{W}，表示为 $\bar{Q_{k+1}}$ ，然后把 $Q_{k}$ 和 $\bar{Q_{k+1}}$ 相匹配。

特征点的提取方法与之前章相同，但使用了10倍的特征点(即里程计10次输出的数据）。为了找到特征点的对应关系，我们将点云存储在地图 $Q_{k}$ 上，大小为10立方米。立方体中与 $\bar{Q_{k+1}}$ 相交的点被提取并存储在3D KD树中。我们在 $Q_{k}$ 中寻找特征点周围的特定区域中的点。设 S′为一组选取的周围点的集合。我们计算 S ′ 的协方差矩阵，表示为M，以及M的特征值V和特征向量E。如果 S ′分布在一条边缘线上，V包含一个明显大于其他两个的特征值(即一大两小)，E中与最大特征值相关的特征向量代表边缘线的方向。另一方面，如果 S ′ 分布在平面上，V包含两个较大的特征值，第三个明显较小（一小两大），而E中与最小特征值相关联的特征向量表示平面片的方向。边缘线或平面的位置通过穿过 S ′的几何中心来确定。
为了计算从特征点到其对应点的距离，我们在一条边缘线上选择两个点，在一个平面上选择三个点。然后根据之前的计算方式进行距离计算，构建非线性函数。与之前不同的是 $\bar{Q_{k+1}}$ 中的所有点都共享时间戳 t（k+2）（即不需要线性插值）。再次通过LM方法求解非线性问题，并将 $\bar{Q_{k+1}}$ 匹配到地图上，为了均匀分布这些点云，通过体素滤波器缩小为5cm的立方体。