LOAM 论文阅读

LOAM

一、介绍

LOAM: Lidar Odometry and Mapping in Real-time

2014, Robotics: Science and Systems

论文下载地址：http://www.roboticsproceedings.org/rss10/p07.pdf

LOAM的参考代码链接：
A-LOAM
A-LOAM-Notes
LOAM-notes

概述：该论文是Lidar 3D SLAM的经典之作，一直以来都霸占着 KITTI 的前列。作者另辟蹊径将复杂的 SLAM 问题分为：1. 高频的运动估计； 2. 低频的环境建图，巧妙地解决了实时性的难题。近些年来，依靠 LOAM 框架也产出了很多文章，理解了 LOAM，就可以很好的理解 LOAM 系列的其他文章。

J. Zhang and S. Singh, “Loam: Lidar odometry and mapping in realtime.” in Robotics: Science and Systems, vol. 2, no. 9, 2014.

二、总结

作者设计实现了两组算法。一组是：高频率低精度的里程计算法；另一组是低频率高精度的建图算法。最后将两个算法结合，进行实时建图。

里程计算法（Lidar Odometry Algorithm）

由于机器人的运动，激光里程计直接获得的数据与实际数据点位置存在差异，因此需要进行修正。原始点云数据经过激光里程计修正之后畸变效果明显降低。

特征点提取（Feature Point Extration）

特征点提取主要是提取边缘点和平面点。

通过计算平滑度（曲率）来判断，值大的为边缘点，值小的的平面点（注意这里设置了阈值）。

为了使特征点均匀分布，将每条 scan 均分成几个小区域，每个区域单独提取一定数量限制的特征点。同时选的点周围不能有点已经被选中。

此外还需要舍弃掉不可靠的平行点（所在平面和激光束平行）和遮挡点（某点与左右相邻两个点的距离过大）。

特征关联（Finding Feature Point Correspondence）

特征关联主要是为了找出前后两帧特征点的对应关系。对于边缘点，寻找边缘点和边缘点所在直线的关系，对于平面点，寻找平面点和平面的关系。

由于涉及到两帧的点，所以这里每一帧都要估计激光雷达的运动以进行时间戳（坐标系）的转换。让时间戳 $t_k$ 作为扫描帧 $k$ 的开始时间，到时间戳 $t_{k+1}$ 的过程中，会源源不断接收到存在偏差的原始点云数据 $P_{k}$ ，对这部分偏差进行校正（需要上一帧求出的转换矩阵做初值并迭代更新，投影到这一帧的起始时刻），得到的点云会投射到结束时刻 $t_{k+1}$ （需要上一帧求出的转换矩阵并迭代更新），这部分无偏差的点云数据称为 $\overline{P}_k$ 。算法实际上是从这个时刻开始的，也就是已知上一帧的变换矩阵和无偏点云数据 $\overline{P}_k$ ，对新接收的未校正的点云 $P_{k+1}$ 进行处理得到下一扫描帧的转换矩阵（做初值）。

设 $\mathcal{E}_{k+1}$ 和 $\mathcal{H}_{k+1}$ 分别为从 $\mathcal{P}_{k+1}$ 中提取出的边缘点集和平面点集。我们将从 $\overline{\mathcal{P}}_{k}$ 中找到边缘线（edge lines）作为 $\mathcal{E}_{k+1}$ 中的点的对应，找到平面片（planar patches）作为 $\mathcal{H}_{k+1}$ 中的点的对应。

在第 $k+1$ 次扫描过程中，由于我们只有 $k$ （帧开始时间/位置）和 $k+1$ （帧结束时间/位置）之间的转换矩阵，因此在每次迭代中，使用当前估计的变换矩阵将 $\mathcal{E}_{k+1}$ 和 $\mathcal{H}_{k+1}$ 重新投影到扫描的开始时刻（此时要进行匹配的两帧特征点点集在同一个位置），得到重投影点集 $\tilde{\mathcal{E}}_{k+1}$ 和 $\tilde{\mathcal{H}}_{k+1}$ 。对于 $\tilde{\mathcal{E}}_{k+1}$ 和$ \tilde{\mathcal{H}}_{k+1}$ 中的每个点，我们将在 $\overline{\mathcal{P}}_{k}$ 中找到最邻近点。这里我们将 $\overline{\mathcal{P}}_{k}$ 存储在 3D KD-tree 中，用于快速索引。

对于边缘点，找两个最近点形成一条直线（避免三点共线）。对于平面点，找三个最近点形成一个平面（避免三点共线退化成直线）。

找这些点最基本的出发点是想建立一个优化的方向。前面提到过，向前投影本身是粗略的，也就是存在偏差的，在没有偏差的情况下，向前投影的特征点应该是和 $\overline{P}_k$ 的特征点重合的，但因为偏差存在，特征点之间也是存在距离的，所以我们优化的方向应该是让这个距离尽可能小，为了增加算法的鲁棒性，这里没有选择特征点之间的距离最小，而是让点到特征区域的距离最小，这个距离对于边缘点就是点 $i$ 到直线 $jl$ 的距离，对于平面点就是点 $i$ 到平面 $jlm$ 的距离。

对于求距离的两个公式，只有将 $k+1$ 帧扫描中的点 $i$ 重新投影到 $k+1$ 帧开始时刻后的坐标 $\tilde{\boldsymbol{X}}_{(k+1, i)}^{L}$ 是不知道的，这里就涉及到前面提到的粗略的转换矩阵，这个矩阵其实就对应着第三部分也就是运动估计。

运动状态估计（Motion Estation）

对这篇论文而言，它假设雷达的运动是匀速的，所以变换矩阵理论上是可以根据时间关系进行线性插值的。

$$\boldsymbol{T}_{(k+1, i)}^{L} = \frac{t_i - t_{k + 1}}{t - t_{k + 1}} \boldsymbol{T}_{k+1}^{L} \quad\quad\quad\quad\quad (4)$$

其中， $t$ 为当前时间戳， $t_{k+1}$ 为第 $k+1$ 次扫描开始的时间戳， $t_{i}$ 为第 $k+1$ 次扫描过程中 $i$ 点的时间戳。因此分数的部分实际上就是扫描 $i$ 点时用的时间在整个 sweep 耗时中所占的比例。 $\boldsymbol{T}_{(k+1, i)}^{L}$ 表示的是 $t_{k+1}$ 时刻与 $t_{i}$ 时刻的坐标转换，右边 $\boldsymbol{T}_{k+1}^{L}$ 表示的是 $t_{k+1}$ 与当前 $t$ 时刻的坐标转换（也可以看成是不断更新的帧间变换矩阵），也就是我们最终的目标。

也就是说，我们可以利用粗略的帧间转换矩阵通过时间关系实现 $t_i$ 时刻与 $t_{k+1}$ 时刻的坐标转换，而这个坐标转换被用在前面的向前投影中，从而和距离挂上了钩。我们要优化的距离，最终变成优化一个最优的帧间变换矩阵，让特征点到达特征区域的距离最小。

地图构建算法（Lidar Mapping Algorithm）

前面的帧间优化的执行频率是高于这部分制图的执行频率的，也就是说大量的优化少量的制图，累计一定的帧数目的点云数据才会进行建图，虽然频率较低，但是精度很高。

本质上来说建图的目的就是将点云数据融入到世界地图里面去，精确估计点云数据在世界坐标系下面的位姿。
就过程而言，制图和前面的里程计实际上是很像的，区别在于制图多了一个坐标系的转换。

在第 $k+1$ 次扫描结束时，激光雷达里程计生成一个去畸变的点云 $\overline{\mathcal{P}}_{k+1}$ ，以及在雷达坐标系下将第 $k+1$ 帧映射到 $k+2$ 时刻的变换矩阵 $\boldsymbol{T}_{k+1}^{L}$ 。将 $\mathcal{Q}_{K}$ 定义为地图上累积到第 $k$ 次扫描的已校正点云数据，并将 $\boldsymbol{T}_{k}^{W}$ 定义为第 $k$ 次扫描结束 $t_{k+1}$ 时地图上激光雷达的姿态变换。

利用这三个量，我们可以首先将 $\boldsymbol{T}_{k}^{W}$ 通过 $\boldsymbol{T}_{k+1}^{L}$ 向后推算一个时刻，从 $t_{k+1}$ 扩展到 $t_{k+2}$ ，也就是得到从 $t_{k+1}$ 到 $t_{k+2}$ 的变换矩阵 $\boldsymbol{T}_{k+1}^{W}$ 。再利用这个转换关系，将 $ \overline{\mathcal{P}}_{k+1}$ 投影到世界坐标 $\{ W \}$，表示为 $\overline{\mathcal{Q}}_{k+1}$ （这里加横杠变成了前投影）。

这样问题转换为对 $\boldsymbol{T}_{k+1}^{W}$ 的优化，优化用到的工具是 $\mathcal{Q}_{K}$ 和 $\overline{\mathcal{Q}}_{k+1}$ 两个点云数据。问题和里程计很像，同样都是利用两个投影到同一个时刻的点云进行距离最优化，最后变成位姿的最优化，从而得到前后两帧的转换关系。转换关系得到后进行点云的匹配拼接，就可以扩大地图的范围。

因为针对的对象换成了制图，所以需要做一定的改变，一方面使用的 $\overline{\mathcal{Q}}_{k+1}$ 是 10 帧里程计输出的数据，而 $\mathcal{Q}_{K}$ 是之前的地图数据。也就是说 $\overline{\mathcal{Q}}_{k+1}$ 是攒了 10 次 sweep 的数据。对于 $\mathcal{Q}_{K}$ ，如果采用之前全部的地图来计算，必然会导致开销很大，所以这里变为使用边长为 10m 的立方体，用来代替全局地图进行 $\boldsymbol{T}_{k+1}^{W}$ 的计算。

由于实时性的缘故，找对应的特征点更换了方法，制图其他的算法步骤和里程计的基本一致：

将 $\mathcal{Q}_{K}$ 和 $\overline{\mathcal{Q}}_{k+1}$ 中相交的存入到 KD-tree 中，这里的相交部分，也就是判定是否处于这个 cube 中。相交的部分属于两个 map 之间的重合部分，可以用来作为点云匹配的依据。

在这里，首先选取相邻点集合 $S$ ，针对平面点和边缘点又有两种处理方法：

• 平面点：$S$ 只保留平面特征点；
• 边缘点：$S$ 只保留边缘特征点。

计算 $S$ 的协方差矩阵，记为 $M$ 。 $M$ 的特征值记为 $V$ ，特征向量记为 $E$ 。

如果 $S$ 分布在一条线段上，那么 $V$ 中一个特征值就会明显比其他两个大， $E$ 中与较大特征值相对应的特征向量代表边缘线的方向（一大两小，大为方向）。如果 $S$ 分布在一块平面上，那么 $V$ 中一个特征值就会明显比其他两个小， $E$ 中与较小特征值相对应的特征向量代表平面片的方向（一小两大，小为方向）。边缘线或平面块的位置通过穿过 $S$ 的几何中心来确定。

可以清楚地看到，特征向量的长度反应了点的分布，也就是说我们根据特征值和特征向量就能计算出直线的方向。平面也是同理，我们可以根据两个较长的特征向量计算平面的法向量，三个向量相交于几何中心，这样平面就确定了。

通过这种方法就可以快速的确定对应的边缘线和平面片了。

这样就可以快速地找到 $\overline{\mathcal{Q}}_{k+1}$ 中的一个点 $i$ ，和 $\mathcal{Q}_{K}$ 中的边缘点 $j, l$ 以及平面点 $j, l, m$ 。

这样就可以使用公式(2)和公式(3)，利用 $LM$ 法来求解 $\boldsymbol{T}_{k+1}^{W}$ 了。

LOAM的优缺点

优点：

• 将里程计和建图分隔开，一个高频低精（处理每次帧数据），一个低频高精（累积一定次数再处理）
• 整理框架是串行结构，将整个问题逐步划分为多个层次。
• 可实时建图的开源3D Lidar SLAM。

局限性：

• 点云特征处理可进一步改善：提取出更有效的特征点（去除干扰点），运动的人等物体
• 缺少回环检测
• 假设匀速运动模型

备注

LOAM 中提到可以采用IMU进行点云去畸变或旋转角度的获取。

A-LOAM 是 LOAM 的一个简洁代码实现，其采用 ceres 库来进行距离的优化函数求解，并且没有 IMU 部分。

三、论文精读

Abstract

我们提出了一种实时的里程计和建图方法，该方法使用在 6 个自由度上移动的 2 轴激光雷达测量距离【作者使用单线激光雷达自制】。这个问题的难点在于测量的距离信息是在不同的时间接收的【因为激光雷达需要通过旋转的方式采集信息】，运动估计中的误差可能会导致最终点云的错误配准。迄今为止，可以通过离线批量处理（off-line batch）的方法构建连贯的 3D 地图，并使用回环（loop closure）来校正随时间而产生的漂移。我们实现了一种低漂移和低计算复杂度的方法，无需高精度测距或惯性测量。**该方法的核心思路是通过分成两种算法来实现同时定位和建图的问题，同时优化大量的变量。第一种算法执行高频率【10 Hz】但低精度的里程计去估计雷达速度【前端】。另一种算法以低一个数量级的频率【1 Hz】运行，用于点云的精细匹配和配准（registation）【后端】。 **通过结合这两种算法即可实现实时建图。该方法已通过大量实验以及 KITTI 里程计基准（benchmark）进行了评估。结果表明，该方法可以达到离线批处理方法的精度水平。

图 1. 该方法旨在使用移动的 2 轴激光雷达进行运动估计和建图。由于激光雷达的运动，激光点是在不同的时间接收到的，在点云中会出现失真（如图左激光雷达云所示）。我们提出的方法通过两个并行运行的算法来分解问题。里程计算法（odometry）估计激光雷达的速度并校正点云中的失真，然后建图算法（mapping）匹配并配准（registers）点云以创建地图。两种算法的结合确保了要实时解决的问题的可行性。

2轴激光雷达的理解是水平的旋转加上垂直的运动。

三维激光雷达（多线雷达，激光束在垂直方向按照时间间隔依次发射）先垂直扫描再水平旋转实现三维扫描。

I. INTRODUCTION

3D建图仍然是一种主流的技术[1]–[3]。使用激光雷达进行建图是非常常见的，因为激光雷达可以提供高频的距离测量信息，无论测量的距离如何，误差都相对恒定。当激光雷达的唯一运动是旋转激光束时，其点云的配准非常简单。然而，如果激光雷达在多自由度（in many applications of interest）中运动，则精确的建图需要在连续激光测距期间了解激光雷达的姿态变换。解决该问题的一种常见方法是使用独立的位置估计（例如通过 GPS/INS）将激光点配准到固定的坐标系中。另一组方法使用里程计测量，例如从车轮编码器或视觉里程计系统[4]，[5]记录激光点。由于里程计集成了随时间推移的小增量运动，因此必然会发生漂移，并且会将大量注意力放在减小漂移上（例如，使用回环 loop closure）。

在这里，我们考虑通过使用在 6 自由度运动的 2 轴激光雷达的低漂移里程计来进行地图创建的场景。使用激光雷达的一个关键优势是它对场景中的环境光亮和光学纹理不敏感。激光雷达的最新发展减小了它们的尺寸和重量。激光雷达可以由人手持[6]，甚至可以连接到微型飞行器[7]。由于我们的方法旨在推动里程计估计中的最小化漂移的问题，因此目前不涉及回环检测（loop closure）。

该方法实现了低漂移和低计算复杂度，无需高精度测距或惯性测量。获得这一性能水平的关键思想是通过两种算法将 SLAM 问题[8]进行划分，同时寻求（seek）去优化大量变量。一种算法以高频但低精度的性能进行里程测量，以估计激光雷达的速度。另一种算法以较低数量级的频率运行，用于点云的精细匹配和配准（registration）。尽管非必要，但如果 IMU 可用，则可以用于提供运动先验以帮助计算高频运动。具体来说，这两种算法都提取位于锐边和平面上（sharp edges and planar surfaces）的特征点，并分别将特征点与边线段和平面匹配。在里程计算法中，通过足够快速的计算来找到特征点的对应关系。在建图算法中，通过相关的特征值和特征向量来检查局部点簇（local point clusters） 的几何分布来确定对应关系。

通过对原始问题进行分解，首先解决了一个简单的问题，即在线运动估计。之后，mapping 通过批量优化（类似于迭代最近点（ICP）方法[9]）的方法来生成高精度运动估计和地图。并行算法结构保证了问题实时求解的可行性。此外，由于运动估计是在更高的频率下进行的【里程计比建图频率更高】，因此给了 mapping 足够的时间来提高精度。当以较低的频率运行时，建图算法算法处理了大量的特征点，并使用足够多的迭代进行收敛。

II. RELATED WORK

激光雷达已成为机器人导航中有用的距离传感器[10]。对于定位和映射，大多数应用使用 2D 激光雷达[11]。当激光雷达扫描速率与其外部运动相比较高时，扫描中的运动失真通常可以忽略不计。在这种情况下，可以使用标准 ICP 方法[12]来匹配并返回激光的不同扫描。此外，提出了一种两步方法来消除失真[13]：基于 ICP 的速度估计步骤之后是使用计算的速度来进行失真补偿的步骤。类似的技术也用于补偿单轴 3D 激光雷达引入的失真[14]。然而，如果扫描运动相对较慢，则运动失真可能会很严重。当使用 2 轴激光雷达时尤其如此，因为一个轴通常比另一个轴慢得多。通常，使用其他传感器来提供速度测量，从而可以消除失真。例如，可以通过与 IMU 集成的视觉里程计的状态估计来注册激光雷达点云[15]。当 GPS/INS 和车轮编码器等多个传感器同时可用时，通常该问题可通过扩展卡尔曼滤波器[16]或粒子滤波器[1]解决。这些方法可以实时创建地图，以帮助机器人导航中的路径规划和碰撞避免。

如果使用 2 轴激光雷达而不借助其他传感器，则运动估计和失真校正成为一个问题。Barfoot 等人使用的一种方法是根据返回的激光强度来创建视觉图像，并在图像之间匹配视觉上不同的特征[17]，以恢复地面车辆的运动[18]–[21]。车辆运动在[18]、[19]中建模为恒定速度，在[20]、[21]中采用高斯过程。我们的方法在里程计算法中使用了与[18]、[19]相似的线性运动模型，但具有不同类型的特征。方法[18]-[21]涉及强度图像的视觉特征，需要密集点云。我们的方法提取并匹配笛卡尔空间中的几何特征，对云密度的要求较低。

最接近我们的方法是 Bosse 和 Zlot 的方法[3]，[6]，[22]。他们使用 2 轴激光雷达获得点云，通过匹配局部点簇的几何结构进行配准[22]。此外，他们使用多个 2 轴激光雷达来绘制地下矿山的地图[3]。该方法结合了 IMU，并使用循环闭包（loop closure）来创建大型地图。由同一作者提出，Zebedee 是一种由 2D 激光雷达和 IMU 组成的标测设备，IMU 通过弹簧固定在手杆上[6]。通过工人对设备点头来实施建图。通过批优化方法恢复轨迹，该方法处理分段数据集，并在分段之间添加边界约束。在该方法中，IMU 的测量用于配准激光点，优化用于校正 IMU 偏差。本质上，Bosse 和 Zlot 的方法需要批量处理才能生成精确的地图，因此不适用于需要实时地图的应用。相比之下，所提出的方法实时生成的地图在质量上与 Bosse 和 Zlot 的地图相似。区别在于，我们的方法可以为自动驾驶车辆的导航提供运动估计。此外，该方法利用了激光雷达扫描模式和点云分布。在里程计和建图算法中，分别实现了确保计算速度和精度的特征匹配。

总结如下：

1、在机器人的状态估计问题上，加装 GPS/INS 传感器，利用数据融合的方法可以解决激光雷达的运动状态估计问题，但存在累计误差，且必产生该情况导致的数据漂移，因此会有较差的情况，滤波器的收敛性会降低；

2、激光雷达是在定位与构图领域的最优传感器，但处理点云数据时，由于存在运动失真、匹配计算偏差、计算速度慢等原因，使得结果可信度较低，需要大量计算。如果融合了其他可以测速的传感器，运动失真可以有较好补偿，但如果不使用激光，只用 GPS 与编码器之类的传感器，也会与1遇到相同难点，问题无法解决；

3、如果没有其他传感器，只使用激光，问题将会变得比较复杂。利用激光数据进行环境重建，对不同特征进行匹配，构建模型来推导运动过程，从而获得状态位姿，是一种较好的方案。在作者之前有过类似在矿井下的应用，但作者在以无人驾驶汽车的特别场景下进行了新算法（A-LOAM）的测试与验证，从而为汽车提供了一个运动里程计。

III. NOTATIONS AND TASK DESCRIPTION

本文解决的问题是利用 3D 激光雷达感知到的点云进行自运动估计，并建立遍历环境的地图。我们假设激光雷达是预先校准的。我们还假设激光雷达的角速度和线速度是平滑和连续的，没有突变。第二项假设将在 Section VII-B 中通过使用 IMU 提出。

作为本文的一个惯例，我们使用右上角的大写上标来表示坐标系。我们将 sweep 定义为激光雷达完成一次扫描覆盖的时间。我们使用右下角的小写下标 $k, k \in Z^+$ 表示扫描帧（sweeps）， $P_k$ 表示第 $k$ 次扫描期间感知到的点云。让我们定义如下两个坐标系。

• 雷达坐标系 $\{ L \}$ 是一个三维坐标系，其原点位于雷达的几何中心。$x$ 轴指向左侧，$y$ 轴指向上方，$z$ 轴指向前方。激光点 $i, i \in P_k$ 在 $\{ L_k \}$ 坐标系下表示为 $X^L_{(k, i)}$ 。
• 世界坐标系 $\{ W \}$ 是一个三维坐标系，其初始位置与 $\{ L \}$ 一致。激光点 $i, i \in P_k$ 在 $\{ W_k \}$ 坐标系下表示为 $X^W _{(k,i)}$ 。

当时的 lidar 坐标系是上述指向，目前 lidar 坐标系，例如 velodyne 是 $x$ 轴指向前， $y$ 轴指向左， $z$ 轴指向上的坐标系。

通过假设和标记，我们的激光雷达里程计和建图问题可以定义为

问题：给定第 $k$ 次扫描的激光点云序列 $P_k, k \in Z^+$ ，用 $P_k$ 来估计每次扫描后的运动状态，并利用这个点云建立一张地图。

IV. SYSTEM OVERVIEW

A. Lidar Hardware

本文的研究在基于 Hokuyo UTM-30LX 激光扫描仪的定制 3D 激光雷达上得到验证，但不限于此。通过本文，我们将使用从激光雷达收集的数据来说明该方法。激光扫描仪的视野为 $180^{\circ}$ ，分辨率为 $0.25^{\circ}$ ，扫描速率为 40 行/秒。激光扫描仪与电机相连，电机被控制在 $-90^{\circ}$ 到 $90^{\circ}$ 中以 $180^{\circ}/s$ 的角速度旋转，激光扫描仪的水平角度为零。对于此特定单元，一次扫描帧（sweep）是从 $-90^{\circ}$ 到 $90^{\circ}$ 或相反方向进行的（持续 1 s）。请注意，对于连续旋转的激光雷达，一次扫描帧（sweep）只是一个半球形旋转。车载编码器以 $0.25^{\circ}$ 的分辨率测量电机旋转角度，激光点被投影到激光雷达坐标 $\{ L \}$ 。

注：文中使用单线激光雷达和电机构成了一个多线激光雷达。通过 Hokuyo，装到电机上完成三维点云扫描，一次完整的扫描为一次 sweep。

一般的，对于多线雷达而言，其中某一线束旋转一周称之为 scan，而所有线束完整旋转一周则称为 sweep。即sweep为激光雷达完成一次扫描的所有scans的组合，一个 sweep 包含多个 scan。

畸变描述：单纯的激光数据会出现畸变，这是因为激光的点云数据不是同一时间获取的，激光雷达在获取一帧点云的过程中是不断运动的，故激光的点云数据不是同一时间获取的，造成一帧点云数据中越早获得的点相对其对应的真实位置偏移量越大。

如下图所示，当 sweep 开始时激光处于 A 位置，当 sweep 结束时激光处于 B 位置，因为激光扫描时间较长，如果是一堵平行的墙面，理想状况下激光处于 A 或者 B 位置测量的距离应该一样，但是实际中激光处于 A 位置测量的距离和处于 B 位置的测量的距离一定会出现差异，失去了原始平行墙面的特征，进行激光 SLAM 时， AB 位置的差距必须要考虑，或者说激光在这期间的运动必须要考虑，对应到点云数据会发现，点云数据出现较大的偏移。因此在数据处理时需要构造激光运动模型来消除点云畸变带来的影响。

下图中激光雷达 朝向物体运动，旋向为顺时针，这就导致点云左边点在激光雷达运动方向上的畸变就要大于右边点的偏移量。

B. Software System Overview

图 3 为软件系统框架图。定义 $\hat{P}$ 为激光扫描所接收到的点。在每次扫描期间， $\hat{P}$ 在激光雷达坐标系 $\{ L \}$ 中配准。第 $k$ 次扫描期间的点云组合形成 $P_k$ 。然后，用两种算法处理 $P_k$ 。lidar odometry 通过点云来计算激光雷达在两次连续的扫描帧之间的运动。估计的运动被用来校正在 $P_k$ 中的畸变。该算法的运行频率约为 $10\mathrm{Hz}$ 。输出的结果被 lidar mapping 算法施行进一步的处理，它以 $1\mathrm{Hz}$ 的频率将去畸变的点云匹配并配准到地图上。最后，将两种算法发布的姿态变换进行集成，得到激光雷达姿态相对于地图的约 $10\mathrm{Hz}$ 的变换输出。Section V and VI 对软件框图中的模块进行了详细介绍。

图 3 为LOAM的软件系统框架图

V. LIDAR ODOMETRY

A. Feature Point Extraction（特征点提取算法）

首先我们从激光雷达点云 $P_k$ 中提取特征点。图 2 所示的激光雷达在 $P_k$ 中会产生分布不均匀的点。激光扫描仪返回的分辨率为 $0.25^{\circ}$ 。这些点位于一个扫描平面上。但是，由于激光扫描仪以 $180^{\circ}/s$ 的角速度旋转，并以 $40\mathrm{Hz}$ 的频率生成扫描，垂直于扫描平面的分辨率为 $180^{\circ} / 40 = 4.5^{\circ}$ 。考虑到这一事实，特征点仅使用从 $P_k$ 中提取的单个扫描（scan）的信息，具有共面几何关系.

图2。本研究使用的 3D 激光雷达由一个旋转运动电机驱动的 Hokuyo 激光扫描仪和一个测量旋转角度的编码器组成。激光扫描仪的视野为 $180^{\circ}$ ，分辨率为 $0.25^{\circ}$ ，扫描速率为 40 行/秒。电机被控制从 $-90^{\circ}$ 到 $90^{\circ}$ 旋转，且水平方向上角度为零。

我们选择位于锐边（sharp edges）和平面片（planar surface patches）上的特征点【选择角点和面点作为特征点】。定义 $i$ 为激光雷达点云 $P_k$ 中的一个点，$i \in P_k$，设 $S$ 为激光扫描仪在同一次扫描中返回的 $i$ 的连续点的集合。由于激光扫描仪以 CW 或 CCW 顺序【CW 为顺时针，CCW 逆时针，目前的 lidar 大部分是顺时针旋转】返回生成点，$S$ 中在 $i$ 左右两侧各有一半的点，其中每两个点之间的间隔为 $0.25^{\circ}$ 。定义一个术语以评估局部曲面的平滑度（smoothness）【也可以叫做曲率】，

$$c = \frac{1}{\left| S \right| \cdot \left\| X_{(k, i)}^{L} \right\|} \left\| \sum_{j \in S, j \ne i}{\left( X_{(k, i)}^{L} - X_{(k, j)}^{L} \right)} \right\| \quad\quad\quad\quad\quad (1)$$

$X_{(k, i)}^{L}$ 指的是 $L$ （雷达）坐标系下第 $k$ 次扫描的点云 $P_k$ 中的第 $i$ 个点。

通过归一化矢量和的模来判断点i的类型。边缘点的矢量和的模一般较大，矢量和不为零向量，而对应平面点的矢量和的模一般较小，矢量和为零向量。

j 就是在 i 周围的点。曲率 = （当前点到其附近点的距离差 / 当前点的值 ) 的总和再求平均 = 平均的距离差

理解式(1)的意思，为什么式(1)的大小可以分辨出边缘点和平面点？因为如果是边缘点，那么 $i$ 点与周围点的差值一定会变大，因为激光测的是距离呀，比如凹凸的地方和平滑的面显然不一样呀，理解一下；个人感觉也可以取一个窗口，或者半径的圆，计算 $i$ 点的光滑度 $c$ 。

扫描中的点根据 $c$ 值【曲率】进行排序，然后选择具有最大 $c$ 值（称为边缘点）和最小 $c$ 值（称为平面点）的特征点。为了在环境中均匀分布特征点，我们将一次扫描分为四个相同的子区域【代码中是6等分】。每个子区域最多可提供 2 个边缘点和 4 个平面点。仅当点 $i$ 的 $c$ 值大于或小于阈值且所选点的数量不超过最大值时，才可以将点 $i$ 选择为边缘点或平面点。

在选择特征点时，我们希望避免选择其周围点已经被选择的点【为了使特征点均匀分布】，或局部平面上与激光束大致平行的点（图4(a)中的点 B）。这些点通常被认为是不可靠的。此外，我们还希望避免位于遮挡区域边界上的点[23]。图4(b)中示出了一个示例。点 A 是激光雷达点云中的一个边缘点，因为其连接表面（虚线段）被另一个对象阻挡。但是，如果激光雷达移动到另一个视角，则遮挡区域可能会发生变化并变得可见。为了避免上述点被选择，我们再次找到点集 $S$ 。仅当 $S$ 不来自大致平行于激光束的平面（surface patch），且 $S$ 中没有任何点通过激光束方向上的间隙与 $i$ 断开，同时更靠近激光雷达（如图4(b)中的点 B）时，才可选择点 $i$ 。

图4。(a)实线段表示局部平面片（local surface patches）。点 A 在一个 平面片（surface patch）上，它与激光束（橙色点线段）有一定的角度。点 B 位于一个与激光束大致平行的 surface patch 上。我们将 B 视为不可靠的激光回波，不选择它作为特征点。(b)实线段是激光的可观测对象。点 A 位于被遮挡区域（橙色点线段）的边界上，会被检测为边缘点。然而，如果从不同的角度看，被遮挡的区域可以改变并变得可见。我们不把 A 作为一个显著的边缘点，也不把它作为一个特征点。

图4(a)的点B因为与激光束平行，雷达运动后可能就看不见了。可能下一帧就没了，无法匹配，因此不可靠。

图4(b)的点A并不是边缘点，而是被遮挡的平面点，但是由于被遮挡断层，导致曲率较大，可能错误认为是边缘点。

总之，特征点被选择为从最大 $c$ 值开始的边缘点和从最小 $c$ 值开始的平面点，如果一个点要被选择，

• 选择的边缘点或平面点的数量不能超过子区域的最大值，并且
• 它的周围点都没有被选择，并且
• 它不能在大致平行于激光束的平面上，或位于遮挡区域的边界上。

从走廊场景中提取特征点的实例如图 5 所示。边缘点和平面点分别用黄色和红色标注。

图5。从一条走廊的激光雷达云中提取边缘点（黄色）和平面点（红色）的例子。同时，激光雷达以 $0.5\mathrm{~m}/\mathrm{s}$ 的速度向图像左侧墙体移动，导致墙体运动变形。

B. Finding Feature Point Correspondence（特征关联算法）

里程计 odometry 算法在一个扫描帧（sweep）中估计激光雷达的运动。让 $t_k$ 作为扫描帧 $k$ 的开始时间。在每次扫描结束时，扫描期间感知到的点云 $P_k$ 被重新投影到时间戳 $t_{k+1}$ ，如图 6 所示。我们将重新投影的点云表示为 $\overline{P}_k$ 。在下一次扫描 $k+1$ 期间， $\overline{P}_k$ 【经过畸变校正】与新接收的点云 $P_{k+1}$ 【未经过畸变校正】一起使用，以估计激光雷达的运动。

图6, 将点云重新投影到扫描的末尾。蓝色线段表示第 $k$ 次扫描期间感知到的点云 $P_k$ 。在第 $k$ 次扫描结束时，将 $P_k$ 重新投影到时间戳 $t_{k+1}$ 以获得 $\overline{P}_k$ （绿色线段）。然后，在第 $k+1$ 次扫描期间， $\overline{P}_k$ 和新感知的点云 $P_{k+1}$ （橙色线段）一起用于估计激光雷达运动。

让我们假设 $\overline{\mathcal{P}}_{K}$ 和 $\mathcal{P}_{K+1}$ 现在都可用，并从查找两个激光雷达点云之间的对应关系开始。对于 $\mathcal{P}_{k+1}$ ，我们使用上一节讨论的方法从激光雷达点云中查找边缘点和平面点。设 $\mathcal{E}_{k+1}$ 和 $\mathcal{H}_{k+1}$ 分别为边缘点集和平面点集。我们将从 $\overline{\mathcal{P}}_{k}$ 中找到边缘线（edge lines）作为 $\mathcal{E}_{k+1}$ 中的点的对应，找到平面片（planar patches）作为 $\mathcal{H}_{k+1}$ 中的点的对应。

这里简单说一下，为什么？理想情况下，没有畸变，激光雷达在短时间里获取的相邻帧数据是含有大量同一特征点的，就是 $P_{k+1}$ 和 $P_{k}$ 中很多特征点只是雷达在不同位置下观察的同一个点。（前面已经去除了不可靠的特征点），那么现在只要找到一个变换关系 $T$ 使 $P_{k+1}$ 和 $P_{k}$ 中对应的点重合，那这个 $T$ 实际上表示了激光雷达的帧间运动。为了减少计算量，增加鲁棒性，不直接使用点与点的匹配，而是使用边缘点和对应的线特征，平面点和对应的面特征，这样也可以得到变换关系 $T$ 。然而现在的点云数据是含有畸变的，并且将矫正的 $P_{k}$ 也重投影到了 $t_{k+1}$ 时刻，那么现在就是一边进行矫正，一边进行匹配。直到所有点和对应特征区域距离和最小。这就是一个迭代的过程，迭代时，变换关系 $T$ 一直在发生变化， $T$ 的变换使所有点和对应特征区域距离和一直朝着最小的方向前进。

请注意，在第 $k+1$ 次扫描开始时， $\mathcal{P}_{k+1}$ 是一个空集【现在的 lidar 是一次性给出一个 sweep 点云的】，在扫描过程中随着接收到更多的点而增长。激光雷达里程计递归地估计扫描期间的 6 自由度运动，并随着 $\mathcal{P}_{k+1}$ 的增加逐渐包括更多点。在每次迭代中，使用当前估计的变换将 $\mathcal{E}_{k+1}$ 和 $\mathcal{H}_{k+1}$ 重新投影到扫描的开始时刻。设 $\tilde{\mathcal{E}}_{k+1}$ 和 $\tilde{\mathcal{H}}_{k+1}$ 为重投影点集。对于 $\tilde{\mathcal{E}}_{k+1}$ 和$ \tilde{\mathcal{H}}_{k+1}$ 中的每个点，我们将在 $\overline{\mathcal{P}}_{k}$ 中找到最邻近点【这个时候他们的时间戳都是 $t_{k+1}$ ，因此可以进行匹配】。这里我们将 $\overline{\mathcal{P}}_{k}$ 存储在 3D KD-tree [24]中，用于快速索引。

图7(a)表示寻找一个边缘点的对应边缘线（edge line）的过程。设 $i$ 是 $\tilde{\mathcal{E}}_{k+1}$ 中的一个点， $i \in \tilde{\mathcal{E}}_{k+1}$ 。边缘线（edge line）由两点表示。定义 $j$ 表示 $\overline{\mathcal{P}}_{k}$ 中 $i$ 的最近邻点， $j \in \overline{\mathcal{P}}_{k}$ ，并定义 $l$ 是与 $j$ 相邻的连续两条扫描线束（scan）【上下各一条线束 scan ，相邻两次扫描是为了防止运动畸变过大导致边缘线特征提取不正确】中 $i$ 的最近邻点。 $(j, l)$ 形成 $i$ 的对应关系。然后，为了验证 $j$ 和 $l$ 都是边缘点，我们基于公式(1)检查局部曲面的平滑度。这里，我们特别要求 $j$ 和 $l$ 来自不同的扫描线束，因为单个扫描线束（scan）不能包含来自同一边缘线（edge line）的多个点【保证线特征与扫描线不是同一条线】。只有一个例外，即边缘线位于扫描平面（scan plane）上。但是，如果是这样，则边缘线将退化并在扫描平面上显示为一条直线，并且不应首先提取边缘线上的特征点。

图7(b)展示出了寻找作为平面点对应的平面片（planar patches）的过程。设 $i$ 为 $\tilde{\mathcal{H}}_{k+1}$ 中的一个点， $i \in \tilde{\mathcal{H}}_{k+1}$ 。平面片由三个点表示。与上一段类似，我们在 $\overline{\mathcal{P}}_{k}$ 中找到 $i$ 的最近邻点，表示为 $j$ 。然后，我们找另外两个点， $l$ 和 $m$ 均是 $i$ 的最近邻点，一个在 $j$ 的相同扫描线束中，另一个在 $j$ 的相邻两个连续扫描线束中。这保证了三个点不共线。为了验证 $j$、$l$ 和 $m$ 都是平面点，我们根据公式(1)再次检查局部曲面的平滑度。

如果三个点在同一条扫描线上会导致平面退化为直线。如果三个点在不同的扫描线上，例如 $j$ 和 $l$ 也不在同一条扫描线上的话，会存在三个点都在竖直方向上共线的风险。所以像(b)那样寻找对应平面片。

图7。(a)表示边缘线（edge line）如何在 $\tilde{\mathcal{E}}_{k+1}$ 中查找对应的线点，(b)表示平面片（planar patch）如何在 $\tilde{\mathcal{H}}_{k+1}$ 中查找对应的面点。在(a)和(b)中， $j$ 是距离特征点最近的点，位于 $\mathcal{P}_{k}$ 中。橙色线条表示 $j$ 的同一条扫描线，蓝色线条表示相邻的两次连续扫描线。为了找到(a)中的边线对应关系，我们在蓝色线上找到另一个点 $l$ ，对应关系表示为 $(j, l)$ 。为了找到(b)中的平面片对应关系，我们在橙色和蓝色线上分别找到另外两个点， $l$ 和 $m$ 。对应关系为 $(j, l, m)$ 。

根据找到的特征点的对应关系，现在我们导出表达式来计算从特征点到其对应关系的距离。在下一节中，我们将通过最小化特征点的总距离来恢复激光雷达的运动。

根据匹配的原理，可以构造优化问题：求解变换关系 $T$ 使边缘点和边缘线距离最短，平面点和平面距离最短。

我们从边缘点开始。对于点 $i \in \tilde{\mathcal{E}}_{k+1}$ ，如果 $(j, l)$ 是对应的边线， $j, l \in \overline{\mathcal{P}}_{k}$ ，则点到线的距离可以计算为

$$d_{\mathcal{E}} = \frac{\left| ( \tilde{\boldsymbol{X}}_{(k + 1, i)}^{L} - \overline{\boldsymbol{X}}_{(k, j)}^{L} ) \times ( \tilde{\boldsymbol{X}}_{(k + 1, i)}^{L} - \overline{\boldsymbol{X}}_{(k, l)}^{L} ) \right|} {\left| \overline{\boldsymbol{X}}_{(k, j)}^{L} - \overline{\boldsymbol{X}}_{(k, l)}^{L} \right|} \quad\quad\quad\quad\quad (2)$$

公式(2)图示：

公式解释如下：

叉乘表示向量组成的平行四边形面积，面积/边 = 高。这里的高就是距离。

为了获取两帧间的数据关联情况，应尽可能的让点到直线的距离最小。

其中 $\tilde{\boldsymbol{X}}_{(k+1, i)}^{L}$ ， $\overline{\boldsymbol{X}}_{(k, j)}^{L}$ 和 $\overline{\boldsymbol{X}}_{(k, l)}^{L}$ 分别是 $\{ L \}$ 中点 $i$，$j$ 和 $l$ 的坐标。

使用当前估计的变换将 $\mathcal{E}_{k+1}$ 和 $\mathcal{H}_{k+1}$ 重新投影到扫描的开始。 $\tilde{\boldsymbol{X}}_{(k+1, i)}^{L}$ 表示将 $k+1$ 帧扫描中的点云重新投影到 $k+1$ 帧开始时刻后的坐标。

然后，对于点 $i \in \tilde{\mathcal{H}}_{k+1}$ ，如果 $(j, l, m)$ 是对应的平面片， $j, l, m \in \overline{\mathcal{P}}_{k}$ ，则点到平面的距离为

$$d_{\mathcal{E}} = \frac{ \begin{vmatrix} ( \tilde{\boldsymbol{X}}_{(k + 1, i)}^{L} - \overline{\boldsymbol{X}}_{(k, j)}^{L} ) \\ ( ( \overline{\boldsymbol{X}}_{(k, j)}^{L} - \overline{\boldsymbol{X}}_{(k, l)}^{L} ) \times ( \overline{\boldsymbol{X}}_{(k, j)}^{L} - \overline{\boldsymbol{X}}_{(k, m)}^{L} ) ) \end{vmatrix} } {\left| ( \overline{\boldsymbol{X}}_{(k, j)}^{L} - \overline{\boldsymbol{X}}_{(k, l)}^{L} ) \times ( \overline{\boldsymbol{X}}_{(k, j)}^{L} - \overline{\boldsymbol{X}}_{(k, m)}^{L} ) \right|} \quad\quad\quad\quad\quad (3)$$

公式(3)图示：

公式解释如下：

叉乘表示向量组成的平行四边形面积，叉乘再点乘表示立体的体积，体积/面积 = 高。这里的高就是距离。

为了获取两帧间的数据关联情况，应尽可能的让点到平面的距离最小（相当于让向量 ij 到向量 jn 的投影的模长最小）。

其中 $\overline{\boldsymbol{X}}_{(k, m)}^{L}$ 是点 $m$ 在 $\{ L \}$中的坐标。

理想下，距离均为0，也就是边缘点位于边缘线，平面点位于平面。但这是不可能的。只能找到一个不为0的最小值。

C. Motion Estimation（运动状态估计算法）

获取帧间关系后，如何用数学形式求解便是我们状态估计算法的侧重内容，论文介绍时使用非线性最小二乘思想，构建残差函数，最优化距离参数，获得的转移矩阵便是我们认为最优解。

激光雷达在扫描的过程中以恒定的角速度和线速度进行运动建模。这允许我们在扫描中对在不同时间接收到的点进行线性插值计算姿势变换。让 $t$ 作为当前时间戳，并记住 $t_{k+1}$ 是第 $k+1$ 次扫描的开始时间。设 $\boldsymbol{T}_{k+1}^{L}$ 为 $\left[t_{k+1}, t\right]$ 之间的激光雷达的姿态变换。 $\boldsymbol{T}_{k+1}^{L}$ 包含激光雷达在 6-DOF 中的刚体运动， $\boldsymbol{T}_{k+1}^{L} = \left[t_{x}, t_{y}, t_{z}, \theta_{x}, \theta_{y}, \theta_{z}\right]^{T}$ ，其中 $t_{x}$ ， $t_{y}$ 和 $t_{z}$ 分别表示为沿着 $\{ L \}$ 的 $x$ ，$y$ 和 $z$ 轴的平移，$\theta_{x}$ ，$\theta_{y}$ 和 $\theta_{z}$ 表示为绕轴的旋转角度，遵循右手规则。给定一个点 $i$ ， $i \in \mathcal{P}_{k+1}$ ，让 $t_{i}$ 作为它的时间戳，让 $\boldsymbol{T}_{(k+1, i)}^{L}$ 作为 $\left[t_{k+1}, t_{i}\right]$ 之间的姿势变换。 $\boldsymbol{T}_{(k+1, i)}^{L}$ 可以通过 $\boldsymbol{T}_{k+1}^{L}$ 的线性插值来计算，

$$\boldsymbol{T}_{(k+1, i)}^{L} = \frac{t_i - t_{k + 1}}{t - t_{k + 1}} \boldsymbol{T}_{k+1}^{L} \quad\quad\quad\quad\quad (4)$$

点的运动状态补偿：就是激光扫描一圈的同时，用线性插值来补偿每个点在运动过程中可能产生的漂移。

计算第 k+1 次 sweep 期间的一个点的位姿。

回想一下， $\mathcal{E}_{k+1}$ 和 $\mathcal{H}_{k+1}$ 是从 $\mathcal{P}_{k+1}$ 中提取的边缘点集和平面点集，而 $\tilde{\mathcal{E}}_{k+1}$ 和 $\tilde{\mathcal{H}}_{k+1}$ 是重新投影到扫描开始的点集。为了得到激光雷达的运动，我们需要在 $\mathcal{E}_{k+1}$ 和 $\tilde{\mathcal{E}}_{k+1}$ ，或者 $\mathcal{H}_{k+1}$ 和 $\tilde{\mathcal{H}}_{k+1}$ 之间建立几何关系。使用公式(4)中的变换，我们可以得出，

$$\boldsymbol{X}_{(k + 1, i)}^{L} = \boldsymbol{R}\tilde{\boldsymbol{X}}_{(k + 1, i)}^{L} + \boldsymbol{T}_{(k+1, i)}^{L}(1:3) \quad\quad\quad\quad\quad (5)$$

其中 $\boldsymbol{X}_{(k+1, i)}^{L}$ 是点 $i$ 在 $\mathcal{E}_{k+1}$ 或 $\mathcal{H}_{k+1}$ 的坐标， $\tilde{\boldsymbol{X}}_{(k+1, i)}^{L}$ 是点 $i$ 在 $\tilde{\mathcal{E}}_{k+1}$ 或 $\tilde{\mathcal{H}}_{k+1}$ 中的对应点。 $\boldsymbol{T}_{(k+1, i)}^{L}(a: b)$ 是 $\boldsymbol{T}_{(k+1, i)}^{L}$ 的第 $a$ 到第 $b$ 个条目，而 $\mathbf{R}$ 是由罗德里格斯公式定义的旋转矩阵，

$$\mathbf{R} = e^{\widehat{\omega}\theta} = \mathbf{I} + \widehat{\omega}\sin\theta + \widehat{\omega}^{2}(1 - \cos\theta) \quad\quad\quad\quad\quad (6)$$

需要说明的是，旋转平移的表达形式有非常多种，论文中使用 [R,T] 的直观形式来表示，代码中使用四元数进行计算， $\boldsymbol{T}_{(k+1, i)}^{L}$ 是一个 1x3 的列向量。

在上面的等式中， $\theta$ 是旋转的幅度，

$$\theta = \left\| \boldsymbol{T}_{(k+1, i)}^{L}(4:6) \right\| \quad\quad\quad\quad\quad (7)$$

$\omega$ 表示旋转方向的单位向量，

$$\omega = \frac{ \boldsymbol{T}_{(k+1, i)}^{L}(4:6) }{ \left\| \boldsymbol{T}_{(k+1, i)}^{L}(4:6) \right\| } \quad\quad\quad\quad\quad (8)$$

$\widehat{\omega}$ 是 $\omega$ 的斜对称矩阵。

这里是旋转的角轴（李群）的基本知识使用。

回想一下，公式(2)和公式(3)计算 $\tilde{E}_{k+1}$ 和 $\mathcal{H}_{k+1}$ 中点之间的距离及其对应关系。结合公式(2)和公式(4) - 公式(8)，我们可以导出 $\mathcal{E}_{k+1}$ 中的边缘点与相应边缘线（edge line）之间的几何关系，

$$f_{\mathcal{E}}(\boldsymbol{X}_{(k + 1, i)}^{L}, \boldsymbol{T}_{k+1}^{L}) = d_{\mathcal{E}}, i \in \mathcal{E}_{k + 1} \quad\quad\quad\quad\quad (9)$$

类似地，结合公式(3)和公式(4) - 公式(8)，我们可以在 $\mathcal{H}_{k+1}$ 中的平面点和相对应的平面片（planar patch）之间建立另一个几何关系，

$$f_{\mathcal{H}}(\boldsymbol{X}_{(k + 1, i)}^{L}, \boldsymbol{T}_{k+1}^{L}) = d_{\mathcal{H}}, i \in \mathcal{H}_{k + 1} \quad\quad\quad\quad\quad (10)$$

最后，我们使用非线性最小二乘法（Levenberg-Marquardt method）求解激光雷达运动[26]。对 $\mathcal{E}_{k+1}$ 和 $\mathcal{H}_{k+1}$ 中的每个特征点叠加公式(9)和公式(10)，我们得到一个非线性函数，

在代码中使用的是高斯牛顿法。

$$\boldsymbol{f}(\boldsymbol{T}_{k+1}^{L}) = \boldsymbol{d} \quad\quad\quad\quad\quad (11)$$

求解变换矩阵 T 从而让两个 d（残差：点到线的距离和点到面的距离）最小。

其中， $\boldsymbol{f}$ 的每一行对应一个特征点， $\boldsymbol{d}$ 包含相应的距离。我们计算 $\boldsymbol{f}$ 关于 $\boldsymbol{T}_{k+1}^{L}$ 的雅可比矩阵，表示为 $\mathbf{J}$ ，其中$\mathbf{J}=\partial f / \partial \boldsymbol{T}_{k+1}^{L}$ 。 然后，公式(11)可以通过非线性迭代求解【列文伯格-马夸尔特方法】，将向零最小化 $\boldsymbol{d}$ ，

$$\boldsymbol{T}_{k+1}^{L} \leftarrow \boldsymbol{T}_{k+1}^{L} - (\mathbf{J}^{T}\mathbf{J} + \lambda diag(\mathbf{J}^{T}\mathbf{J}))^{-1}\mathbf{J}^{T}\boldsymbol{d} \quad\quad\quad\quad\quad (12)$$

$\lambda$ 是由非线性最小二乘法（Levenberg-Marquardt method）确定的系数。

由该方法可以获得最优化的参数估计，从而获得状态转移关系。

D. Lidar Odometry Algorithm（里程计算法）

激光雷达里程计算法如 Algorithm 1 所示。该算法将最后一次扫描的点云 $\overline{\mathcal{P}}_{k}$ 、当前正在扫描的增长点云 $\mathcal{P}_{k+1}$ 和最后一次递归的姿势变换 $T_{k+1}^{L}$ 作为输入【匀速模型】。如果一个新的扫描开始， $T_{k+1}^{L}$ 被设置为零（第 4 - 6 行）。然后，该算法从 $\mathcal{P}_{K+1}$ 中提取特征点，在第 7 行构造 $\mathcal{E}_{k+1}$ 和 $\mathcal{H}_{k+1}$ 。对于每个特征点，我们在 $\overline{\mathcal{P}}_{k}$ 中找到其对应关系（第 9 - 19 行）。运动估计适用于鲁棒拟合（robust fitting）。在第 15 行中，算法为每个特征点分配一个二次方权重（bisquare weight）。对与其对应点距离较大的特征点分配较小的权重，对距离大于阈值的特征点视为异常值并分配零权重【类似于核函数】。然后，在第 16 行中，姿势变换迭代更新一次。如果发现收敛或满足最大迭代次数，则非线性优化终止。如果算法到达扫描结束时， $\mathcal{P}_{k+1}$ 将使用扫描期间的运动估计重新投影到时间戳 $t_{k+2}$ 【当前 lidar 直接获得一帧数据，此处判断可以忽略】。否则，下一轮递归只返回转换 $T_{k+1}^{L}$ 。

VI. LIDAR MAPPING（地图构建算法）

建图 mapping 算法的运行频率低于里程计 odometry 算法，每次扫描只调用一次。在第 $k+1$ 次扫描结束时，激光雷达里程计生成一个去畸变的点云 $\overline{\mathcal{P}}_{k+1}$ ，同时生成一个 $\left[t_{k+1}, t_{k+2}\right]$ 之间包含扫描期间的激光雷达运动的姿势变换 $\boldsymbol{T}_{k+1}^{L}$ 。mapping 算法将 $\overline{\mathcal{P}}_{K+1}$ 在世界坐标 $\{ W \}$ 中匹配并配准，如图 8 所示。为了解释这个过程，我们将 $\mathcal{Q}_{K}$ 定义为地图上累积到第 $k$ 次扫描的点云，并将 $\boldsymbol{T}_{k}^{W}$ 定义为第 $k$ 次扫描结束 $t_{k+1}$ 时地图上激光雷达的姿态。利用激光雷达里程计的输出，mapping 算法将 $\boldsymbol{T}_{k}^{W}$ 从 $t_{k+1}$ 扩展到 $t_{k+2}$ ，以获得 $\boldsymbol{T}_{k+1}^{W}$ ，并将 $ \overline{\mathcal{P}}_{k+1}$ 投影到世界坐标 $\{ W \}$，表示为 $\overline{\mathcal{Q}}_{k+1}$ 。接下来，该算法通过优化 lidar 姿态 $\boldsymbol{T}_{k+1}$ 将 $\overline{\mathcal{Q}}_{k+1}$ 与 $\mathcal{Q}_{k}$ 进行匹配。

图 8 为 mapping 过程的图示。蓝色曲线表示地图上的激光雷达姿态 $\boldsymbol{T}_{k}^{W}$ ，由第 $k$ 次扫描的 mapping 算法生成。橙色曲线表示第 $k+1$ 次扫描期间由里程计算法计算的激光雷达运动 $\boldsymbol{T}_{k+1}^{L}$ 。使用 $\boldsymbol{T}_{k}^{W}$ 和 $\boldsymbol{T}_{k+1}^{L}$ 将里程计算法发布的去畸变后的点云投影到地图上，表示为 $\overline{\mathcal{Q}}_{k+1}$ （绿色线段），并与地图上的现有的点云 $\mathcal{Q}_{k}$ （黑色线段）进行匹配。

$\overline{\mathcal{Q}}_{k+1}$ 为最新帧投影到开始时的点云。

里程计得到的位姿只能作为最后建图的初值，从图中绿色线段就可以看出存在较大的误差，LOAM中是通过采用 scan-map 的匹配优化 $\boldsymbol{T}_{k+1}^{W}$ ，至于 $\boldsymbol{T}_{k}^{W}$ 是通过第一帧作为世界坐标，所有相对运动（位姿） $\boldsymbol{T}_{k}^{L}$ 累乘即可得到。

特征点的提取方法与 Section V-A 相同【但是是在地图中找匹配点】，但使用了 10 倍的特征点【即里程计 10 次输出的数据】。为了找到特征点的对应关系，我们将点云存储在地图 $Q_k$ 上，以 10 立方米的大小存储。立方体（cube）中与 $\overline{Q}_{k+1}$ 相交的点被提取并存储在 3D KD-tree 中[24]。我们在 $Q_k$ 中寻找特征点周围的一定区域内的点。设 $S'$ 为一组周围点。对于边缘点，我们只在 $S'$ 中保留边缘线（edge lines）上的点，对于平面点，我们只保留在平面片（planar patches）上的点。然后，我们计算 $S'$ 的协方差矩阵，表示为 $M$ ， $M$ 的特征值和特征向量分别表示为 $V$ 和 $E$ 。如果 $S'$ 分布在边缘线上， $V$ 包含一个显著大于其他两个的特征值， $E$ 中与最大特征值相关的特征向量表示边缘线的方向。另一方面，如果 $S'$ 分布在一个平面片上， $V$ 包含两个较大的特征值，第三个特征值明显较小， $E$ 中与最小特征值相关的特征向量表示平面片的方向【法向量】。边缘线或平面片的位置通过 $S'$ 的几何中心确定。

根据数学知识，可以得到一个区域的三维点坐标分布与这些点三维坐标形成的协方差矩阵是存在一定关系的。

为了计算从特征点到其对应点的距离，我们在边缘线上选择两个点，在平面片上选择三个点。这允许使用与公式(2)和公式(3)相同的公式计算距离。然后，基于公式(9)或公式(10)推出每个特征点的方程，但不同之处在于 $\overline{Q}_{k+1}$ 中的所有点都具有相同的时间戳 $t_{k+2}$ 【即不需要线性插值】。使用非线性最小二乘法（Levenberg-Marquardt method）[26]通过鲁棒拟合（robust fitting）[27]可以再次解决非线性优化问题，并将 $\overline{Q}_{k+1}$ 在地图上配准。为了均匀分布点，地图点云通过体素网格过滤器（voxel grid filter）[28]缩小为体素大小为 $5\mathrm{~cm}$ 的立方体【下采样，每个立方体中保留一个点】。

后端就改变了匹配的方式，相对于 scan-scan 匹配，scan-map 的匹配精度更高。由于地图较大，所以通过体素滤波降采样。

姿势变换的集成（intergration）如图9所示。蓝色区域表示激光雷达 mapping 输出的位势 $\boldsymbol{T}_k^W$ ，每次扫描生成一次。橙色区域表示激光雷达里程计 odometry 输出的变换（transform） $\boldsymbol{T}_{k+1}^L$ ，频率约为 $10\mathrm{~Hz}$ 。相对于地图的激光雷达姿态是两个变换的组合，频率与激光雷达里程计相同。

图9。位姿变换的集成。蓝色区域显示了 mapping 算法中的激光雷达位姿 $\boldsymbol{T}_k^W$ ，每次扫描生成一次。橙色区域是当前扫描范围内的激光雷达运动 $\boldsymbol{T}_{k+1}^L$ ，由里程计 odometry 算法计算而来。激光雷达的运动估计是两种变换的组合，频率与 $\boldsymbol{T}_{k+1}^L$ 相同。

VII. EXPERIMENTS

在实验期间，处理激光雷达数据的算法在笔记本电脑上运行，笔记本电脑具有 2.5 GHz 四核和 6 Gib 内存，在 Linux 中的机器人操作系统（ROS）上运行。该方法总共需要两个核心，里程计和地图程序在两个独立的核心上运行。我们的软件代码和数据集是公开的。

A. Indoor & Outdoor Tests

该方法已在室内和室外环境中进行了测试。在室内测试中，激光雷达与电池和笔记本电脑一起放在一辆手推车上。一个人推着车走。图10（a）和图10（c）显示了在两个代表性室内环境中构建的地图，一个狭长的走廊和一个大堂。图10（b）和图10（d）显示了从同一场景拍摄的两张照片。在户外测试中，激光雷达安装在地面车辆的前部。图10（e）和图10（g）显示了从两排树木之间的植被道路和果园生成的地图，照片分别显示在图10（f）和图中。在所有测试中，激光雷达以 0.5m/s 的速度移动。

图 10 。（a）-（b）狭长走廊、（c）-（d）大堂、（e）-（f）植被道路和（g）-（h）两排树木之间的果园中生成的地图。激光雷达在室内测试中被放置在推车上，在室外测试中被安装在地面车辆上。所有测试均采用0.5m/s的速度。

为了评估地图的局部准确性，我们从相同的环境中收集第二组激光雷达云。在数据选择期间，激光雷达保持静止，并放置在每个环境中的几个不同位置。使用点对面ICP方法匹配并比较两个点云。匹配完成后，一个点云与第二个点云中对应的平面面片之间的距离被视为匹配误差。图 11 显示了误差分布的密度。它表明室内环境中的匹配误差比室外环境中的小。结果是合理的，因为在自然环境中的特征匹配不如在制造环境中精确。

图 11 。走廊（红色）、大厅（绿色）、植被道路（蓝色）和果园（黑色）的匹配误差，对应于图 10 中的四个场景。

结论1：本方案针对不同场景下的收敛度不同，室内环境优于室外环境，此结果符合预期，且误差结果均较优。

此外，我们进行测试以测量运动估计的累积漂移。我们选择包含闭环的室内实验走廊。这使我们可以在同一地点开始和结束。运动估计在开始位置和结束位置之间生成间隙，该间隙指示漂移量。对于户外实验，我们选择果园环境。搭载激光雷达的地面车辆配备了高精度 GPS/INS ，用于地面实况采集。将测量的漂移与作为相对精度的行驶距离进行比较，并在表 I 中列出。具体而言，测试 1 使用与图 10（a）和图 10（g）相同的数据集。一般来说，室内测试的相对准确度约为 1%，室外测试的相对准确性约为 2.5%。

表 I 。运动估计漂移的相对误差。

B. Assistance from an IMU

我们在激光雷达上安装了 Xsens MTi-10 IMU，以应对快速速度变化。在将点云发送到所提出的方法之前，以两种方式对点云进行预处理，1）利用IMU的定向，旋转在一次扫描中接收到的点云，以与该扫描中激光雷达的初始定向配准，2）利用加速度测量，部分消除运动失真，就像激光雷达在扫描期间以恒定速度移动一样。然后，点云由激光雷达里程表和测绘程序处理。

IMU定向是通过在卡尔曼滤波器中对陀螺仪的角速率和加速度计的读数进行积分而获得的[1]。图 12（a）显示了样本结果。一个人拿着激光雷达走在楼梯上。当计算红色曲线时，我们使用IMU提供的方向，我们的方法仅估计平移。方位漂移超过 $25^{\circ}$ 在 5 分钟的数据收集期间。绿色曲线仅依赖于我们方法中的优化，假设没有IMU可用。蓝色曲线使用IMU数据进行预处理，然后使用所提出的方法。我们观察到绿色和蓝色曲线之间的微小差异。图 12（b）显示了对应于蓝色曲线的地图。在图 12（c）中，我们比较了图 12（b）中黄色矩形中地图的两个闭合视图。上图和下图分别对应于蓝色和绿色曲线。仔细比较发现上图中的边缘更尖锐。

图12。有/无 IMU 辅助的结果比较。一个人拿着激光雷达走在楼梯上。黑点是起点。在（a）中，使用IMU的方向和我们的方法估计的平移来计算红色曲线，绿色曲线仅依赖于我们的方法中的优化，蓝色曲线使用IMU数据进行预处理，然后使用该方法。（b） 是对应于蓝色曲线的地图。在（c）中，使用由（b）中的黄色矩形标记的区域，上下图分别对应于蓝色和绿色曲线。上图中的边缘更清晰，表明地图上的精度更高。

结论2：加装IMU设备时的几组方案对比中，纯IMU的结果较差，纯激光A-LOAM与附加IMU的激光A-LOAM差别不大，附带IMU的方案优势体现在转弯处，因为它可以作为一种额外的运动补偿输入。

表 II 比较了使用和不使用 IMU 时运动估计的相对误差。激光雷达由一个人以 0.5 米/秒 的速度行走，并以 0.5 米左右的幅度上下移动激光雷达。地面实况由卷尺手动测量。在所有四个测试中，在IMU的帮助下使用所提出的方法可以获得最高的精度，而使用 IMU 的定向只能获得最低的精度。结果表明，IMU 在消除非线性运动方面是有效的，因此，所提出的方法处理线性运动。

表 II 。使用/不使用IMU时的运动估计误差。

C. Tests with KITTI Datasets

我们还使用 KITTI 里程计基准[30]，[31]的数据集评估了我们的方法。数据集与安装在结构化道路上行驶的乘用车（图13（a））顶部的传感器仔细登记。车辆配备了 $360^{\circ}$ Velodyne 激光雷达、彩色/单色立体相机和用于地面实况的高精度 GPS/INS 。激光雷达数据以 10Hz 记录，并由我们的方法用于里程表估计。由于空间问题，我们无法包含结果。然而，我们鼓励读者在基准网站上查看我们的结果。

数据集主要涵盖三种类型的环境：周围有建筑物的“城市”，场景中有植被的小路上的“乡村”，以及道路较宽且周围环境相对清洁的“公路”。图13（b）显示了来自城市环境的激光雷达云样本和相应的视觉图像。数据集中包含的总行驶距离为39.2公里。上传车辆轨迹后，基准服务器自动计算准确度和排名。我们的方法在基准评估的所有方法中排名第一，而不考虑传感模式，包括最先进的立体视觉里程表[32]，[33]。平均位置误差为行进距离的0.88%，使用三维坐标中100m、200m、…、800m长度的轨迹段生成。

图 13 。（a）KITTI基准使用的传感器配置和车辆。该车安装有Velodyne激光雷达、立体相机和高精度GPS/INS，用于地面实况采集。我们的方法仅使用Velodyne激光雷达的数据。（b） 来自城市场景的激光雷达云样本（上图）和相应的视觉图像（下图）。

结论3：基于KITTI的无人驾驶汽车数据集，得到的结果较好，在基于前述计算精度的基准上评估轨迹与算法的运动估计，平均偏差0.88%，排名靠前。

VIII. CONCLUSION AND FUTURE WORK

使用旋转激光扫描仪的点云进行运动估计和建图可能很困难，因为该问题涉及到恢复激光雷达点云中的运动和校正运动失真。所提出的方法通过两种并行运行的算法来解决该问题：激光雷达里程计（lidar odometry 算法）以较高频率进行粗处理以估计速度，而激光雷达建图（lidar mapping 算法）以较低频率进行精细处理以创建地图。这两种算法的结合允许精确的运动估计和实时的建图。此外，该方法可以利用激光雷达扫描模式和点云分布的优势。特征匹配是为了保证里程计 odometry 算法的快速计算，并提高建图 mapping 算法的准确性。该方法已在室内外以及 KITTI 里程计基准上进行了测试。

由于目前的方法不识别闭合的环路（loop closure），我们未来的工作涉及开发一种通过闭合回路（closing the loop）来修复运动估计漂移的方法。此外，我们还将我们的方法的输出与卡尔曼滤波器中的 IMU 集成，以进一步减小运动估计漂移。

四、参考

loam论文翻译：https://blog.csdn.net/weixin_45626706/article/details/121886389 （论文翻译）

（Lidar SLAM 论文）LOAM: Lidar Odometry and Mapping in Real-time：https://blog.csdn.net/weixin_44035919/article/details/124926931 （论文翻译）

SLAM算法工程师之路：A-LOAM论文研读与框架算法学习：https://zhuanlan.zhihu.com/p/431082432 （总结性过了一遍论文）

【论文阅读】LOAM: Lidar Odometry and Mapping in Real-time：https://blog.csdn.net/weixin_43849505/article/details/121342354 （各个模块之间的关系讲的不错）

LOAM SLAM之论文原理解读：https://blog.csdn.net/i_robots/article/details/108331306 （很多拓展知识）

激光雷达 LOAM 论文 解析：https://blog.csdn.net/hltt3838/article/details/109261334 （很多拓展知识，还介绍了雷达工作原理）

LOAM-SLAM原理深度解析：https://zhuanlan.zhihu.com/p/111388877 （结合代码讲，很细）