本文主要是对LeGO-LOAM论文的一些学习总结，如有不对之处，希望大家提出宝贵的意见，共同进步~
由于LeGO-LOAM是基于LOAM提出的一种SLAM方法，而作者在涉及到与LOAM算法一致的方面选择一笔带过，所以建议先查看 LOAM: Lidar Odometry and Mapping in Real-time 论文阅读.

文章摘要

作者提出了一种轻量级、基于地面优化的激光里程计和建图算法LeGo-Loam,能够地面物体的六自由度位姿估计。
轻量级：能够在低功耗嵌入式系统中进行实时的位姿估计。
基于地面优化：在分割和优化的过程中利用地面的存在
本文首先利用点云分割来去除噪声，然后进行特征提取获得独特的平面和边缘特征。然后通过两段式LM方法利用平面和边缘特征在连续扫描中求解6自由度变换的不同分量。

与Loam相比，在使用地面车辆在可变地形中收集的数据集时，LeGO-LOAM在降低计算量的同时达到了相似或更好的精度。作者为了消除由漂移引起的姿态估计误差，还将LeGO-LOAM方法融入到了SLAM框架中，并使用了使用KITTI数据集进行测试。

I. 简介

地图构建和状态估计是智能机器人功能中最基本的先决条件，人们已经做出巨大努力，通过基于视觉和基于激光雷达的方法实现实时6自由度同时定位和制图。虽然基于视觉的方法在闭环检测中具有优势，但是如果用作唯一的导航传感器，它们对光照和视点变化的敏感性可能会使这种能力不可靠。另一方面，基于激光雷达的方法即使在晚上也能工作，许多高分辨率3D激光雷达允许远距离捕捉环境的细节。因此，本文重点研究利用三维激光雷达支持实时状态估计和建图。

寻找两次激光雷达扫描之间转换关系的典型方法是ICP。通过一一寻找对应，迭代计算，直到满足停止标准。但是在面对大量点云时，ICP的计算成本非常高。为了提高效率和准确性，人们提出了许多ICP的变体：比如point-to-plane ICP，将点与局部平面面片匹配；Generalized-ICP使用局部平面与全部点云匹配；除此之外还有几种ICP变种使用并行加速计算。

基于特征的匹配方法正引起越来越多的关注，因为它们通过从环境中提取代表性特征来减少计算量。而这些特征应该适合于有效匹配和视点不变。（作者接下来介绍了很多使用特征进行点云配准的算法，在这里不在赘述，有兴趣的可以看原文引用9-18）。

接下来作者介绍了LOAM,不熟悉的可以先看完 LOAM: Lidar Odometry and Mapping in Real-time 论文阅读。LOAM通过点特征到边缘/平面的扫描匹配，以找到扫描之间的对应关系。LOAM通过计算点在其局部区域的粗糙度来提取特征，选择粗糙度值高的点作为边缘特征。类似地，粗糙度值低的点被作为平面特征。实时性是创造性地将估计问题分成两个独立的算法来保证的。一种算法以高频运行，低精度估计传感器运动。另一种算法运行频率较低，但返回高精确运动估计。这两个估计融合在一起，以高频和高精度实现帧间运动估计。在KITTI数据集上，仅通过激光雷达的估计，LOAM的精度是最好的。

而作者的工作是在装备有3D激光雷达的地面车辆中实现一种可靠的、实时的、六自由度位姿估计。出于几个原因，这样一项任务并不简单。首先。很多无人驾驶车辆上没有强大的计算单元。其次，在可变地形上行驶的小型无人驾驶车辆经常会发生非平滑运动，其结果是采集的数据经常失真。更何况由于重叠有限的大运动，在两次连续扫描之间也很难找到可靠的特征对应。除此之外，从3D激光雷达接收的大量点对使用有限的机载计算资源的实时处理提出了挑战。

若UGV以平滑的运动和稳定的特征运行，并且有足够的计算资源支持时，LOAM可以获得低漂移的运动估计。然而，当资源有限时，LOAM的性能会很差。由于需要计算密集3D点云中每个点的粗糙度，轻量级嵌入式系统上特征提取的更新频率不能总是跟上传感器更新频率。UGV在嘈杂环境中的运行也给LOAM带来了挑战。由于激光雷达的安装位置通常靠近地面，来自地面的传感器噪声可能是持续存在的。例如，在草地可能会导致粗糙度值变高，因此，可能会从这些点云中提取不可靠的边缘特征。类似地，也可能从树叶中返回的点提取边缘或平面特征。这种特征对于扫描匹配来说通常是不可靠的，因为在两次连续扫描中可能看不到相同的草叶或叶子。使用这些特征可能导致不准确的配准和大的漂移。

因此，作者提出了一种轻量级和地面优化的LOAM(LeGo-ROLOM)，用于复杂环境下可变地形下的姿态估计。LeGO-LOAM是一种轻量级、实时的、能在嵌入式设备实现的位姿估计和建图的算法。在地面分割以后，执行点云分割以去除不可靠的特征点。由于LeGO-LOAM是基于地面优化的，所以引入了两步位姿优化。****在第一步中，通过从地面获取的平面特征获取 [ t z , θ r o l l , θ p i t c h ] [t_z,\theta_{roll},\theta_{pitch}] [tz​,θroll​,θpitch​];第二步则是通过匹配从分段点云中提取的边缘特征获取剩下的 [ t x , t y , θ y a w ] [t_x,t_y,\theta_{yaw}] [tx​,ty​,θyaw​]。同时还集成了回环检测以校正运动估计的漂移。

II. 硬件系统

激光雷达：
（1）VLP-16：
测量范围高达100米，精度为±3厘米。它具有30°（15°）的垂直视场（FOV）和360°的水平FOV。16通道传感器提供2°的垂直角分辨率。水平角分辨根据旋转速率不同，从0.1°到0.4°变化。论文中，我们选择10Hz的扫描速率，水平角分辨率为0.2°。

（2）HDL-64E（通过KITTI数据集研究这项工作中）：
具有360°的水平FOV，但是它还有48个通道。 HDL-64E的垂直FOV为26.9°。

移动车辆：
本文中使用的UGV是Clearpath Jackal。 由270瓦时的锂电池供电，最大速度为2.0米/秒，最大有效载荷为20千克。Jackal还配备了低成本惯性测量单元（IMU），CH Robotics UM6方位传感器。

提出的框架在Nvidia Jetson TX2和2.5GHz i7-4710MQ的laptop上验证：
Jetson TX2是一款嵌入式计算设备，配备ARM Cortex-A57 CPU。
笔记本电脑CPU以与LOAM中使用的计算硬件相同。

III. 轻量级激光雷达里程计和建图

A. 系统概况

下图展示了LeGO-LOAM的系统框架：
输入：三维激光点云

输出：六自由度位姿估计

整个系统可以分为五个模块）：分割：指获取单次扫描的点云，并将其投影到距离图像上进行分割。分割的点云被发送到特征提取模块。激光雷达里程计使用从先前模块中提取的特征来寻找与连续扫描相关的变换。这些特征在激光建图部分被转换到全局点云地图中进一步处理。最后，变换集成模块融合来自激光雷达里程计和激光雷达建图的姿态估计结果，并输出最终的姿态估计。相对于LOAM中的原始通用框架，该系统旨在提高地面车辆的效率和精度。这些模块的细节介绍如下。

B. 分割

设 P t = p 1 , p 2 , . . . . . . , p n P_t={p_1,p_2,......,p_n} Pt​=p1​,p2​,......,pn​是在时间 t t t获得的点云,其中 p i p_i pi​是 P t P_t Pt​中的一个点。 P t P_t Pt​首先被投影到范围图像（range image）上，其分辨率为1800*16，因为VLP-16的水平和垂直角度分辨率为 0.2°and 2°（360/0.2 * 16）。因此， P t P_t Pt​中的每一个有效点 p i p_i pi​均可以从范围图像中的唯一像素点来决定。与 p i p_i pi​相关的范围值 r i r_i ri​代表着点 p i p_i pi​与传感器的欧几里得距离。我们一般不认为在许多环境中很常见的斜坡地形是平坦的地面，所以在分割前先进行地面点的提取。对距离图像的逐列评估，进行地平面估计。在这个过程之后，可能代表地面的点被标记为地面点，并且不用于分割。

然后，将基于图像的分割方法应用于距离图像，将点分组为许多簇。来自同一簇的点被分配一个唯一的标签。请注意，地面点是一种特殊类型的簇。对点云进行分割可以提高处理效率和特征提取精度。假设机器人在嘈杂的环境中操作，小物体，例如树叶，可能形成琐碎和不可靠的特征，因为在两次连续扫描中不可能看到相同的树叶。为了使用分割的点云执行快速和可靠的特征提取，我们省略了少于30个点的聚类。下图表示了分割前后点云的可视化。原始点云包括许多点，这些点有可能是从不可靠特征的周围植被中获得的。

图片说明：
（a）图表示了原始点云
（b）图中红色点被标记为平面点。其余的点是分割后保留的点
（c）图中蓝色和黄色点表示 F e F_e Fe​和 F p F_p Fp​中的边缘和平面特征
（d）图中蓝色和黄色点表示 F e \mathbb{F}_e Fe​和 F p \mathbb{F}_p Fp​中的边缘和平面特征
关于 F e F_e Fe​、 F p F_p Fp​、 F e \mathbb{F}_e Fe​、 F p \mathbb{F}_p Fp​在后面介绍

在这个过程之后，只有可能代表大物体(例如，树干)的点和地面点被保留用于进一步处理。同时，范围图像中只保存这些点。同时，我们还可以每个点的三种属性：(1)作为地面点或分割点的标签，(2)它在范围图像中的列和行索引，(3)范围值。这些属性将在以下模块中使用。

C. 特征提取

特征提取过程类似于LOAM中使用的方法。然而，我们不是从原始点云中提取特征，而是从地面点和分割点中提取特征。设S是距离图像同一行的连续点的集合(即同一线激光）,S中各有一半的点在 p i p_i pi​的两边。本文将|S|设为10。使用在分割期间计算的范围值(range value)，我们可以评估点 S S S中 p i p_i pi​的粗糙度(或者说平滑度):

为了从所有方向均匀地提取特征，我们将距离图像水平地分成几个相等的子图像。然后，我们根据粗糙度值c对子图像每行中的点进行排序。与LOAM相似，我们使用阈值 c t h c_{th} cth​来区分不同类型的特征。若 p i p_i pi​对应的 c i c_i ci​大于 c t h c_{th} cth​则称为边缘特征， c i c_i ci​小于c则称为平面特征。然后从子图像每行中选取 n F e n_{\mathbb{F}_e} nFe​​个不属于地面的、 c c c值最大的边缘点；然后以相同的方式从地面点或者分割点中选择具有最小 c c c值的 n F p n_{\mathbb{F}_p} nFp​​平面特征点。让 F e \mathbb{F}_e Fe​和 F n \mathbb{F}_n Fn​成为所有子图像的所有边缘和平面特征的集合。这些特征上图(d)所示。接着选择具有最大 c c c值的、不属于地面 n F e n_{F_e} nFe​​个边缘特征点和最小 c c c值的、只属于地面 n F p n_{F_p} nFp​​个平面特征点。定义 F e F_e Fe​和 F p F_p Fp​为这一过程产生的边缘点和平面点的集合。显然 F e ⊂ F e F_e \subset \mathbb{F}_e Fe​⊂Fe​、 F p ⊂ F p F_p \subset \mathbb{F}_p Fp​⊂Fp​。上图（c）显示了 F e F_e Fe​和 F p F_p Fp​中的特征。每个子图像的分辨率为300×16。 n F e n_{F_e} nFe​​、 n F p n_{F_p} nFp​​、 n F e n_{\mathbb{F}_e} nFe​​和 n F p n_{\mathbb{F}_p} nFp​​分别选择为2、4、40和80。

D. 激光里程计

激光雷达里程计模块估计两次连续扫描之间的相对运动。两次扫描之间的转换是通过执行点到边缘和点到平面的扫描匹配来实现的。换句话说，我们需要在前一帧的特征点集合 F e \mathbb{F}_e Fe​和 F p \mathbb{F}_p Fp​中寻找 F e F_e Fe​和 F p F_p Fp​的对应特征关系。寻找对应关系的方法与LOAM一致。

但是，我们注意到可以进行一些更改来提高特征匹配的准确性和效率:

1.标签匹配： 由于 F e t F_e^t Fet​和 F p t F_p^t Fpt​中的每个特征在分割之后都用标签进行编码，因此我们需要在 F e t − 1 \mathbb{F}_e^{t-1} Fet−1​和 F p t − 1 \mathbb{F}_p^{t-1} Fpt−1​寻找具有相同标签的对应点。对于 F p t F_p^t Fpt​中的点的匹配，只有 F p t − 1 \mathbb{F}_p^{t-1} Fpt−1​中被标记为地面的点才会用于寻找对应的平面。而对于 F e t F_e^t Fet​中的点的匹配，其对应的线来自于 F e t − 1 \mathbb{F}_e^{t-1} Fet−1​中。通过这种方式找到对应关系有助于提高匹配精度。换句话说，相同对象的匹配对应更有可能在两次扫描之间找到。这个过程也缩小了潜在匹配候选的范围。

2.两步LM优化： 在LOAM中，来自当前扫描的边缘和平面特征点之间的距离以及它们与先前扫描的对应关系的一系列非线性表达式被编译成单个综合距离向量。使用LM方法来寻找两个连续扫描之间的最小距离变换。
这里我们介绍一个两步的LM优化方法。最优变换T分两步找到:(1)通过匹配 F p t F_p^t Fpt​中的平面特征与 F p t − 1 \mathbb{F}_p^{t-1} Fpt−1​对应关系估计 [ t z , θ r o l l , θ p i t c h ] [t_z,\theta_{roll},\theta_{pitch}] [tz​,θroll​,θpitch​]，（2）然后，剩余的 [ t x , t y , θ y a w ] [t_x,t_y,\theta_{yaw}] [tx​,ty​,θyaw​]使用 F e t F_e^t Fet​中的边缘特征及其在 F e t − 1 \mathbb{F}_e^{t-1} Fet−1​中的对应关系进行估计，同时使用 [ t z , θ r o l l , θ p i t c h ] [t_z,\theta_{roll},\theta_{pitch}] [tz​,θroll​,θpitch​]作为约束。应该注意的是，虽然 [ t x , t y , θ y a w ] [t_x,t_y,\theta_{yaw}] [tx​,ty​,θyaw​]也可以从第一个优化步骤中获得，但它们不太精确，不能用于第二个步骤。最后，通过融合 [ t z , θ r o l l , θ p i t c h ] [t_z,\theta_{roll},\theta_{pitch}] [tz​,θroll​,θpitch​]和 [ t x , t y , θ y a w ] [t_x,t_y,\theta_{yaw}] [tx​,ty​,θyaw​]找到两次连续扫描之间的6D变换。通过使用所提出的两步优化方法，我们观察到在计算时间减少约35%的情况下可以获得类似的精度。

E. 激光建图

激光雷达映射模块将 ( F e t , F p t ) (\mathbb{F}_e^{t},\mathbb{F}_p^{t}) (Fet​,Fpt​)中的要素与周围的点云地图 Q ˉ t − 1 \bar Q^{t-1} Qˉ​t−1进行匹配，以进一步优化姿态变换，但运行频率较低。然后在这里再次使用L-M方法，得到最终的变换。关于详细的匹配和优化程序，我们请读者参考LOAM中的描述。

LeGO-LOAM的主要区别在于最终的点云地图是如何存储的。我们不保存单点云地图，而是保存每个单独的特征集 F e t , F p t {\mathbb{F}_e^{t},\mathbb{F}_p^{t}} Fet​,Fpt​。设 M t − 1 = ( ( F e 1 , F p 1 ) , . . . . . . , ( F e t − 1 , F p t − 1 ) ) M^{t-1}=((\mathbb{F}_e^{1},\mathbb{F}_p^{1}),......,(\mathbb{F}_e^{t-1},\mathbb{F}_p^{t-1})) Mt−1=((Fe1​,Fp1​),......,(Fet−1​,Fpt−1​))为之前所有时刻特征集合的集合。 M t − 1 M^{t-1} Mt−1中每个特征集合与当时时刻liadr进行scan时的位姿相关，我们有两种方式的从 M t − 1 M^{t-1} Mt−1得到 Q ˉ t − 1 \bar Q^{t-1} Qˉ​t−1。

在第一种方法中，通过选择Lidar视野内的特征集来获得 Q ˉ t − 1 \bar Q^{t-1} Qˉ​t−1。为简单起见，我们可以在Lidar当前位置100米以内的特征集。然后，所选的特征集被转换并融合到单个周围地图 Q ˉ t − 1 \bar Q^{t-1} Qˉ​t−1中。这种地图选择技术类似于LOAM中使用的方法。

我们还可以将图优化SLAM引入到LeGO-LOAM中，将特征集合中的每个Lidar位姿作为图优化的一个节点,而 T i T^i Ti对应的 ( F e i , F p i ) (\mathbb{F}_e^{i},\mathbb{F}_p^{i}) (Fei​,Fpi​)则视为该节点的测量值。由于激光雷达建图模块的姿态估计漂移非常低，我们可以假设在短时间内没有漂移。这样， Q ˉ t − 1 \bar Q^{t-1} Qˉ​t−1可以通过选择最近的一组特征集来形成。 Q ˉ t − 1 = { { ( F e t − k , F p t − k } , . . . . . . , { ( F e t − 1 , F p t − 1 } } \bar Q^{t-1}=\{ \{(\mathbb{F}_e^{t-k},\mathbb{F}_p^{t-k}\},......, \{(\mathbb{F}_e^{t-1},\mathbb{F}_p^{t-1}\}\} Qˉ​t−1={{(Fet−k​,Fpt−k​},......,{(Fet−1​,Fpt−1​}}，其中 k k k为 Q ˉ t − 1 \bar Q^{t-1} Qˉ​t−1的大小。然后，可以使用L-M优化后的位姿变换来添加新节点和 Q ˉ t − 1 \bar Q^{t-1} Qˉ​t−1中所选节点之间的空间约束。我们可以通过执行闭环检测来进一步消除该模块的漂移。在这种情况下，如果在当前特征集和以前的特征集之间通过ICP找到匹配，则添加新的约束。然后将位姿图发送到图优化系统中来更新估计的位姿。

IV. 实验

未完待续~

V. 总结与讨论

未完待续~