LeGO-Loam代码解析（一） 项目介绍、论文解读、配置安装

目录

1.项目介绍

2. 论文解读  LeGO-LOAM：轻量级且地面优化的可变地形激光里程计与建图

2.1 摘要

2.2 介绍

2.3 正文部分1 --- System Review

2.4 正文部分2 --- Segmatation（地面点角面点分离）

2.5  Feature Extraction 正文部分3 --- 特征提取

2.6 Lidar Odom 正文部分4 --- 前端里程计

2.7 Liadr Mapping 正文部分5 --- 后端优化

3 项目配置及编译问题解决

---

1.项目介绍

        LeGO-LOAM 是 Tixiao Shan 提出的一种基于 LOAM 的改进版本，其主要是为了实现
小车在多变地形下的定位和建图，其针对前端和后端都做了一系列的改进，具体来说：
        前端
1. 对地面点进行分类和提取，避免一些一场边缘点的提取
2. 应用了一个简单的点云聚类算法，剔除了一些可能的 outlier
3. 两步迭代求解前端帧间里程记，不影响精度的情况下减轻计算负载，保障了嵌入式平台的实时性

        后端
1. 使用 slam 中关键帧的概念对后端部分进行了重构
2. 引入回环检测和位姿图优化概念,使得地图的全局一致性更好

        最后，通过融合 和 找到两次连续扫描之间的6D变换。通过使用所提出的两步优化方法，我们观察到可以实现类似的精度，同时计算时间减少了约35%（表III）。

2. 论文解读  LeGO-LOAM：轻量级且地面优化的可变地形激光里程计与建图

2.1 摘要

        摘要—我们提出了一种轻量级且地面优化的激光里程计与建图方法，称为LeGO-LOAM，用于实时地面车辆的六自由度姿态估计（要求雷达水平安装）。LeGO-LOAM是轻量级的，因为它可以在低功耗嵌入式系统上实现实时姿态估计。LeGO-LOAM是地面优化的，因为它利用分割和优化步骤中地面平面的存在。

        我们首先应用点云分割来滤除噪音，并进行特征提取以获取显著的的平面和边缘特征。然后，一个两步的Levenberg-Marquardt优化方法使用这些平面和边缘特征来解决连续扫描之间六自由度变换的帧间里程计。我们使用从可变地形环境中的地面车辆收集的数据集将LeGO-LOAM的性能与现有方法LOAM进行比较，并显示LeGO-LOAM在减少计算成本的同时实现了类似或更好的精度。我们还将LeGO-LOAM集成到SLAM框架中，以消除因漂移引起的姿态估计误差，这在使用KITTI数据集进行了测试。

2.2 介绍

        在本工作中，我们追求为装备有3D激光雷达的地面车辆实现可靠的、实时的六自由度姿态估计，以一种适用于小规模嵌入式系统高效实现的方式。这样的任务有其不容易的原因。许多无人地面车辆（UGV）由于其尺寸有限，不具备悬挂系统或强大的计算单元。小型UGV在不同地形上行驶时经常遇到非平滑的运动（运行的时候存在颠簸），因此获取的数据常常会发生畸变。由于有限的重叠区域，两次连续扫描之间的可靠特征对应关系也很难找到，因为存在大的运动（较大的颠簸会使两帧之间的运动关联出现错误）。此外，从3D激光雷达接收到的大量点云数据对于使用有限的机载计算资源进行实时处理构成了挑战。

        当我们将LOAM用于这种任务时，在无人地面车辆（UGV）以平稳运动在稳定特征中运行，并且拥有足够的计算资源支持时，我们可以获得低漂移的运动估计。然而，当资源有限时，LOAM的性能会下降。由于需要计算密集的3D点云中每个点的曲率，在轻量级嵌入式系统上进行特征提取的更新频率可能无法始终跟上传感器的更新频率。在嘈杂的环境中操作UGV也对LOAM构成了挑战。由于激光雷达在小型UGV上的安装位置通常靠近地面，来自地面的传感器噪声可能是一个持续存在的问题。例如，草地反射可能导致高粗糙度值。因此，可能会从这些点中提取出不可靠的边缘特征。同样，也可能从树叶返回的点中提取出边缘或平面特征。这些特征通常对于扫描匹配来说是不可靠的，因为同一片草叶或叶子可能不会在两次连续的扫描中出现。使用这些特征可能导致不准确的配准和较大的漂移。

        因此，我们提出了一种轻量级且地面优化的LOAM（LeGO-LOAM），用于在复杂的可变地形环境中估计UGV的姿态。LeGO-LOAM是轻量级的，因为可以在嵌入式系统上实现实时的姿态估计和建图。我们进行点云分割，以丢弃可能在地面分离后代表不可靠特征的点。LeGO-LOAM还是地面优化的，因为我们引入了一个两步优化的姿态估计方法。从地面提取的平面特征在第一步中用于获取。在第二步中，通过与分割点云提取的边缘特征匹配，获得其余的变换。我们还整合了执行闭环来纠正运动估计漂移的能力。本文的其余部分组织如下。第二部分介绍了用于实验的硬件。第三部分详细描述了所提出的方法。第四部分在各种室外环境下进行了一系列实验。

2.3 正文部分1 --- System Review

        所提出的框架概览如图1所示。系统从3D激光雷达接收输入，并输出6自由度姿态估计。整体系统分为五个模块。

        第一个模块是分割模块，它将单次扫描的点云投影到一个距离图像中进行分割。

        然后，分割后的点云被发送到特征提取模块。接下来，激光里程计使用前一个模块提取的特征来找到关联连续扫描的变换。这些特征在激光建图中进一步处理，将它们注册到全局点云地图中。最后，变换融合模块将激光里程计和激光建图的姿态估计结果合并，并输出最终的姿态估计。所提出的系统旨在相对于原始的广义LOAM框架，寻求改进地面车辆的效率和精度。以下是这些模块的详细介绍。

        "6自由度位姿"是描述物体在三维空间中位置和方向的能力。它包括沿三轴的平移和绕三轴的旋转，用于机器人、工程和计算机图形学中精确控制物体运动和定位。 

2.4 正文部分2 --- Segmatation（地面点角面点分离）

        设为时刻采集的点云，其中是 中的一个点。首先，被投影到一个距离图像上。由于VLP-16的水平和垂直角分辨率分别为0.2°和2°，所以投影的距离图像的分辨率为1800x16（每根线1800个点，共16线）。中的每个有效点现在由距离图像中的一个唯一像素表示。与点相关联的距离值表示从相应点到传感器的欧几里德距离。由于在许多环境中存在倾斜地形，我们不假设地面是平坦的。在分割之前，会对距离图像进行列优先评估，这可以看作是地面平面估计，以提取地面点。经过这个过程后，可能代表地面的点被标记为地面点，不用于分割。

        然后，对距离图像应用了一种基于图像的分割方法，将点云分成许多聚类。同一聚类中的点被赋予唯一标签。需要注意的是，地面点是一种特殊类型的聚类。将分割应用于点云可以提高处理效率和特征提取的准确性。假设机器人在嘈杂的环境中运行，小物体（例如树叶）可能形成微不足道且不可靠的特征，因为同一片叶子不太可能在两次连续扫描中出现。为了使用分割后的点云进行快速和可靠的特征提取，我们忽略了点数少于30个的聚类。图2显示了分割前后的点云可视化。原始点云包括许多点，这些点来自周围可能会产生不可靠特征的植被。

在嘈杂环境中的扫描的特征提取过程。原始点云如图(a)所示。在图(b)中，红色点被标记为地面点。其余的点是分割后保留的点。只保留树干..树叶点都被剔除掉了在图(c)中，蓝色和黄色点表示F e 和 F p 中的边缘和平面特征。在图(d)中，绿色和粉色点分别代表F e 和 F p 中的边缘和平面特征。

        在这个过程之后，只有可能代表大型物体（例如树干）和地面点的点被保留供进一步处理（如图2(b)所示）。与此同时，只有这些点被保存在距离图像中。我们还为每个点获得了三个属性：（1）它作为地面点或分割点的标签，（2）它在距离图像中的列和行索引，以及（3）它的距离值。这些属性将在接下来的模块中被利用。

2.5  Feature Extraction 正文部分3 --- 特征提取

        与其从原始点云中提取特征不同，我们从地面点和分割点中提取特征。设 S 为来自距离图像相同行的点 p i 的连续点集。S 中一半的点位于 p i 的两侧。在本文中，我们将 |S| 设置为 10。利用在分割期间计算的距离值，我们可以评估 S 中点 p i 的粗糙度，

        为了均匀地从各个方向提取特征，我们将距离图像在水平方向上分成若干相等的子图像。然后，根据它们的粗糙度值 c，对子图像中每一行的点进行排序。与 LOAM 类似，我们使用一个阈值 c th 来区分不同类型的特征。我们将 c 大于 c th 的点称为边缘特征，将 c 小于 c th 的点称为平面特征。然后，从子图像的每一行中选择具有最大 c 的 n F e 个不属于地面的边缘特征点。以相同方式选择具有最小 c 的 n F p 个平面特征点，它们可能被标记为地面点或分割点。设 F e 和 F p 分别为所有子图像中的边缘和平面特征的集合。这些特征在图2(d)中可视化。然后，从子图像的每一行中提取具有最大 c 的 n F e 个不属于地面的边缘特征。类似地，从子图像的每一行中提取具有最小 c 的 n F p 个平面特征，它们必须是地面点。设 F e 和 F p 分别为此过程中的所有边缘和平面特征的集合。在这里，我们将360°的距离图像分成了6个子图像。每个子图像的分辨率为300x16。选择 n F e、n F p、n F e 和 n F p 分别为2、4、40和80。

2.6 Lidar Odom 正文部分4 --- 前端里程计

        激光里程计模块估计了两个连续扫描之间的传感器运动。通过执行点对边和点对平面的扫描匹配，找到了两次扫描之间的变换。换句话说，我们需要从上一次扫描的特征集合中找到与这次扫描对应的特征。

        然而，我们注意到可以进行一些改进来提高特征匹配的准确性和效率：

        1) 标签匹配：由于  和 中的每个特征在分割后都被编码为其标签，我们只寻找在 和 中具有相同标签的对应关系。对于 中的平面特征，在 中仅使用被标记为地面点的点来寻找一个平面块作为对应。对于 中的边缘特征，其对应的边缘线在 的分割聚类中找到。通过这种方式找到对应关系可以提高匹配的准确性。换句话说，相同物体的匹配对应关系更有可能在两次扫描之间找到。这个过程也缩小了对应关系的潜在候选集。

        2) 两步L-M优化：在[20]中，编译了当前扫描中的边缘和平面特征点与它们在前一次扫描中的对应关系之间距离的一系列非线性表达式，形成一个综合距离向量。应用Levenberg-Marquardt（L-M）方法来找到两次连续扫描之间的最小距离变换。

        我们在这里引入了一个两步的L-M优化方法。在两步中找到最优的变换T：

        (1) 通过匹配 中的平面特征和它们在 中的对应关系来估计

        (2) 再使用 中的边缘特征和它们在 中的对应关系来估计剩余的，同时以 作为约束。需要注意的是，尽管也可以从第一步优化中获得，但它们的准确性较低，不用于第二步。

        最后，通过融合 和找到两次连续扫描之间的6D变换。通过使用所提出的两步优化方法，我们观察到可以实现类似的精度，同时计算时间减少了约35%（表III）。

激光里程计模块的两步优化。首先通过匹配从地面点提取的平面特征获得 [t z , θ roll , θ pitch ]。然后，使用从分割点提取的边缘特征来估计 [t x , t y , θ yaw ]，同时将 [t z , θ roll , θ pitch ] 作为约束。

2.7 Liadr Mapping 正文部分5 --- 后端优化

        激光建图模块将特征 与周围的点云地图 进行匹配，以进一步优化姿态变换，但运行频率较低。然后，再次在此处使用 L-M 方法来获得最终的变换。

        LeGO-LOAM的主要区别在于最终点云地图的存储方式。与保存单个点云地图不同，我们保存每个独立的特征集。令为保存所有先前特征集的集合。中的每个特征集也与传感器在扫描时的姿态相关联。然后，可以从中以两种方式获得。

        在第一种方法中，是通过选择传感器视野范围内的特征集来获得的。为简单起见，我们可以选择传感器姿态在当前传感器位置附近100米范围内的特征集。然后，选择的特征集被转换并融合成一个单独的周围地图。

        我们还可以将位姿图（pose-graph）SLAM集成到LeGO-LOAM中。每个特征集的传感器姿态可以被建模为位姿图中的一个节点。特征集可以被视为这个节点的传感器测量。由于激光建图模块的姿态估计漂移非常小，我们可以假设在短时间内没有漂移。通过这种方式，可以通过选择一组近期的特征集来形成，即, 其中 k 定义了 的大小。然后，可以使用L-M优化后获得的变换将新节点 t−1 与 Q 中选择的节点之间添加空间约束。我们还可以通过执行闭环检测来进一步消除该模块中的漂移。在这种情况下，如果当前特征集与之前的特征集之间存在匹配，可以使用ICP（Iterative Closest Point）添加新的约束。然后，通过将位姿图发送到优化系统来更新传感器的估计姿态。

3 项目配置及编译问题解决

        本人计算机为Y9000K 2022 3080Ti电脑，Ubuntu版本为18.04.06，ROS版本为ROS melodic。

        Lego-LOAM依赖gtsam库，作者用的是gtsam4.0.0，但是lio-sam、lvi-sam用的gtsam为4.0.2，这里我安装gtsam4.0.2依旧可以编译运行。

        在编译时会遇到如下问题：

/usr/include/flann/ext/lz4.h:249:72: error: conflicting declaration ‘typedef struct LZ4_streamDecode_t LZ4_streamDecode_t’
 typedef struct { unsigned long long table[LZ4_STREAMDECODESIZE_U64]; } LZ4_streamDecode_t;

        解决方法是建立软链接：

sudo mv /usr/include/flann/ext/lz4.h /usr/include/flann/ext/lz4.h.bak
sudo mv /usr/include/flann/ext/lz4hc.h /usr/include/flann/ext/lz4hc.h.bak

sudo ln -s /usr/include/lz4.h /usr/include/flann/ext/lz4.h
sudo ln -s /usr/include/lz4hc.h /usr/include/flann/ext/lz4hc.h

        运行即可