【LLOAM翻译，回环检测】LLOAM: LiDAR Odometry and Mapping with Loop- closure Detection Based Correction

LLOAM: LiDAR Odometry and Mapping with Loop-closure Detection Based Correction
是2019年 IEEE ICMA 上的一篇文章。
论文链接：https://pan.baidu.com/s/1J5aLexKyQQUaTWIctNBMig.
提取码：26la
【摘要】
标准的SLAM系统分为3部分：前端里程计，回环检测，后端的全局优化。文章提出了基于回环检测的点云分割匹配前端和基于图优化的后端，从而建立了一个完整的3D LiDAR SLAM 系统。

【一、介绍】（背景部分跳过）

提了一下回环检测的好处，消除累计误差。系统介绍：
前端有两部分工作，
1.特征点提取和帧间匹配，得到相邻帧之间的相对姿态变换，如果有IMU数据可以用IMU数据来线性插值从而校正运动失真。
2.分割当前点云，然后使用形状描述符描述分割的点云簇并将其保存到本地地图。本地地图中的分割之后的点云段被用作回环检测中的目标点云，即通过当前获得的点云与目标点云作匹配来检测环路是否闭合，从而让Lidar知道自己之前来过这里。
后端基于前端计算出的帧间相对转换和一系列的位姿构建因子图？？（什么是因子图）
当前端检测到闭环时，它将计算两个点云之间的相对转换，并将其作为约束发送到后端。在约束条件下，后端将优化相关的位姿，并进一步校正地图。

【二、相关工作】

文献[1]概述了SLAM在过去十年的工作，视觉SLAM产生了环路闭合、前端和后端的概念。在这里，我们只介绍了KITTI最著名的SLAM评估数据集的前三位作品和最近两年基于3D激光雷达的SLAM的进展，它们代表了最先进的技术水平。

在以往基于激光雷达的里程测量方法中，LiDAR里程测量与测绘（LOAM）[5]取得了最好的精度。它专注于激光雷达扫描中的边缘和平面特征，并将它们作为直线和平面保存在地图中，以便进行点对线和点对平面的匹配。在LOAM的基础上，文献7增加了以高频（60HZ）运行的视觉模块来估计运动，并增加了以低频（1HZ）运行的激光雷达里程计来细化运动估计[7]。Deschaud[8]提出了一种扫描到模型的匹配方法，通过最小化当前点到隐式移动最小二乘（IMLS）表示的曲面的距离来完成姿态估计，但这种方法不是实时的。Serafin等人[9] 提出了一种基于特征的点邻域主成分分析（PCA）方法，对稀疏点云具有良好的效果。Droeschel等人[10] 提出了一种基于高效局部映射和层次优化后端的连续时间SLAM方法。这些方法在局部估计方面取得了很高的精度，但它们没有考虑环闭合检测，也没有在室外远距离环境中评估全局一致性。

总结就是描述了之前一些工作的不足如实时性低、基于Lidar的SLAM做回环检测的少。

【三、方法描述】

如下图，前端包括两个并行的子模块，第一个子模块用于特征点提取、帧间匹配、校正和建图（在此输出的频率是1Hz），第二个子模块用于对点云进行分割和描述，最后做回环检测（在此输出的频率是1Hz），两个子模块都是以10Hz频率处理点云数据。后端是用于存储历史位姿和点云数据，然后增量显示轨迹的优化模块。

A. 系统概述

SLAM的核心是通过前端以10Hz频率接收处理3D点云信息，估计LiDAR自身在世界坐标系的运动，同时，根据雷达位置，在LiDAR自身坐标系上收集的点云被注册到世界坐标系来构建遍历环境的地图。

机体坐标系L，XYZ的方向分别是右下前。在 tk 时刻，雷达扫描的点云 i （单个点？）在机体坐标系下表示为，世界坐标系W，为起始位置。已经存储的地图点 i 在世界坐标系下描述为。L相对于W的位姿变换，描述为。xyz为位置，对应θ角为绕该轴的旋转角。（注意这里的xyz是世界坐标系下的）。

B. 雷达里程计和建图

LiDAR每帧的扫描包含数百万个点，根据LiDAR的扫描方法，首先需要按空间和时间顺序记录这些点以进行遍历。由于雷达扫描的时候在运动，在第k个sweep期间，雷达所获得的点并不都在坐标系Lk下。而IMU数据采集频率非常高，可以将其内插到点云中来纠正运动失真（怎么插的，IMU不应该测出来的是姿态信息么）。里程计和建图算法参考的文献[20]，因为它把用LiDAR做里程计的精度达到了极限。

根据扫描线的空间顺序从上到下逐行提取特征点，并按时间顺序从前到后提取特征点。在本文中，我们略微简化了它，以满足计算的实时性要求。设 i 为 tk 时刻获得的点云 Pk 中的一个点，S 表示点 i 前后各5个点（一共10个点）的集合（这里的前后应该是指程序中把点记录到数组里之后的前后关系）。通过如下公式来表征点 i 附近的局部表面光滑度，即计算这10个点与 i 的差值之和的范数。

根据点云中点的时间戳把点云 Pk 分为4部分，每一部分根据上面算出的 c 值排序，然后从中挑选出具有最大 c 值的2个点作为角点，最小 c 值的4个点作为（组成特征平面的）平面点。

为了确保特征点有一定的稳定性和鲁棒性，即即使在存在外部噪声时，用这种方法在大多数的帧里都能提取到特征点，在提取特征点时，作者进行了一些剔除策略。首先，仅当点 i 的 c 值大于或小于阈值时，才可以将其选择为角点或平面点。其次，不再考虑先前选择的特征点周围的点作为特征点。第三，为防止噪声点的干扰，不考虑太近的点（这个太近指的和谁近？距离如何测得？）。最后，通过计算 i 点、LiDAR坐标点和 i 点前后两个相邻点形成的两个三角形的边长，消除了与激光束平行的曲面上的点和被遮挡区域边界上的点。（这个求三角形边长具体怎么消除的？原理？）

相邻扫描线上的两个角点形成一条边缘线，三个平面点形成一个平面。设 dc 为 t(k+1) 时刻角点到 tk 时刻边缘线的距离，dp 为 t(k+1) 时刻平面点到 tk 时刻 平面的距离。根据 tk 时刻到 t(k+1) 时刻之间的LiDAR姿态变换，可以把 t(k+1) 时刻的点可以投影到kt。然后根据点到线和点到平面的公式得到一个非线性函数，如下所示：

假定激光雷达的运动是均匀的，的初始值可以通过线性计算得到。每一个特征点都可以根据公式（2）得到一个 d，然后叠加成 d，通过非线性迭代，如通过将 d 最小化为0的LM方法可以求解

上式中 是雅克比矩阵，λ 是迭代步长系数。一系列的相乘就是激光雷达在世界坐标系下的姿态。

Pk+1 时刻的点云在下采样之后通过注册到地图里。之前已经提取到的特征点也保存到地图里。利用最邻近搜索算法，将当前时刻提取的特征点和之前构建的地图中的特征点做匹配。如果有足够多的好的点对，则使用与（2），（3）相同的算法来校正累积误差。近邻搜索会消耗计算资源，特别是当局部地图的尺度较大时，为了保证算法的实时性，校正算法的频率为1Hz。

C. 基于回环检测的点云分割

基于回环检测的点云分割过程类似于文献[17]中的方法，这是第一个应用于基于3D激光雷达的SLAM的实时分割匹配算法，并且其模块化结构易于移植和二次开发。

原始输入点云首先需要用体素网格表示，以滤除噪声点。然后通过去除垂直方向的均值和方差满足预设阈值的体素来去除地平面。剩余的点云可以通过European clustering（这是个什么聚类方法？）得到一系列的点云段 Ci 。

每一个点云段 Ci 都需要特征提取来存储以及随后的识别和分类。提取的点云段特征由特征向量 fi = [ f1 f2 …… fn ] 表示，每个元素表示一个局部或全局描述方法（这里应该指的是描述局部或者全局的地图？）。文献[26]提供了21个描述符，其中7个基于特征值，描述了点云的局部线性、平面性、散射、各向异性、全方差、特征熵和曲率变化。通过计算节段内各点与其重心的差，得到节段的3D结构张量及其特征值λ1、λ2、λ3。 7种描述符如（4）所示，其中 ei 代表归一化的特征值。

文献[27]提供了10个直方图描述符，由3个形状函数D2、D3和A3计算得出。D2表示随机选择的两点之间的距离。D3表示由随机选择的三个点组成的三角形区域。A3把随机采样三个点产生的两条线形成的角度进行编码（编码干嘛？？）。

集成学习算法随机森林由于使用方便、能处理高维特征、在大型数据库上具有良好的定时性能，在此用于在当前点云段的源点云中寻找对。在发送分类器之前，首先根据特征空间中的邻域选择候选匹配对以保持效率。最后，在随机森林中随机抽样生成的决策树选出优胜类。用计算出的两段特征向量之间的绝对差，与阈值进行比较，以确保匹配的准确性。根据匹配对的重心，使用与第 III-B 节相同的方法计算变换，然后将其作为约束反馈给后端。图3显示了点云分割和匹配的结果。

D. 图优化

SLAM问题本质上是状态估计问题。除了基于过滤器的传统方法外，图优化作为一种基于平滑的方法，已广泛用于视觉SLAM。有许多成熟的优化库可用，例如g2o，Google的Ceres Solver和GTSAM。在本文中，我们使用基于要素图优化方法的GTSAM 4.0，并且生成的图占用更少的存储空间。当添加新节点或找到闭环时，它使用基于贝叶斯树的iSAM2方法执行增量推理，并且仅优化与闭环关联的局部子图，从而可以保证在大地图如户外时算法的实时性。

因子图优化是一种基于概率和图论的优化方法。图中的节点代表在前端获得的姿势和位置，节点之间的边表示获得的约束。如图所示，节点 xk 为 tk 时刻的LiDAR姿态，lk 为地图中的观测段，因子为两个节点之间的转换。由于激光雷达的运动和观测含有随机噪声，因此图中的节点是服从柯西分布的随机变量。优化过程是通过调整节点的值使概率图模型的联合概率最大化。优化方法是常用的非线性优化方法，如高斯-牛顿法或L-M法。当通过观察检测到环路闭合时，例如，在图4（b）中，通过观察1l和2l，6x的值应与1x相同，然后建立1x、6x和5x之间的约束，并校正一系列节点。

…………