文章目录

LOAM、LEGO-LOAM与LIO-SAM的知识总结
- 1.概要
- 2.传感器信息读取
- 3.数据的预处理
- 4.激光雷达里程计
- - 4.1特征点提取
  - 4.2特征点关联匹配
  - - 4.2.1 标签匹配
    - 4.2.2 两步LM优化
    - 4.2.3 LIO-SAM优化
    - - 4.2.3.1 IMU预积分
      - 4.2.3.2 关键帧的引入
      - 4.2.3.3 因子图
      - 4.2.3.4 GPS因子
      - 4.2.3.5 回环因子
- 5. 实时建图
- Reference

LOAM、LEGO-LOAM与LIO-SAM的知识总结

1.概要

LOAM、LEGO-LOAM与LIO-SAM这三个实现的算法是激光SLAM上的经典之作。三者的框架在大体上差不多。首先，明确激光SLAM的核心目的，便是基于一个可以移动和旋转的激光雷达机器人进行6自由度状态估计的实时定位和建图技术。6自由度即沿着X,Y,Z三个轴移动以及绕着X,Y,Z三个轴旋转，而这六个变量也就是我们所要估计优化的变量。过程则一般是有以下几步：传感器信息读取、前端视觉里程计、后端（非线性）优化以及实时建图。其中也有些会包含预处理减少数据量以及回环检测减少路线的漂移。我按照算法步骤分别列举三者的相同点与不同点。

2.传感器信息读取

LOAM：采用了Hokuyo UTM-30LX激光扫描仪，扫描的范围是180°（-90°到90°）分辨率为0.25°，旋转速度为180°/s，扫描的密度是40线/s。扫描是从−90◦ 至90◦ 或相反方向（持续1s）。当然除了扫描机器人本身，还搭载了Xsens MTi-10 IMU惯性传感器，以应对高速变化。

LEGO-LOAM：利用Velodyne VLP-16和HDL-64E三维激光雷达采集数据集。利用Velodyne VLP-16和HDL-64E三维激光雷达采集的数据集对本文提出的框架进行了验证。VLP-16测量范围高达100m，精度为±3cm。它的垂直视野为30°(±15°)，水平视野为360°。16线传感器提供2°的垂直角度分辨率。根据旋转速率，水平角度分辨率从0.1°-0.4°不等。在论文中选择了10HZ的扫描速率，所以它能提供0.2°的水平角度分辨率。HDL-64E的水平视野也为360°，垂直视野为26.9°。设备是配置在Clearpath Jackal无人车上，由270瓦时锂电池供电，最大速度为2.0m/s，最大有效载荷为20kg。小车配置了一个低成本的惯性测量单元（IMU），即CH Robotics UM6方位传感器。

LIO-SAM：采用Velodyne VLP-16激光雷达、一个 MicroStrain 3DM-GX5-25 IMU和一个Reach M GPS。UGV依旧采用Clearpath Jackal。

总的来说loam的激光扫描仪还是时间比较早，比较差一点，只能扫描半圈，相对来说LEGO后采用的这个扫描仪就强多了，精度也更高。LIO-SAM在LEGO的基础上提升了IMU的质量，添加了一个GPS模块用来增加一些因组图的因子以提升优化精度。

3.数据的预处理

LOAM：很遗憾，LOAM在拿到数据后并未做太多预处理，只是筛除了一些明显数值异常的点。

LEGO-LOAM：首先将所有三维点云数据投影到距离图像上，这个图像分辨率为1800✖16，其中1800是水平方向分辨率是0.2，3600.2=1800\frac{360}{0.2}=18000.2360=1800,竖直方向有16根扫描线。这样每一个点就占该距离图像中的一个像素点。而与点关联的距离值为点到传感器的欧氏距离。其中的竖直方向上为16，这个也就是一个维度用来标记他是否是地面点。按常理来说，VLP-16的激光扫描束竖直方向的角度为[-15°，15°],地面点应当出现在[-15°，-1°]上。

接着是用距离图像应用到基于图像的分割，将点分为多个簇，来自同一个簇的点被指定一个唯一的标签。地面点的处理比较特殊，是独立出来处理的。当然如果碰到这种类似树叶的点（噪音项），如果该聚类少于30个点，我们便忽略掉。该聚类寻找算法其实就是广度优先算法。这个BFS算法不是传统意义上用距离作为判断依据，而是角度，原因是垂直方向与水平方向上的分辨率差太大（2°和0.2°，这个网上找的一个博文说的，难道是因为水平方向密很多，垂直方向疏，不太好设定阈值吗），所以不能一概而论。但是用角度那就具有统一性。下图比较直观描述这判断的方法：

其中θ\thetaθ与did_idi和di+1d_{i+1}di+1我们在距离图中已知，那么三角形便固定，那个要判断的角度便可计算，对应的便是角度越大离得越近（小于90°时），其实我觉得和判断两点间的距离大体差不多，但是确实他是在考虑点到传感器距离以及两点扫描角度差的前提下的结果，确实更合理。这样角度大于阈值并且没有标记的为一个聚类，小于阈值没有标记的进入下一个BFS循环，聚类标签加一。

这样我们就通过预处理获得了距离图像的索引以及值，以及各个筛选后的点包括其标签值（点分好聚类，地面点标签为0，其他标签依次递增）。其实在聚类过程中，点也不是全部考虑拿来聚类的，除了前面筛选，代码中还体现了只取了总点云的1/5来参与聚类，减少了很多数据量

LIO-SAM：步骤相对LEGO-LOAM简单很多，存储一个距离图像，但是好像没有采用BFS来分聚类和标记地面点。

4.激光雷达里程计

4.1特征点提取

LOAM：最重要的就是作者采用了一种曲率公式，如下：
c=1∣S∣⋅∥X(k,i)L∥∥∑j∈S,j≠i(X(k,i)L−X(k,j)L)c = \frac{1}{\vert S\vert ·\Vert X^L_{(k,i)}\Vert }\Vert\sum\limits_{j\in S,j\neq i}(X^L_{(k,i)}-X^L_{(k,j)}) c=∣S∣⋅∥X(k,i)L∥1∥j∈S,j=i∑(X(k,i)L−X(k,j)L)
翻译一下便是，在代码实现中，取该点前后各五个点，分别对X，Y，Z维度取出偏差量，或者说是偏移量，然后三个维度偏差量取平方和，该值便是曲率。在后续判断特征点时，曲率较大的点为边缘点，较小的点为平面点。在实现中，作者还定义了次边缘点和次平面点（cornerPointsLessSharp and surfPointsLessFlatScan），边缘点范围较小，次边缘点范围较大，除了边缘点次边缘点以及平面点的点，都是次平面点。

当然除了这个判断条件外，还有两类异常点，作者特地在论文中提到筛除：

该点的周围的点尽量不要被再被取到，这样可以使整体的特征点分布更加的平均（实现的时候也是判断周围点的距离因素）
该点不能与雷达扫描束过于平行，这样也会出现问题（个人猜测用角度判断，但是A-LOAM中反正没体现）

LEGO-LOAM：特征提取过程类似于LOAM中使用的方法。从地面点和分割后的点中提取特征，以代替从原始点云中提取特征。论文中算法如下（这么说是因为论文和源代码实现有出入）：
c=1∣S∣⋅∥ri∥∥∑j∈S,j≠i(rj−ri)∥c=\frac{1}{|S| \cdot\left\|r_{i}\right\|}\left\|\sum_{j \in S, j \neq i}\left(r_{j}-r_{i}\right)\right\| c=∣S∣⋅∥ri∥1j∈S,j=i∑(rj−ri)
曲率的计算是取当前点的左边5个点和右边5个点和当前点 * 10的距离差值（距离传感器的值），然后求平方。而论文中的计算方法和代码中的不一样，论文中是取距离差的平均值，然后除以自身的模，也就是说论文中的曲率和尺度无关，不管当前点离lidar近还是远，只要曲率超过一定的值就可以，同时在仿真环境也可以解决尺度的问题，而代码中直接取得是绝对大小。貌似LOAM也是有这种差异的，本人不知道原因是为什么。

对于异常点两种情况的处理也是一样的。当然特征提取的过程中边缘点只能来自于非地面点，平面点则必须是地面点，其他类同LOAM。

LIO-SAM：在这一块，LIO-SAM与前两者在思路上有较大出入。曲率的算法同LEGO-LOAM，但是区别在于，它直接取出曲率前20的点为边缘点，而只要曲率小于一个阈值，就全部标为-1。但是最后，传入到surface点云的，是所有cloudLabel不为1的点云，也就是说，只要不属于smoothness最大的20个边缘点，都会被放进去，即不是最大的20个边缘点都是平面点。处理的步骤相对于LEGO-LOAM更简单。显然，在LIO-SAM的算法中，边缘点是可以来自于任何点的，包括地面或者别的树叶啥的。但是这样，LIO-SAM的效率应该也快不少。且作者没有沿用之前的分为四类的方法，而是分为两类，不懂其中的奥妙。

4.2特征点关联匹配

LOAM：论文讲的很繁琐，我的理解就是，前面已经将剩下的有效点云分为sharp、lesssharp、flat、lessflat四个类，分别记作{Fe,Fme,Fp,Fmp}\{F_e,F_{me},F_p,F_{mp}\}{Fe,Fme,Fp,Fmp}。记当前时刻为t，上一时刻为t-1，我们选取 t 时刻的特征点 FetF_e^tFet 和 FptF_p^tFpt，以及 t−1 时刻的 Fmet−1F_{me}^{t-1}Fmet−1 和 Fmpt−1F_{mp}^{t-1}Fmpt−1。由于Fe⊂Fme&Fp⊂FmpF_e\subset F_{me}\And F_p\subset F_{mp}Fe⊂Fme&Fp⊂Fmp,所以肯定可以从 Fmet−1F_{me}^{t-1}Fmet−1 和 Fmpt−1F_{mp}^{t-1}Fmpt−1中找到 FetF_e^tFet 和 FptF_p^tFpt的关联点。具体的方法就是对于每个t中的特征点，比如边缘点A，从t-1时刻中找一个最近的次边缘点B，再在该最近点B往前后一定范围找一个离B最近的点C（不能是同一扫描线），这样BC确定一条线，点到线(a)图（点到面(b)图）的距离就可以构建一个非线性优化问题，用LM（列文伯格-马夸尔特）法迭代求解。

LEGO-LOAM:总体上是与LOAM相似的，LeGO-LOAM还做了些许的改进来提高匹配的准确性和效率，主要在以下两个方面：

标签匹配
两步LM优化

4.2.1 标签匹配

在前面部分将点云划分为地面点和聚类点，使用这些标签来进行对应关联点的搜寻。

对于边缘点：在Fmet−1F_{me}^{t-1}Fmet−1中具有相同聚类标签的点云中寻找FetF_e^tFet的对应关联点
对于平面点，在Fmpt−1F_{mp}^{t-1}Fmpt−1中具有地面点标签的点云中寻找FptF_p^tFpt的对应关联点

这一步的操作其实非常巧妙，有以下两个优点：

在相邻帧中地面信息基本保持不变
聚类后点云被分为若干块，缩小了对应点的候选范围

但是当地面并非平坦的时候，地面点标签并不理想，该匹配效果就大打折扣。

4.2.2 两步LM优化

在这一步，和LOAM相比，将针对6-DOF([tx,ty,tz,troll,tpitch,tyaw])( [t_x,t_y,t_z,t_{roll},t_{pitch},t_{yaw}] )([tx,ty,tz,troll,tpitch,tyaw])的一次LM优化，更改为了两次LM优化方法：

优化计算{Fpt,Fmpt−1}\{F_p^t,F_{mp}^{t−1}\}{Fpt,Fmpt−1}的对应点约束，得到[tz,troll,tpitch][t_z,t_{roll},t_{pitch}][tz,troll,tpitch]
优化计算 {Fet,Fmet−1}\{F_e^t,F_{me}^{t−1}\}{Fet,Fmet−1} 的对应点约束，基于第一步得到的 [tz,troll,tpitch][t_z,t_{roll},t_{pitch}][tz,troll,tpitch]，得到 [tx,ty,tyaw][t_x,t_y,t_{yaw}][tx,ty,tyaw]

两步计算有着一定的合理性：

由于地面在相邻帧间基本保持不变，所以，使用点到面的约束关系，可以计算出竖直维度的变动[tz,troll,tpitch][t_z,t_{roll},t_{pitch}][tz,troll,tpitch]。
当算出竖直维度的变动时，可以以此为初值输入到第二步的优化中，减少迭代次数，算出水平维度的变动[tx,ty,tyaw][t_x,t_y,t_{yaw}][tx,ty,tyaw]，提升计算效率。

实际上代码实现是用的牛顿迭代法，并没有用LM

4.2.3 LIO-SAM优化

大多比较类似LOAM，没有用LEGO-LOAM的那两种优化方法，因为压根没聚类。

4.2.3.1 IMU预积分

IMU预积分的立足点落在相邻两帧（点云）之间的IMU积分测量值计算及其动态更新上，获得每周期PVQ（位置、速度和姿态）增量的测量值，对照其他通过非IMU方式获得的PVQ增量的估计值，进而获得PVQ增量的残差。然后以该残差构造代价函数对每个节点的PVQ进行迭代更新和优化。

其中估计值通常需要通过非IMU的方式获得，例如通过点云到Map的匹配，这也是IMU预积分与外部的唯一界面，或者说是IMU预积分对外部的唯一依赖，只要有了这个估计值，IMU预积分就可以独立运作了。测量值就来自IMU预积分。

在图优化的过程中，要进行局部甚至全局的反复优化，随着优化的推进，IMU的测量偏差（bias）也会变化，残差中的PVQ增量测量值就需要重新计算，IMU预积分就提供了一个近似的测量值修正方法，免去了积分的重新计算，是预积分降低计算量的关键。

4.2.3.2 关键帧的引入

简而言之，LIO-SAM选取的雷达帧是通过机器人的位置和旋转的变化量来确定的。这些被选中的雷达帧即为后续使用匹配的关键帧。种方式加入关键帧不仅实现了地图密度和内存消耗之间的平衡，还有助于维持一张相对稀疏的因子图，它适合于实时的非线性优化。在文中，加入新关键帧的位置和旋转变化阈值被选为1m和10°。之后算法从这些关键帧中提取了局部边缘点、平面点云集合，并且进行了下采样。

4.2.3.3 因子图

因子图概念的引入，使得整个系统真正变成了一个紧耦合的雷达惯性里程计建图系统，并完美融合了多传感器，提供了更高的精度。增量平滑和建图，使其可以自动增量处理大规模优化问题；具体来说，其内部使用一种基于概率的贝叶斯树，使得每次给因子图增加一个约束时，会根据贝叶斯树的连接关系，调整和当前结点“关系比较密切”的结点（概率比较大）。当然具体原理很复杂，不过好处在于使用方法比较简单。

4.2.3.4 GPS因子

引入能够提供绝对测量的传感器来消除漂移。

接收到GPS测量值后，讲这些值转换到局部笛卡尔坐标系。有一个新的节点出现，就把新的GPS因子关联到这个点。如果这两个的获取世界不是同步的，就根据激光雷达帧的时间戳在GPS测量值之间进行线性插值

因为雷达惯性里程计的漂移增长缓慢，所以实际没有必要不断添加GPS因子。只当估计的位置协方差是比接收到的GPS位置协方差大的时候，才添加GPS因子

4.2.3.5 回环因子

一个基于欧式距离的回环检测方法（和LEGO-LOAM一样）。

当一个新的状态 xi+1\mathbf{x}_{i+1}xi+1被添加到因子图中，我们首先搜索图欧式距离上靠近 xi+1\mathbf{x}_{i+1}xi+1 的先前状态．拿上图举例来说，x3\mathbf{x}_{3}x3是一个返回的候选者. 然后，我们使用扫描匹配的方法尝试匹配 Fi+1\mathbf{F}_{i+1}Fi+1 和子关键帧 {F3−m,…,F3,…Fm+3}\left\{\mathbf{F}_{3-m}, \ldots, \mathbf{F}_{3}, \ldots \mathbf{F}_{m+3}\right\}{F3−m,…,F3,…Fm+3}，注意Fi+1\mathbf{F}_{i+1}Fi+1和过去的子关键帧在扫描匹配前是首先被转换到W\mathbf{W}W坐标系. 我们获得了相对转换 ΔT3,i+1\Delta \mathbf{T}_{3, i+1}ΔT3,i+1 ，然后将这个转换作为回环因子添加到因子图中. 整个论文中，我们设置 m = 12，用于回环的搜索距离设置为 15 米（也即前后12个关键帧中的距离当前状态小于15m的关键帧参与匹配，得到的相对转换）.

5. 实时建图

LOAM：首先LOAM算法的特点就是高频低精里程计，低频高精建图。精优化时使用的点云数量是原先高频时的10倍，在处理时是采用了cube存储点云，但与此同时处理的效率为里程计的1/10。算法便是定当前传感器所在位置为中心，范围为相邻水平、竖直方向各两个cube，深度方向一个cube的点云。

算法在精优化点线距时，分为以下两步：

选取点最近邻5个点，进行协方差矩阵特征值、特征向量计算，若其中一个特征值远大于其他特征值，则说明该点是边线点，其中最大的特征值对应的特征向量就是该线的方向向量。
利用得到的直线的方向向量在该点附近构造2个邻近点，同里程计使用同样点到线的距离约束方程进行约束。

在精优化点面距时，则是特征值是一个小，两个大。较小特征值对应的特征向量则是平面的法向量。在通过类似的方法构造即可。

LEGO-LOAM: LeGO-LOAM的主要区别在于最终点云地图的存储方式。我们不保存单个点云地图，而是保存每个单独的特征集{Fet,Fpt}\left\{\mathbb{F}_{e}^{t}, \mathbb{F}_{p}^{t}\right\}{Fet,Fpt}。定义Mt−1={{Fe1,Fp1},…,{Fet−1,Fpt−1}}M^{t-1}=\left\{\left\{\mathbb{F}_{e}^{1}, \mathbb{F}_{p}^{1}\right\}, \ldots,\left\{\mathbb{F}_{e}^{t-1}, \mathbb{F}_{p}^{t-1}\right\}\right\}Mt−1={{Fe1,Fp1},…,{Fet−1,Fpt−1}}为保存所有先前特征的集合。Mt−1M^{t-1}Mt−1中的每个特征集也与扫描时传感器的姿势对应。然后Qˉt−1\bar{Q}^{t-1}Qˉt−1可以通过两种方式从Mt−1M^{t-1}Mt−1获得。($\bar{Q}^{t-1} $为周围的点云映射)

基于传感器视域：Qˉt−1\bar{Q}^{t-1}Qˉt−1是通过选择传感器视野中的特征集获得的。为简单起见，我们可以选择传感器位姿在传感器当前位置100m100\mathrm{~m}100 m范围内的特征集。然后，将所选的特征集转换并融合为单个局部地图。

基于图优化：我们还可以将因子图集成到LeGO-LOAM中。每个特征集的传感器姿势可以建模为因子图中的一个节点。特征集合集{Fet,Fpt}\left\{\mathbb{F}_{e}^{t}, \mathbb{F}_{p}^{t}\right\}{Fet,Fpt}可被视为此节点的传感器测量值。由于激光雷达测绘模块的姿态估计漂移非常小，我们可以假设在短时间内没有漂移。在这种方式中，Qˉt−1\bar{Q}^{t-1}Qˉt−1可通过选择最近的一组特征集形成，即Qˉt−1={{Fet−k,Fpt−k},…,{Fet−1,Fpt−1}}\bar{Q}^{t-1}= \left\{\left\{\mathbb{F}_{e}^{t-k}, \mathbb{F}_{p}^{t-k}\right\}, \ldots,\left\{\mathbb{F}_{e}^{t-1}, \mathbb{F}_{p}^{t-1}\right\}\right\}Qˉt−1={{Fet−k,Fpt−k},…,{Fet−1,Fpt−1}}, 其中kkk定义Qˉt−1\bar{Q}^{t-1}Qˉt−1的大小。

这一部分和LOAM的最大区别在于加入了因子图和回环检测，解决了LOAM没有后端优化的弊端，提升了建图的效率。当然还有存储特征集合的方式也在一定程度上提升了建图效率。

LIO-SAM：因为整体的框架已经是以因子图来对整个系统进行耦合，前面已经交代过了。主要就是LIO-SAM直接略过了逐帧匹配的步骤，而是直接用滑动窗口机制选出一定数量的子关键帧来进行匹配优化。所以其实没有LOAM和LEGO-LOAM那样明显分为两步的措施。核心就是因子图的应用使得整个系统的精度提升了很多，里面包含了GPS因子、回环因子、关键帧优化的位姿。

Reference

LOAM翻译

LeGO-LOAM翻译

LIO-SAM翻译