F-LOAM

一、介绍

F-LOAM : Fast LiDAR Odometry and Mappin

2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)

论文下载地址：https://arxiv.org/pdf/2107.00822.pdf

代码地址：https://github.com/wh200720041/floam

今天介绍的F-LOAM是新加坡南洋理工大学的王晗(Wang Han)博士的工作，论文发表于2020年的IROS，代码开源于他的github主页。

这个工作本质上是A-LOAM的进一步整理，技术方法上基本没有太大变化，重点在于代码架构上的进一步优化，使系统的运行效率有了显著地提升。

与LeGO-LOAM相比

上期文章介绍了F-LOAM，它重点针对LOAM计算效率问题进行了优化，旨在不影响精度的情况下尽量缩减其对平台算力的需求，使之能够轻量化地部署于算力有限的无人平台上（例如：无人机、移动机器人等）。F-LOAM所采用的优化思路是舍弃LOAM中不必要的laserOodmetry线程并以匀速运动模型估计的位姿预测代替。代码实现整体与A-LOAM一脉相承，除结构优化外在实现细节方面并没有什么差异。

相比于F-LOAM， LeGO-LOAM不仅整合了LOAM的系统结构，同时对LOAM中的特征提取、位姿估计计算都进行了优化改进，此外还加入了闭环检测和全局优化，将LOAM这一LO系统构建为完整的SLAM系统，整体工作的创新性和完整性都更加突出。

二、总结

主要工作

核心要点

创新点

1. 非迭代两段式失真补偿方法对原始点云的畸变进行校正

机械式激光雷达的帧率通常在10Hz以上，一帧扫描的时间都非常短，因此可以假设运动和旋转都是不变的（短时间内的角速度和线速度恒定），也即上一帧的运动速度作为本帧运动的预测，对所有特征点进行一次去运动畸变，得到的特征点被用于位姿估计。位姿估计完成后，用得到的准确位姿重新对特征点进行去畸变，本次去畸变后的点将用于地图更新。

2. 在位姿估计的非线性优化计算过程中对特征点约束定权

F-LOAM 根据曲率为每一个特征点赋予不同的权重，角点曲率越大权重越大，面点曲率越小权重越大。

3. 摒弃了LOAM中odometry和mapping两线程并行的思路

实验过程

先后在大规模室外场景（KITTI数据集）和中等规模室内场景（实验室仓库）进行了实验，在室内场景实验前，现在 ROS-Gazebo 中进行了仿真。

选择 00-11 数据集进行了实验，涵盖了高速路/市区/郊区等场景，以 Intel i7 3.2GHz Processor为处理核心，并和 IMLS-SLAM，A-LOAM，LOAM，LeGO-LOAM，VINS-MONO，HDL-SLAM 进行了对比，以 ATE 和 ARE 作为评价指标。

实验结果

相较于LOAM的改进

结论部分作者再次重申了F-LOAM在计算效率上的提升，F-LOAM的算法创新点在于 1-非迭代两步法实现畸变补偿，2-提取强角点和强面点用于迭代位姿估计。实验部分主要讲述在KITTI数据集上的结果对比，以及在实验室仓库环境下的实验结果（含仿真），如下面所示。其实，F-LOAM 相比于众多其它激光 SLAM 的特点，或者说，作者想让大家看到的F-LOAM的优势，就是其精度和计算速度的均衡性，如下图所示，在7种对比方案中，F-LOAM 轨迹误差第二小，计算速度第三快。

整体来说，本文相比于 LOAM/LeGO-LOAM，并没有根本性差异，算是 LOAM 在提升实时性方面的一个工程改进，考虑到代码很早就已经开源，想来这次发paper更像是补作业，倒也无需详细读三遍。

从策略上讲，F-LOAM废除了LOAM/LeGO-LOAM中高频+低频里程计结合的方式，在一帧数据的整个处理周期中，有效减少了参与迭代的点数、和迭代优化的次数，从而提升实时性。

相较于LOAM的第一个区别：采用两步法对原始点云的畸变进行校正，第一步在位姿计算中进行，第二步在点云地图的保存和可视化时进行。
相较于LOAM的第二个区别：在位姿估计的非线性优化计算过程中对特征点约束定权，即根据特征点平滑度(smothness)的大小确定特征匹配点对在非线性优化中的作用，特征越明显，其对优化计算结果的影响越大。
相较于LOAM的第三个区别：摒弃了LOAM中odometry和mapping两线程并行的思路，将odometry的scan-to-scan的匹配移除，仅保留scan-to-map的匹配。因此，mapping线程不再进行优化计算，仅处理点云地图的保存和可视化等工作。

三、论文精读

Abstract

同时定位与建图（SLAM）在机器人领域有着广泛的应用，如自动驾驶和无人驾驶等领域。对于一个好的 SLAM 系统来说，其计算效率和定位精度都至关重要。现有的基于激光雷达的 SLAM 研究通常将问题描述为两个模块：点云帧到点云帧匹配和点云帧到地图的精细化匹配。这两个模块都是通过迭代计算来求解的，计算量很大。

在本文提出了一个通用的解决方案，旨在为基于激光雷达的 SLAM 提供一个计算效率高且精度准的框架。具体而言，我们采用了一种非迭代的两段式失真补偿方法来降低计算量。对于每个输入的扫描帧，提取边缘和平面特征，分别匹配到局部边缘图和局部平面图，其中局部平滑度被考虑用于迭代姿态优化。

通过深入的实验评估了其在具有挑战性的场景中的性能，包括仓库自动导引车（AGV）的定位和自动驾驶的公共数据集。该方法在公共数据集评估中以 10Hz 以上的处理速率获得了具有竞争力的定位精度，为实际应用提供了性能和计算成本之间的良好折衷。该方法目前是 KITTI 数据集排名中最准确、最快的开源 SLAM 系统之一。

I. INTRODUCTION

激光 SLAM 的一些局限性

提出当前激光 SLAM 在实际应用中仍存在一些局限，例如：

不同场景下的鲁棒性。从室内到室外，从静态到动态。
车载平台计算资源有限。车载处理单元要同时执行高频率的定位和路径规划。嵌入式平台上计算消耗过大导致实时性不足。
许多作品没有开源，很难复现相同的结果。

计算量消耗大的原因

作者进一步指出，计算消耗大的原因主要在于估计两个点云帧之间变换时点的数量过多和迭代计算量大。

估计两个点云帧之间变换的最经典方法是迭代最近点（ICP），其中两个点云帧通过最小化点云距离迭代对齐。优化过程中涉及大量的点，计算效率低下。

LOAM 通过提取边缘和平面特征，并通过最小化点到平面和点到边缘的距离来计算姿态。该方式大大减少了点的数量，提升了匹配计算效率，但畸变补偿和激光里程计仍需要依赖高昂计算量的迭代计算。

本文工作

本文提出了一种轻量化的实时激光雷达 SLAM 方案，该框架包括了特征提取、畸变补偿、位姿优化和建图四个部分，能够在低功耗的嵌入式计算单元上实现高达 20Hz 的实时性能。

与传统方法相比，本文采用了非迭代的两段式失真补偿方法来代替计算效率低下的迭代失真补偿方法。

对于匹配非常重要的具有较高局部平滑度的边缘特征和具有较低平滑度的平面特征通常是在连续扫描帧中一致提取的。为了提高定位精度，在迭代姿态估计中还考虑了局部几何特征。

为了证明该方法的鲁棒性，对该方法进行了全面的评价，包括室内和室外实验。与现有的最新方法相比，该方法能够以较低的计算代价获得具有竞争力的定位精度，这是性能和速度之间的一个很好的折衷。值得注意的是，所提出的方法是KITTI基准中最准确和最快的开源方法之一。

本文组织结构

第二节回顾了现有激光雷达SLAM方法的相关工作。第三节描述了所提出方法的细节，包括特征点选择、激光点对准、激光里程计估计和建图。第四节展示了实验结果，第五节给出结论。

图1：所提方法在KITTI数据集中的示例。其中（a）展示了序列05上的建图结果。（b）是通过整合相机视图重建的3D道路场景。（c）根据F-LOAM和地面实况绘制轨迹。

点云配准

激光 SLAM 中最核心的环节是点云配准，目前主要利用点云之间的对应关系，包括两大类：原始点云配准和特征点配准。

原始点云配准

对于原始点云匹配，最经典的方法是迭代最近点（ICP），通过在欧几里德空间中找到最近的点来测量对应点。通过迭代地最小化对应点之间的距离残差，两个点云之间的姿态转换收敛到最终位置。

改进的点云匹配 IMLS-SLAM 通过为每个点分配权重值，来提升配准精度。基于目标点的局部曲面法线，采用 IMplicit Least Square（IMLS）求出每个权重。然而，应用原始点云匹配通常计算成本很高，例如，在 IMLS-SLAM 中估计一帧需要1.2秒，还远达不到实时性能的要求。

特征点配准

由LOAM引入的基于特征的方法是特征点配准最主流的做法。主要利用点对面/边缘的匹配，

基于局部平滑度分析预先提取出边缘点和面点，并将当前点云中的点与地图中的边缘特征和平面特征进行匹配，其中距离成本为点到线、点到曲面之间的欧氏距离。此外，LeGO-LOAM 在特征提取之前分离了地面优化，该方法广泛应用于地面车辆，可以获得更好的性能。

对于自动导航车辆（AGV）来说，z轴并不重要，大多数情况下，我们可以假设机器人在2D空间中移动。因此，地面点对x-y平面的定位没有贡献。

额外模块

一些工作试图通过引入额外的模块来提高性能。

回环检测

回环检测是SLAM中的另一个关键组件。在一些大规模场景中，后端中的回环检测常用于识别重复的位置，进而校正里程表漂移误差。

在仅激光测距和定位（LOL: Lidar-only Odometry and Localization in 3D point cloud maps）中，作者将激光测距扩展为具有环路闭合的全SLAM。

多传感器融合

与惯性测量单元（IMU）融合是一种替代解决方案。

Shan等人提出了一种名为 LIO-SAM 的 IMU 和 GPS 的融合方法。不同的传感器同步并紧密耦合地输入。

在室外环境中，融合SLAM系统被证明比LOAM更准确。

基于融合的方法可以提高定位的精度，但多传感器的融合需要同步和全面校准。

深度学习

在过去几年中，也有一些与深度学习相关的工作试图将卷积神经网络（CNN）用于点云处理。与手工制作的特征不同，特征是通过CNN训练提取的。

深度学习的方法受限于特征必须要事先学习。

例如在 CAE-LO 中，作者通过无监督卷积自动编码器（CAE）提出了基于深度学习的特征点选择规则，而激光雷达测程估计仍然基于特征点匹配。实验结果表明，学习的特征可以提高匹配成功率。

尽管深度学习方法在公共数据集中表现良好，但在室内/室外和城市/农村场景等不同环境中，其鲁棒性并不强。

III. METHODOLOGY

在本节中，将详细介绍提出的方法。首先介绍了传感器模型和特征提取。然后用失真补偿对提取的特征进行校准。然后，我们给出了一般特征提取和全局特征估计。最后，我们解释了激光里程计的计算以及建图。

A. Sensor Model and Feature Extraction 传感器模型&特征提取

传感器模型

机械式三维激光雷达通过旋转一个尺寸为M的垂直排列的激光束阵列来感知周围的环境。它用M个平行读数扫描垂直面。在每个扫描间隔期间，激光阵列在水平面上以恒定速度旋转，同时激光测量按顺时针或逆时针顺序进行。

对于机械式激光雷达传感器模型，我们将第 $k$ 次激光雷达扫描帧表示为 $\mathcal{p}_{k}$ ，其中每个点表示为 $p_{k}^{(m, n)}$ ，其中 $m$ 和 $n$ 分别表示点云的行号和列号。

特征提取

原始点云匹配方法（如ICP）对噪声和动态对象（如自动驾驶的人）敏感。此外，激光雷达扫描包含数以万计的点，这使得ICP计算效率低下。

与ICP等原始点云匹配方法相比，特征点匹配在实际应用中更加鲁棒和高效。为了提高匹配精度和匹配效率，这里利用点云数据的表面特征和边缘特征，同时丢弃那些有噪声或不太重要的点。如上所述，3D机械激光雷达返回的点云在垂直方向上是稀疏的，在水平方向上是稠密的。因此，水平方向的特征更加明显，在水平方向上进行错误特征检测的可能性较小。

这一部分与 LOAM 中所采用的方法完全一致。

对于 $t_k$ 时刻的点云 $\mathcal{p}_k$ 中的每一点 $p_{k}^{(m, n)}$ ，通过下面公式来评估局部曲面的平滑度 $\sigma_{k}^{(m, n)}$ ：

$\sigma_{k}^{(m, n)} = \frac{1}{\left| S_{k}^{(m, n)} \right|} \sum_{p_{k}^{(m, j)} \in S_{k}^{(m, n)} }{\left( \left\| p_{k}^{(m, j)} - p_{k}^{(m, n)} \right\| \right)}$

其中 $S_{k}^{(m, n)}$ 是 $p_{k}^{(m, n)}$ 在水平方向上（同行）的相邻点， $\left| S_{k}^{(m, n)} \right|$ 是局部点云中的点数（相邻点个数）。 $S_{k}^{(m, n)}$ 可以基于点的ID n来很容易地收集到，这与局部搜索相比可以降低计算成本。

在实验中，我们分别沿顺时针和逆时针方向选取5个点。

对于墙等平面，平滑度值较小，而对于角点或边点，平滑度值较大。对于每个扫描平面 $\mathcal{p}_k$ ，用高 $\sigma$ 选择边缘特征点，用低 $\sigma$ 选择表面特征点。因此，我们可以将边缘特征集表示为 $\mathcal{E}_k$ ，将表面特征集表示成 $\mathcal{S}_k$ 。

注意LeGO-LOAM中，角点/面点有强弱之分，弱面点其实是所有原始点里边排除掉强角点、弱角点、强面点之后剩下的所有点，这样，弱面点数量其实很多，相当于“所有非特征点”。但在 F-LOAM 中，不会用这样的排除法，也不会对特征点分出强弱。提取出的角点/面点，都是通过计算曲率然后取最大/最小得到的真正的特征，因此，F-LOAM中用于配准的特征点数量更少。

B. Motion Estimation and Distortion Compensation 运动估计&畸变补偿

在 LOAM 和 LeGO-LOAM 等现有方案中，通过点云帧对点云帧匹配来校正失真，该匹配方法迭代地估计两次连续激光扫描点之间的转换。然而，要找到变换矩阵需要迭代计算，计算效率很低。在本文中提出使用两级失真补偿来降低计算成本。

请注意，大多数现有的3D激光雷达能够以 10Hz 以上的频率运行，两次连续激光雷达扫描之间的时间通常非常短。因此，可以先假设短时间内的角速度和线速度恒定，来预测运动并校正畸变。在第二阶段，在姿态估计过程之后，将重新计算畸变，并将重新计算的未畸变特征更新到最终的地图。

在实验中，我们发现两级失真补偿可以达到相似的定位精度，但计算成本要低得多。

C. Pose Estimation

位姿估计将当前未失真的边缘特征和平面特征与全局特征地图对齐。全局特征地图由边缘特征和平面特征组成，分别进行更新和维护。

为了减少搜索的计算量，边缘特征地图和平面特征地图都存储在 3D KD-trees 中。通过从边缘和平面特征图中收集附近的点来估计全局线和平面。对于每个边缘特征点，从全局边缘特征图中计算其附近点的协方差矩阵。当点分布在一条直线上时，协方差矩阵包含一个更大的特征值。将与最大特征值相关的特征向量视为直线方向，直线的位置视为附近点的几何中心。类似地，对于每个平面特征点，这里可以得到一个具有位置和曲面范数的全局平面。注意，与全局边缘不同，全局平面的范数作为与最小特征值相关的特征向量。

位姿估计同样是优化特征点的 point-to-line/surface 误差，并没有什么不同。唯一区别在于，F-LOAM 根据曲率为每一个特征点赋予不同的权重，角点曲率越大权重越大，面点曲率越小权重越大。

传统的特征匹配只优化上述几何距离，而没有考虑每个特征点的局部几何分布。然而，可以观察到，具有较高局部平滑度的边缘特征和具有较低平滑度的平面特征通常在连续扫描中被一致地提取，这对于匹配更为重要。因此，引入了权重函数来进一步平衡匹配过程。为了降低计算成本，重新使用先前定义的局部平滑度来确定权重函数。

论文中叙述的根据各点平滑度定权的策略并未在开源的代码版本中体现，仅仅只是采用了Huber函数作为损失函数。

D. Mapping Building & Distortion Compensation Update

全局地图由边缘特征地图和平面特征地图组成，并基于关键帧进行更新。当平移变化大于预定义的平移阈值或旋转变化大于预定义的旋转阈值时，将选择作为关键帧。与逐帧更新相比，基于关键帧的地图更新可以减少计算量。如上所述，为了降低计算成本，基于恒定速度模型而不是迭代运动估计来执行失真补偿。

然而，这不如LOAM中的迭代失真补偿准确。因此，在第二阶段，根据最后一步的优化结果重新计算失真。

重新计算的未失真边缘特征和平面特征将分别更新为全局边缘图和全局平面图。每次更新后，通过使用3D体素化网格方法对地图进行下采样，以防止内存溢出。

一句话总结该部分：运动过滤筛选关键帧，关键帧去运动畸变后更新到地图中。此处的去运动畸变即 Section B 中的第二次去畸变。

IV. EXPERIMENT EVALUATION

先后在大规模室外场景（KITTI数据集）和中等规模室内场景（实验室仓库）进行了实验，在室内场景实验前，现在 ROS-Gazebo 中进行了仿真。

A. Experiment Setup 实验设计

为了验证该算法，在大规模的室外环境和中等规模的室内环境下对 F-LOAM 进行了评估。对于大规模实验，在 SLAM 评估中最流行数据集之一的 KITTI 数据集上评估了所提出的方法。然后将该算法集成到物流仓库中，首先在模拟仓库环境下进行了验证，然后在AGV平台上进行了测试。

B. Evaluation on Public Dataset KITTI数据集实验

首先在广泛用于户外定位评估的KITTI数据集上测试了我们的方法。数据集是从一辆装有Velodyne HDL-64激光雷达、摄像头和GPS的驾驶汽车上收集的。大多数最先进的SLAM方法都是在此数据集上进行评估的，例如 ORB-SLAM、VINS Fusion、LIMO 和 LSD-SLAM。

为了验证算法的鲁棒性，我们在 KITTI 数据集的所有序列上评估了所提出的算法，该数据集包括高速公路、市中心、县道、住宅区等不同场景。序列11-21的地面实况不对公众开放，因此，我们主要展示了在序列00-11上的性能。

我们计算由 KITTI 数据集定义的平均平移误差 Average Translational Error（ATE）和平均旋转误差 Average Rotational Error（ARE）。

结果如图2所示。我们的方法在11个序列中实现了 0.80% 的平均平移误差和 0.0048 deg/m 的平均旋转误差。

图2：KITTI数据集上运行F-LOAM的结果。估计的轨迹和地面实况分别以绿色和红色绘制。其中（a）-（e）为序列00-04，（f）-（j）为序列06-10。

我们还将我们的方法与最先进的方法进行了比较，包括激光雷达 SLAM 和视觉 SLAM 。在11个序列中行驶长度为22公里，总共有23201帧。还记录了所有序列的平均计算时间。

为了准确评估计算时间，所有提到的方法都是在基于 Intel i7 3.2GHz 处理器的计算机上进行的测试。

计算从特征提取开始到里程计估计输出的时间。将所提出的方法与最先进的激光雷达开源方法进行比较，如LOAM、A-LOAM、HDL-Graph-SLAM、IMLS-SLAM 和 LeGO-LOAM 。注意，IMLS-SLAM 不是开源的，其结果是从原始论文中收集的。此外，还比较了 VINS-MONO 等视觉 SLAM。为了保持一致性，不使用 IMU 信息，并删除环路闭合检测。测试环境基于 ROS Melodic 和 Ubuntu 18.04 。结果如图3所示。

KITTI数据集序列00-10上使用不同定位方法的比较

图3：KITTI数据集序列00-10上使用不同定位方法的比较。

其中 IMLS-SLAM 达到了最高精度，因为其在迭代姿态估计中使用了所有点，故处理速度较慢。

就计算成本而言，LeGO-LOAM 是最快的激光雷达 SLAM ，因为它只对非地面点进行优化。

在所有比较的方法中，我们的方法在平均处理速率超过 10Hz 时达到了第二高的精度，这是计算成本和定位精度之间的一个很好的折衷。我们的方法能够为实时机器人应用提供快速且准确的 SLAM 解决方案，在最先进的方法中具有竞争力。

C. Experiment on warehouse logistics 实验室仓库实验

在本实验中，我们的目标是建立一个自主的仓储机器人来取代人工主导的制造业。AGV设计用于执行日常任务，如运输。这就要求机器人平台在复杂的环境中进行主动定位。

Simulation 仿真环境

1）仿真环境：首先在一个仿真环境中验证我们的算法。仿真环境建立在 Gazebo 和 Linux-ubuntu18.04 上。如图4（a）所示，我们使用虚拟先锋机器人和虚拟 Velodyne VLP-16 作为地面车辆平台。模拟环境重建了一个复杂的仓库环境，包括各种对象，如移动的工人、货架、机器等。机器人由操纵杆控制，以 2m/s 的最大速度移动。我们记录 F-LOAM 的轨迹，并将其与地面真值进行比较。结果如图4（c）所示。F-LOAM 轨迹和地面真值分别以绿色和红色绘制。我们的方法能够在有人工操作员在仓库环境中行走的动态环境下跟踪AGV的高速移动。

图4：具有静态和动态对象的模拟仓库环境。其中(a）为露台模拟环境，(b）为进行评估所用的模拟先锋机器人和Velodyne激光雷达，(c）为F-LOAM与地面真值的轨迹比较。

Experiment 实际仓库环境

2）实验：为了进一步证明我们方法的性能，我们在一个用于智能制造的实际 AGV 上实现了 F-LOAM 。如图5（b-e）所示，仓库环境由三个主要区域组成：自动充电站、物料搬运区和制造站。一个完全自主的工厂需要机器人将材料运送到制造机器上进行组装和收集产品。所有这些操作都需要精确定位，以确保可靠性和安全性。用于测试的机器人平台如图5（a）所示，配有 Intel NUC 微型计算机和 Velodyne VLP-16 传感器。定位和建图的结果如图5所示。在这种情况下，机器人自动探索仓库，并使用所提出的方法同时构建地图。

图5：仓库环境中的F-LOAM。(a）为用于实验的自动引导车。(b-e）是为 AGV 操作而搭建的先进工厂环境，包括操作机器、自动充电站和存储架。中心图像：仓库定位和建图的F-LOAM结果。

Performance evaluation 性能评估

3）性能评估：该方法也在装有 VICON 系统的室内进行了测试，以评估其定位精度。机器人被远程控制在测试区域内移动。结果如图6所示，其中 F-LOAM 轨迹和地面真实轨迹分别以绿色和红色绘制。可以看出，我们的方法可以精确地跟踪机器人的姿态。与 VICON 系统提供的地面真值相比，它的平均定位精度达到了 2cm 。

图6：所提出的方法与地面真值的比较。该方法能精确跟踪机器人的位姿，平均定位误差为2cm。

Ablation study 消融实验

4）消融实验：为了进一步评估提出的失真补偿方法的性能，我们在仓库环境中比较了不同方法的结果。我们首先去除了 F-LOAM 中的失真补偿，并记录计算时间和定位精度。然后我们将 LOAM 中的迭代失真补偿方法添加到我们的方法中，并记录结果。最后，我们使用所提出的运动补偿并记录结果。

表一：定位精度和计算成本的消融实验。

结果如表I所示。可以看出，所提出的方法比具有运动补偿的 LOAM 快得多，且定位精度也稍好一些。

V. CONCLUSION

本文提出了一个计算效率高的 LiDAR SLAM 框架，旨在为计算资源有限的机器人应用提供一个公共的解决方案。

与传统方法相比，我们提出使用非迭代两级失真补偿来减少计算量。

另外，在连续扫描点云帧过程中，通常会提取局部平滑度较高的边缘特征和平滑度较低的平面特征，这对于扫描帧与地图的匹配更为重要。因此，在迭代姿态估计中也考虑了局部几何特征。

为了验证该方法在实际应用中的鲁棒性，进行了深入的实验来评估其性能，包括仿真、室内 AGV 测试和室外自主驾驶测试。我们的方法在室内测试中的平均定位精度达到了 2cm ，是 KITTI 数据集中最准确、最快的开源方法之一。