论文笔记—LiPMatch: LiDAR Point Cloud Plane Based Loop-Closure

论文笔记—LiPMatch: LiDAR Point Cloud Plane Based Loop-Closure

文章摘要

Abstract—本论文提出了一种基于点云的闭环方法（closed-loop approach）用于校正基于光检测和测距（time-of-flight lidar）的同时定位与建图（simultaneous localization and mapping, SLAM）系统的长期漂移（long-term drift）。在此方法中，每个关键帧被公式化为一个完全连接的图形（graph），其中节点代表平面（planes）。为了检测回路闭合情况，我们引入了几何约束（geometric constraints）来定义相似性（similarity），从而匹配当前关键帧与地图中存在的关键帧（existing ones in the map）。经过相似性评估后，在满足几何约束条件下的候选关键帧将逐一通过平面法向约束进一步检出，并采用改进型迭代最近点算法（improved iterative closest point algorithm）进行验证。后者不仅能够估计当前关键帧与全局参考帧之间匹配的关键帧之间的相对姿势变换（relative pose transformations），还能提供较高的准确性。实验结果表明本方法能够实现快速且可靠的闭环闭合

导语

~~~~~~~闭环是实现精确的同时定位和映射（SLAM）解决方案的关键挑战之一。 由于在没有全局定位信息的情况下进行状态估计时不可避免地会发生漂移[1] – [3]，因此可靠的闭环对于许多机器人平台都至关重要。近年来，基于光检测和测距（LiDAR）的SLAM [4] – [7] LiDAR传感器能够以相当高的分辨率稳定地捕获几何特征，因此在光照和视点变化时不会像计算机视觉那样遭受太大的损失。然而，基于LiDAR的闭环仍然存在挑战 范例，因为LiDAR点云仅包含三维（3D）空间的几何信息，这由于缺少纹理和颜色而使回路闭合检测变得困难。
为了缓解基于LiDAR的闭环中存在的挑战，我们针对基于多通道LiDAR的SLAM开发了一种快速，可靠和完整的闭环，包括快速闭环检测和闭环校正，在其中利用平面及其几何关系。具体来说，我们提出了一种基于平面的关键帧的全连接图表示形式。因此，将闭环检测视为一个图匹配问题：将代表当前关键帧的图与已经集成到全局地图中的那些关键帧进行比较。为了解决这个问题，我们利用平面的几何特征及其相对几何关系。为了提高鲁棒性，我们引入了平面的法线约束。为了执行闭环校正，提出了一种改进的迭代最近点（ICP），它集成了平面约束。ICP的输出也可以有益于图形匹配结果的验证。我们提出的闭环方法的主要优点是：1）快速，可靠。 2）能够提供准确的闭环校正。
总而言之，我们的贡献包括四个方面：
1）针对多通道基于LiDAR的SLAM，我们开发了一种快速，可靠的基于LiDAR的闭环检测方法，其中有效地评估了两个关键帧的相似性；
2）在闭环校正过程中，应用匹配平面之间的几何约束条件来改进ICP方法；
3）我们将闭环检测和闭环校正方法整合到LOAM中，建立了一个完整且实用的基于LiDAR的SLAM系统；
4）通过使我们的系统在GitHub上开源，我们为基于LiDAR的闭环提供了一种负担得起的解决方案。

相关工作

~~~~~~~在SLAM框架中实现闭环检测仍面临诸多挑战。尽管已有多种方法提出解决方案并确定了主要研究方向[9] - [11]。
Bosse与Zlot [8]通过直接从点云提取关键点，并利用3D格式塔描述符对其进行特征建模。随后，在投票矩阵中基于邻居关系进行特征投票判定闭环问题的完成情况[7]。
近期研究[9]-[11]进一步提出采用全局描述符进行闭环检测的方法[9] - [11]。该方法将三维空间中的复杂场景分解为重叠区域，并结合几何特征求解全局最优匹配结果[9]-[12]。
尽管上述研究取得一定进展但现有方案仍存在不足之处：一方面局部关键点法难以有效捕捉复杂场景中的整体几何关系另一方面基于全局特征的方法又容易陷入计算冗余等问题。
针对这些问题本研究借鉴平面约束范式的基础上提出了改进型三维约束模型以适应复杂环境下的闭环检测需求。

闭环检测

在本节中, 我们对从3D点云实现闭环检测这一技术进行了详细阐述。 该系统如图2所示, 其主要组成部分包括平面提取与参数化以及图形匹配两大模块. 我们对系统各组件的具体工作原理进行了详细阐述, 如下所述

A.平面提取和参数化

参考文献[10]中所提出的思路在此基础上被采用。具体而言，在处理每个进入点云时（即每一个被采集到的空间点数据），我们首先将其转换为全局坐标系，并将其累加到当前的关键帧中（即记录系统中的重要时间点）。每隔一段时间（车辆移动一定距离），就会生成一个新的关键帧（记录系统状态的重要时间节点）。对于每一个生成的关键帧而言，在对其累积的点云数据进行处理时（即对所有已获取的空间数据进行分析），我们采用欧几里得聚类算法将其划分为若干个点簇（即空间上相互接近的一组点数据）。然而该方法需要去除地面点（即地表上的物体反射回来的数据），而我们已经通过文献[22]中所述的方法解决了这一问题。具体来说，在实际操作中我们剔除了那些包含少于100个有效点的聚类群（即那些数量不足的数据集群会被排除在外），并对其余满足条件的群落进行后续处理（即对筛选出的有效集群进行进一步分析）。最后我们应用PCL库[21]所提供的区域增长法[10]来进行平面拟合（即对空间分布规律进行建模）。平面被定义为其法向量与该平面至原始全局坐标系之间的垂直距离之和的形式表示（即用数学方式描述平面位置与方向）。满足此条件的所有三维空间点p都必须位于其上（即符合方程约束的所有空间坐标）。

每个平面P由一组几何特征描述：
n：法线向量，
c：重心，
d：到全局参考系的距离，
λ0，λ1和λ2：反映点云空间分布的特征值，
I：定义平面凸包的多边形轮廓点，
a：平面的区域。
本文中，n和d由PCL提供，c是通过平均平面上的点坐标来计算的。λ0，λ1和λ2 (λ0≥λ1≥λ2)的计算与我们以前的工作相同[1] 。 凸包I由Jarvis March算法[23]从平面上的点有效地计算，而a由多边形轮廓点l [15]计算。

B.图匹配

~~~~~~~与[15]中提取每个单帧的平面并将整个地图表示为图形的方法不同，我们提取关键帧的平面并将每个关键帧表示为完全连接的平面图。原因如下：1）[15]中的方法应用于室内环境，该环境比室外环境包含更多的平面。 2）[15]中的方法针对地图中的位置识别，而我们的方法则专注于大型室外环境中的闭环检测。 在这种情况下，使用关键帧会提高匹配效率。此外，我们的方法使用自适应参数而不是距离差来衡量相似性。 在图中，每个节点由相应平面的几何特征表示。
为了匹配从当前关键帧生成的G_c和从先前一个关键帧生成的G_p的两个图，我们采用表示为两组一元和二元约束的几何约束，使用一元约束来检查两个单图的对应关系 通过比较它们的几何特征来确定平面。二元约束用于验证G_c和G_p中的两对平面是否具有相似的相对空间位置，例如 G_c内的平面对的质心之间的距离类似于G_p内的平面对的质心之间的距离。
算法1详细介绍了图匹配过程，首先对一元约束进行验证，以得到G_c和G_p之间的候选匹配;其次，使用已经匹配的平面检查二元约束，如果满足所有约束，则两个平面之间匹配关键帧将被接受，并且递归过程将继续使用更新的参数。 探索完所有候选匹配项后，该算法完成，然后返回匹配平面L_{F,M}的列表。
尽管存在大量潜在的平面匹配问题，但大多数都无法满足一元约束。由于仅执行简单的操作（例如3D矢量和标量比较），因此评估这些约束所需的计算量很少。 经过一元和二元评估后，我们选择匹配平面数最大的关键帧作为候选关键帧，并根据平面法线和ICP的适应度评分进一步验证候选关键帧。
1）一元约束：此处介绍的一元约束旨在拒绝两个平面的不正确匹配，它们是相对较弱的约束，这意味着设置了一个相当宽松的阈值以避免拒绝正确的匹配。换句话说，一元约束应验证当两个平面的几何特征具有显着差异时，它们是截然不同的，但是它们缺乏信息来确认这两个观测值属于同一平面。 这是因为不同的平面可以具有相同的特性，例如法线。

其中λ_{10}=λ_1/λ_0，λ_{21}=λ_2/λ_1，上标c和p标记当前图形G_c和先前图形G_p，R是用于过滤不匹配项的阈值参数集。使用参数R的优点包括： 1）与使用距离差作为阈值相比，它减少了参数的数量。 2）当场景比例改变时，它增加了我们方法的鲁棒性。
2）二元约束：关于二元约束，基于[15]，我们应用二元约束对两对平面的相对位置施加几何约束。 这些约束考虑到了通过增强匹配场景的一致性来在图匹配过程中提供鲁棒性。 对匹配图中的每对平面施加三个二元约束，其中n_i^c和n_j^c是来自图G_c的一对平面的法线，同样，n_{i^{\prime}}^p和n_{j^{\prime}}^p是来自图G_p的一对平面的法线， c_i^c和c_j^c是来自图G_c的一对平面的质心，类似地，c_{i^{\prime}}^p和c_{j^{\prime}}^p是来自图G_p的一对平面的质心。

基于一元与二元约束进行评估后，在所有匹配平面上可能出现闭环不正确的情况依然存在；然而我们发现当所有匹配平面均拥有相同的法线方向时会出现故障，并因此提升了算法的鲁棒性。我们的研究重点在于进一步探索正常信息；具体而言，在当前关键帧的所有匹配平面上，在任意两个法线之间的角度最大值低于给定阈值（如公式（8）所示），则判断该关键帧下的匹配平面无法有效地区分目标以实现闭环检测

此外，我们使用等式（9）来消除两个关键帧区域之间的差异显着的情况。

其中，

A^c

代表当前关键帧中的匹配平面总面积，

A^p

代表历史关键帧中的匹配平面总面积。
当所有上述约束条件得到满足时，
我们基于匹配平面的数量n_{match}来判断是否闭合。
基于此，
我们选取具有最大匹配平面数量的关键帧作为候选闭合帧。
通常只有一个候选闭合帧；
如果有多个候选闭合帧，
则选择具有最低拟合度得分的那个。

闭环校正

~~~~~~~一旦检测到闭环，就执行闭环校正以计算两个关键帧之间的相对姿态。闭环校正的问题可以看作是当前点云与历史点云之间的配准。 该方法基于LOAM，我们使用边缘到边缘和平面到平面特征来迭代计算相对姿势。
与LOAM里程表模块中的ICP相比，两个关键帧的相对姿态变换更大，并且更有可能陷入局部最小值。 在本文中，我们利用几何特征来缓解这一问题。这里使用的几何特征是匹配平面的法线。首先，我们使用相应的法线来实现两点云的初始相对姿态变换。 初始旋转值的实现如下：

其中典型的协方差矩阵M_{cov}为基准，在奇异值分解(SVD)过程中得到产物矩阵M_v和M_u，而固定旋转矩阵M_R则由特定算法计算得出。通过引入几何约束条件来优化问题可能会有效降低陷入局部最优解的机会，并采用Levenberg Marquardt方法[24]进行求解。该过程的目标函数旨在最小化ICP匹配误差。

在第k次迭代时，在第k次迭代中，在第k次迭代期间，在第k次迭代中（此处指LOAM算法的第k次迭代），变量D^k具体指的是LOAM在该次迭代中计算得到的误差函数；变量T^k则代表了该次迭代中估算得到的相对姿态变换估计值；变量p_{ij}则代表从历史关键帧起算至当前匹配平面中的第i个点的位置坐标；变量c_i和n_i分别表示当前匹配平面中的质心坐标向量以及法向量方向向量。

校正后的条件下，在满足以下条件的情况下会被认为达到了定位一致性的目标状态：即当且仅当任意两相邻关键帧之间对应的特征点集之间的平均距离小于等于0.1米时（或者等效地，在两相邻关键帧之间对应的特征点集之间的平均距离小于等于0.1米时），则认为这两个相邻的关键帧达到了定位一致性的目标状态；否则（即当至少存在一对相邻的关键框架无法满足上述条件时），则这两对相邻的关键框架无法被视为闭合框架。

一旦确定两个相邻关键框架间存在全局一致性，则将按照文献[25]所述的方法执行姿势图优化。完成姿势图优化后... 并重新计算全局参照系中的所有点云和平面参数（文献[25]）。