论文翻译——BoW3D:用于3D激光雷达SLAM回环检测的实时词袋模型

---

摘要

环路闭合是自主移动系统SLAM的基本组成部分。 在视觉SLAM领域，Bag of Words（Bow）在环路闭合方面取得了巨大成功。用于闭环搜索的BoW特性也可以用于随后的6自由度环校正。然而，对于3D激光雷达SLAM，目前最先进的方法可能无法有效地实时识别环路，而且通常无法纠正完整的6自由度环路姿态。了解决这一限制，本文提出了一种在3D激光雷达SLAM中用于实时闭环检测的新颖的Bag of Words，称为BoW3D。该方法不仅有效地识别了重复的环路位置，而且实时校正了完整的六自由度环路位姿。BoW3D基于3D激光雷达特征LinK3D构建bag of words，该特征高效、姿态不变，可用于精确的点对点匹配。我们进一步将本文方法嵌入到3D激光雷达里程计系统中，以评估闭环检测性能。我们在公共数据集上对该方法进行了测试，并与其他最先进的算法进行了比较。在大多数情况下，BoW3D在F1 max和扩展精度分数方面表现更好。值得注意的是，BoW3D在Intel Core i7 @2.2 GHz处理器的笔记本上运行时，平均需要48毫秒来识别和纠正kitti 00上的闭环(包括4K+ 64射线激光雷达扫描)。

关键字——词袋，LinK3D特征，闭环检测，位置识别，实时。

---

一、引言

长期同步定位和建图(SLAM)的基本要求之一就是快速且鲁棒的闭环检测。标准的帧对帧点配准算法在长期运行后，由于位姿估计存在较大的漂移(drift)，可能无法将当前观测结果与重新访问的位置对齐。当可以鲁棒地识别出重游位置时，闭环检测可以提供正确的数据关联，从而消除漂移，得到更全局的一致性估计。同样的环检测方法也可用于存在跟踪故障情况下的机器人重定位。位置识别的基本技术是根据机器人在线采集的图像(用于视觉SLAM)或点云(用于激光雷达SLAM)建立数据库，以便在获取新的传感器观测数据时能够检索到最相似的。当检测到相同的场景时，将执行闭合回路。不可否认的是，基于距离的关联也可以用于在一定范围内漂移的环路闭合。然而，当应用于长期大规模的情况下，位姿漂移在很大程度上积累，基于距离的关联无法找到当前传感器帧与历史帧之间的正确对应关系。因此，SLAM系统需要一个快速且鲁棒的闭环模块。

在视觉同时定位与建图领域，很多算法[1]、[2]、[3]、[4]、[5]、[6]、[7]、[8]都使用了图像检索技术。他们通常通过比较2D特征(BoW -of-words[9])来匹配图像。BoW实现了非常高效的图像检索，促进了视觉SLAM的发展。遗憾的是，在三维激光雷达同时定位与建图领域，激光雷达点云的不规则性、稀疏性和无序性对三维特征提取和表征[10]、[11]提出了挑战。构建用于3D特征的词袋模型，将其应用于3DLiDARSLAM中实时闭环检测和纠正完整的6自由度闭环姿态，这些仍是问题。

在本文中，我们提出了一种新颖的闭环方法，该方法为激光雷达点云提取的3D特征构建词袋模型。我们使用LinK3D[10]作为3D特征，构建了3D LiDAR点云的BoW，命名为BoW3D。为了实现快速检索，使用哈希表作为数据库的基本结构。所提出的检索和更新算法可以在线执行。找到候选帧后，利用精确的点到点Link3D匹配结果，用RANSAC[12]和SVD[13]计算出完整的6自由度环位姿。此外，我们将我们的BOW3D与公共数据集上的最新方法进行了比较，并将其嵌入到激光雷达里程计A-LOAM[14]中，以评估其在实际应用中的性能。实验结果表明，该方法取得了明显的改进，具有较好的实时性。

综上所述，我们的主要贡献如下:
•我们提出了一种基于词袋的方法，利用三维激光雷达的特征来实现激光雷达的闭环。它建立了LinK3D特征数据库，可以实时有效地识别重访的地点。
•所提出的BoW3D算法还可以对整个6自由度的闭环姿态进行实时校正，为后续在线运行的位姿图优化提供了精确的闭环约束。
•该方法已在激光雷达里程计系统中得到了应用。实验结果表明，该方法能有效消除漂移，提高三维激光雷达SLAM的精度。

图一。我们系统的工作流程主要由三个模块组成：（i）特征提取; (ii) A-LOAM的里程计和建图;（iii）闭环。我们将BoW 3D嵌入到环路闭合的线程中，该线程闭合环路并获得全局一致的SLAM结果。

---

二、相关工作

环路闭合通常包含两个步骤。它首先在数据库中找到与当前观测结果相似的位置，也称为位置识别。然后估计环路的位姿，通过位姿图优化对估计的位姿进行校正。许多不同的传感器模式已被用于寻找环路，包括相机图像和3D激光雷达点。

近年来，提出了多种基于图像的方法[1]、[4]、[5]、[15]、[16]、[17]。一个很好的例子是FAB-MAP[18]，它使用树形结构来学习离线词的协同概率。但是由于它依赖于SURF[19]，在特征提取上花费了大量的时间。DBoW2[4]用于基于 BRIEF 描述符[20]的第一次二进制词汇，使用非常高效的 FAST 特征检测器[21]，大大提高了循环检索的效率。由于其高效性，DBOW2已成功地应用于几个著名的视觉SLAM系统[5]、[6]、[7]、[8]、[22]、[23]中。值得注意的是，基于DBoW2的特征点在长期视觉SLAM中集成后，可用于后续的环路闭合和优化。将词袋应用于视觉同时定位与建图的成功有三个主要原因:(i)在视觉任务中成功使用了许多2D特征，如SIFT[24]、SURF[19]、BRIEF[20]和ORB[25]。(ii)相机SLAM通常提取2D特征，需要检测环路或进行重定位。因此，单词袋非常适合视觉SLAM。(iii)词袋将图像资料压缩成更紧凑的形式。此外，该算法还通过构建树来加速检索过程。这都保证了该算法在视觉SLAM位置识别中的有效性。

对于使用3D点云的方法，Steder et al.[26]提出了一种基于3D Lidar 数据生成的距离图像的位置识别方法，该方法结合使用了词袋模型和基于NARF特征模型[27]。M2DP[28]将点云投影到多个2D平面，并为每个平面中的点生成密度特征。然后使用签名的奇异值分解(SVD)组件来计算一个全局描述符。Scan contex[29]将点云扫描转换为可见空间，并使用相似度得分计算两个扫描上下文之间的距离。然而，该方法只能提供环型激光雷达对的相对1自由度偏航角估计，不能校正环型激光雷达对的全6自由度姿态。 PointNetVlad[30]利用PointNet[31]和NetVlad[15]生成全局描述符。 然后提出了一个“懒惰的三重和四重”损失函数来解决检索任务。ISC[32]提出了一种基于几何和强度信息的全局描述符，然后使用两级层次强度扫描上下文来检测闭环。 LIDAR-IRIS[33]生成LIDAR-IRIS图像表示以检测潜在环路。SGPR[34]通过保留原始点云的语义和拓扑信息，对点云场景使用语义图表示。OverlapNet[35]采用连体网络对LiDAR扫描生成的距离图像进行重叠估计，并提供匹配的LiDAR点云对的相对偏航角估计。SSC[36]使用全局语义扫描上下文来检测循环。OverlapTransformer[37]使用一种轻量级神经网络，利用点云的距离图像表示来实现快速检索。总的来说，现有的大多数方法要么太耗时，无法实时检测在线3D LiDAR SLAM的环路，要么无法提供光雷达闭环所需的6自由度姿态估计。

---

三、背景回顾

A. LinK3D特征回顾

在这项工作中，提出的BoW3D是基于LinK3D [10]特征的。LinK3D由三部分组成：关键点提取、描述符生成和特征匹配。如图2所示，受到2D图像特征SIFT [24]和ORB [25]的启发，LinK3D描述符的核心思想是使用邻域信息来表示当前关键点。LinK 3D描述符由180维向量表示。描述符的每个维度对应于一个扇区区域。第一维度对应于最近的关键点所在的扇区区域，并且其他维度以逆时针顺序布置。LinK3D是轻量级的，在配备Intel Core i7 @2.2 GHz处理器的笔记本电脑上运行时，从64射线激光束收集的点云中提取特征平均需要32毫秒。此外，LinK3D可以实现精确的点对点匹配，可以应用于快速的3D配准。关于LinK3D的详细信息，请参考其原始论文[10]。

图二。LinK3D的核心思想和基于LinK3D的两次激光雷达扫描的匹配结果。绿色的线是有效的匹配。用相邻关键点表示当前关键点的描述符，并将其描述为一个多维向量。描述符的每个维度对应一个扇区。第一个维度对应于当前关键点的最近关键点所在的扇形区域，其他维度对应于逆时针顺序排列的区域。如果扇区区域中有关键点，则搜索扇区区域中最近的关键点，并将其用于表示描述符的相应维度。

B. 词袋的回顾

在二维图像领域，词袋(BoW)[4]通过检索二维特征(如SIFT[24]、ORB[25]等)来识别被重访问的位置。特别是二进制ORB特征的BoW数据库已成功应用于实时地点识别任务。Bow通过从训练图像数据集中提取的描述符集的离线步骤创建了一个树状结构的视觉词汇表。在处理新图像时，词汇表将提取的图像特征转换为低维向量，包含术语频率和逆文档频率(tf-idf)[9]得分。 tf-idf得分的计算方法是：

其中，$n_{wi}$表示图像 $I_i$ 中单词 $w$ 的个数，$n_i$ 表示图像 $I_i$ 中单词的总个数，$N$ 表示到目前为止看到的图像总数，$n_w$ 表示包含单词 $w$ 的图像个数。idf的思想是一个单词在所有图像中出现的次数越少，最终得分越高。这说明该词具有较高的辨识度。tf-idf总分越高，该词在图像中的出现频率越高。如果一个单词的得分足够高，则计算该单词与数据库中的单词之间的相似度。如果数据库中有相似的单词，则使用逆向索引来搜索相应的图像。

---

四、方法

在这一节中，我们介绍了基于BOW3D提出的闭环系统。为了在实践中验证BoW3D的性能，闭环检测系统被嵌入到最先进的A-LOAM1[14]中。如图1所示，本系统主要由三部分组成。它首先提取原始点云中的LinK3D特征。然后利用里程计和建图算法对机器人姿态进行估计。第三部分使用提出的BoW3D检测环路闭合，并校正环路的姿态。如果检测到一个环路并对其进行优化，环路闭合将反馈给建图算法并更新局部映射，从而为后续步骤提供更准确的估计。

A. BoW3D算法

在本节中，我们将介绍所提出的BoW3D算法。正如第III-A节所介绍的，Link3D描述符的每个维度都代表了相应区域中的一个特定关键点，这使得Link3D描述符具有很强的识别性。因此，我们的方法不需要通过在离线步骤中构建树结构词汇表来进一步转换特征。因此，我们的Bow3D不需要加载额外的词汇表文件，这对用户来说更加方便。内存中的数据库结构如图3所示，使用哈希表在单词和单词出现的位置之间建立一对一的映射。理论上哈希表的计算代价为O(1)，适合于高效检索。如图3所示，BoW3D词汇表中的单词由两部分构成:一部分是 Link3D的非零维，另一部分是单词所在的对应维。 地点（点云框架）也由两部分组成：一是框架ID； 另一个是框架中的描述符ID。

图3。BoW3D的数据结构。哈希表用于检索。BoW3D包含描述符中的非零值(Dim-value)和相应的维度(Dim-ID)。每个单词对应一个位置集，该单词在其中出现过。这个位置还包括两个部分，一个是帧ID，另一个是帧中的描述符ID。

1）检索算法：对一个Link3D描述符的单词进行检索，并统计每个地方（点云帧）出现的频率。如果最高的一个大于频率阈值 $T{H}_f$ ，则该地方被认为是候选地方。另外，为了快速检索，采用了反向文档频率(idf)来避免检索出现在多个位置的单词。更具体地说，由于词的出现频率明显高于其他词，因此区分度较低，降低了检索效率。因此，我们定义了一个类似于 idf 的比率因子 ratio，用来衡量当前集合中的位置数与所有集合中的平均值的差值。计算位置数目时，我们用它来决定是否保留当前单词的位集。比率的定义如下:

其中，$N_{set}$ 是当前集合中的位置数。$N/n_w$ 表示一个地点集合的数量平均值，$n_w$ 为词汇中单词的数量，$N$ 为迄今为止出现的地点总数。当位集的比例大于阈值 $T{h}_r$ 时，该位集将不用于计数。检索算法如算法1所示。

2)回环校正:环校正为后端位姿图优化提供约束。首先根据LinK3D的匹配结果计算环的观测约束，然后利用RANSAC[12]去除不匹配。我们遵循[13]中的方法来计算环路姿态$T_{l,c}$ ，这是环路帧 $l$ 和当前帧 $c$ 之间的估计。

给定当前帧的点集 $S_c$ 和循环帧的匹配点集 $S_l$，我们通过最小化以下代价函数来计算循环:

其中 $S_l^i\in S_l$ 和 $S_c^i\in S_c$ 是对应的匹配点。 $R_{l,c}$ 和 $t_{l,c}$ 分别是 $T_{l,c}$ 变换的旋转和平移。$T_{l,c}$ 的定义如下：

我们首先从 $S_l$ 和 $S_c$ 计算质心 $s_l$ 和 $s_c$ 。 设 ${\hat{s}}_l^i$ 和 ${\hat{s}}_c^i$ 为对应点 $s_l^i$ 和 $s_c^i$ 的坐标，分别去掉质心点 $s_l$ 和 $s_c$ 。 然后我们计算矩阵：

求 $W$ 的SVD分解，

如果 $W$ 是满秩的，我们将通过以下方法得到 $R_l,c$ 和 $t_l,c$ 的解：

这使我们能够纠正闭环位姿。此外，还利用闭环位姿对候选闭环帧进行几何验证，并设置距离阈值 $T{h}_{dis}$ 来验证候选闭环是否有效。

3)更新算法:我们提出了一种更新算法，向数据库中添加新的单词和位置。为了提高更新和检索的效率，我们只在数据库中添加一定数量的特征。根据关键点与激光雷达中心之间的距离来选择描述符。具体来说，只有一定数量的更接近的特征被添加到数据库中。类似地，在检索循环时，一定数量的更接近的特征被用于检索。更新算法如算法2所示。

B. 闭环优化

在校正环路位姿后，建立环路帧的位姿图，用于后续的环路优化。位姿图的顶点就是要优化的全局位姿。位姿图的边(顶点之间的连接)是观察约束，包括序列帧之间的相对位姿和修正后的环路位姿。我们将帧 $i$ 和 $j$ 之间的残差定义为:

通过最小化以下代价函数来优化位姿图:

其中 $S$ 是所有序列边的集合，$L$ 是所有闭环检测边的集合。我们使用在图优化器g2o[38]中实现的Levenberg-Marquadt方法来求解优化问题。

位姿优化后，我们更新建图线程的本地映射。其中，局部地图中的点是基于优化后的全局位姿进行更新的，更新后的全局位姿更加精确。这可以确保后续估计的全局一致性。

---

五、实验

为了综合评价算法的性能，我们从四个方面进行了实验评价:(i)评价位置识别性能;(ii)评估我们的方法集成在3D激光雷达SLAM中的性能;(iii)对超参数进行测试，分析方法的鲁棒性;(iv)测试系统各部分的运行时间。们在KITTI[39]数据集上进行了实验验证，该数据集包含了从内城到郊区的不同街道环境数据。KITTI的点云是由Velodyne HDL-64E S2以10 Hz的传感器速率采集的。有11个序列(即，从00到10)与地面真实姿态，这是用来确定真实循环的数量。此外，根据[40]，KITTI中的一些真实位姿误差较大，我们也发现序列02和08的真实位姿在实验中误差较大。 因此，我们使用ICP提炼的更一致的姿势来确定序列02和08中是否存在闭环。与SGPR[34]相似，当两个点云场景之间的欧氏距离小于3 m且时差大于30 s时，两个点云场景被视为一个真正的正环对。选取具有闭环的序列(00，02，05，06，07，08)进行计算，我们在实验中设置$T{H}_{dis}$ =3。实验是在一台配备 Intel Core [email protected]处理器和16 GB RAM的笔记本上进行的。

A. 位置识别性能

在这一节中，我们将我们的方法与目前最先进的激光雷达环路闭合检测和位置识别方法进行比较，包括M2DP[28]、Scan Context[29]、PointNetVLAD[30]、Intensity Scan Context(ISC)[32]、LiDAR IRIS[33]、SGPR[34]、OverlapNet[35]和SSC[36]。根据[36]中引入的评价指标，计算我们方法的F1评分和Extended Precision [41] (EP)的最大值，并采用[36]中F1评分和EP的结果进行比较。此外，我们还展示了这些方法是否可以用于校正全六自由度环位姿。F1成绩的定义如下:

其中 $P$ 和 $R$ 分别是精度（precision）和 召回率（recall）。 $F1$ 表示精度（precision）和 召回率（recall）的调和均值，精度（precision）和 召回率（recall）同等重要，用于评价分类的整体性能。 我们使用每种方法的最大F1得分进行比较。
扩展精度的定义如下:

其中 $P_{R0}$ 表示最小召回率时的精度，$R_{P100}$ 表示100%精度时的最大召回率。$EP$ 专门设计用于验证位置识别性能。比较结果示于表 I 中。

对F1最大值和扩展精度（F1最大值/扩展精度）的比较结果，以及这些方法是否可以用于修正整个6自由度位姿。最好的分数用粗体标出，次好的分数用下划线标出

如表 I 所示，我们的BoW3D在大多数序列上的 F1 max score和 EP 都超过了其他方法。特别是对于序列08，在只有反向循环的情况下，我们的方法仍然可以得到大部分循环，并且精度很高。这说明我们的方法对视角的变化具有鲁棒性，如图4所示。

(a) 在KITTI 00中扫描113（红色）和扫描1564（紫色）之间的角为45°的闭环识别
(b) 在KITTI 08中，扫描236（红色）和扫描1643（紫色）之间的反向识别环路
图 4. 基于我们的位置识别系统的识别结果和基于Link3D的不同方向闭环的匹配结果。 绿线是有效的匹配项。

此外，比较的方法无法对全六自由度环位姿进行校正，限制了3D LiDAR SLAM中后续的环位姿优化。我们的方法表现良好，同时也可以用于修正全六自由度环的姿态。我们的方法优于基线方法的原因如下:(i)我们的BoW3D是基于LinK3D描述符，LinK3D是位姿不变的。这使得我们的方法能够快速识别循环，即使循环是反向的。(ii)如图4所示，我们的系统根据精确的点对点匹配结果形成约束，保证了可以进行有效的回路校正。

B. 基于激光雷达的SLAM的性能

在本实验中，我们评估了用于 3D Lidar SLAM的闭环系统的性能。 为了验证环路校正的准确性，我们参考文献[40]中定义的验证度量:(i)平移的欧几里得距离； (ii)旋转θ。 θ定义如下：

其中，$||R-R|{|}_F$ 是估计和地面真值旋转矩阵之间的弦距离[42]的Frobenius范数。
为了验证整个轨迹的准确性，采用均方根误差(RMSE)进行评估。RMSE定义如下:

其中 $Q_i$ 是地面真实姿态，并且 $P_i$ 是估计的姿态。我们的SLAM系统与A-LOAM的回路校正精度和轨迹比较结果分别见表II和图5。

利用 BoW3D 进行闭环校正和优化时的角度和平移误差以及基于地面真实值的均方根误差的变化结果

图5。将A-LOAM的轨迹与我们的位置识别系统在KITTI上的轨迹进行了比较。我们可以看到，我们的位置识别系统可以有效地识别再访问的位置，并纠正环路姿态。

从图5的对比结果可以看出，闭环系统能够有效地修正累积误差，并且闭环后的位姿估计优于原始的位姿估计。表II 展示了与原始SLAM位姿估计结果相比，在使用我们的闭环后 RMSE 的数量减少。结果表明，提出的闭环控制方法可以有效地减少 3D Lidar SLAM 系统的漂移量。

C. 最大值超参数设置及鲁棒性分析

本部分通过实验对算法1中 $T{h}_r$ 和 $T{h}_f$ 的参数设置进行了深入的研究，这对BOW3D的性能有重要的影响。 为了进行广泛的测试，我们设置了8个关于 $T{h}_r$ 的参数，从2.5~4.25开始，间隔为0.25。$T{h}_f$ 从4到10开始，间隔为1。使用F1分数和 BoW3D 的平均检测时间来衡量不同参数设置的性能。结果如图6所示。

图6。在 KITTI 00上不同 $T{h}_r$和 $T{h}_f$ 设置的F1评分和运行时间的结果。

鲁棒性分析。从图6可以看出，随着 $T{h}_r$ 的增加，所需的运行时间也会增加，F1分数也会增加。当我们设置 $T{h}_r$ 大于3时，F1分数在大多数序列上变化不大，运行时间仍在增加。当我们设置 $T{h}_f$ 小于5时，虽然F1值较高，但算法也需要更多的时间来检测真实位置。如果我们设置 $T{h}_f$ 大于8，这将会降低我们算法的鲁棒性，因为F1的分数很低。为了让我们的算法稳健地工作，并权衡运行时间和准确性，我们将 $T{h}_r$ 设置为4, $T{h}_f$ 设置为5。

D. 系统运行时间

在本实验中，我们对SLAM系统中每个模块的平均运行时间进行了评估。KITTI 00用于评估，其中包括4K+激光雷达扫描。 我们设置 $T{h}_r=4$ ，$T{h}_f=5$ 。另外，在添加到数据库时，我们将更接近的特征数设置为5个，从数据库中检索时，将其设置为3个。各模块运行时间如图7所示。

图7。系统各部分处理KITTI 00上一个点云帧的平均运行时间。

注意，系统的每个模块在不同的线程中单独操作。虽然建图线程和位姿图优化(PGO)的运行时间超过100 ms，但由于它们的频率较低，可以在线执行。特别是，BoW3D处理一帧的时间总体不到100毫秒，保证了BoW3D应用于3D Lidar SLAM系统的实时性。

---

六、结论

在本文中，我们提出了一种新的基于 3D 特征词袋的位置识别算法。所提出的BoW3D利用LinK3D特征构建词袋模型。它由位置检索、闭环校正和数据库更新三部分组成。Bow3D中使用哈希表作为数据库的整体结构，使检索更加高效。与现有的方法相比，BOW3D不仅取得了较好的效果，而且可以实时校正全6自由度的相对环位姿。与基于深度学习的方法相比，我们的方法不需要预先训练和GPU资源。此外，我们还将提出的BOW3D嵌入到激光雷达里程计系统中，在实践中验证其性能。与原有的里程计算法相比，采用BOW3D的 3D Lidar SLAM系统在有环路时具有更低的漂移。克服当前三维激光雷达里程计系统无闭环线程的缺点将是一件非常有趣的事情，我们还将把我们的方法扩展到3D Lidar SLAM中的重定位、局部优化和建图，以提高当前3D Lidar SLAM系统的效率、精度和鲁棒性。