先说总结

现算法已经开源，代码链接：

GitHub - YungeCui/BoW3D: [RA-L] BoW3D: Bag of Words for Real-Time Loop Closing in 3D LiDAR SLAM.

背景

SLAM（同时定位与地图构建）是一种让机器人在构建环境地图的同时，定位自己位置的算法。然而，因为传感器误差的存在，机器人的移动和旋转估计会有误差，会引发所谓的漂移问题，导致地图失真。这就需要引入回环检测，即判断机器人是否返回到了之前探索过的地方，这样可以纠正因误差累积引起的地图和位置偏差，增加地图精度和鲁棒性，对于SLAM的全局一致性至关重要。

原文链接：

[2208.07473] BoW3D: Bag of Words for Real-Time Loop Closing in 3D LiDAR SLAM (arxiv.org)

1. 摘要

回环是用于自主移动的系统的同时定位和映射（SLAM）的基本部分。在视觉SLAM领域，词袋（Bag of Words，BoW）在闭环方面取得了巨大的成功。用于环路搜索的BoW特征也可以用于随后的6-DoF环路校正。然而，对于3D LiDAR SLAM，现有技术的方法可能无法真实的有效地识别环，并且通常不能校正完整的6-DoF环姿态。为了解决这个限制，我们提出了一种新的词袋实时循环关闭3D激光雷达SLAM，称为BoW 3D。我们的方法不仅有效地认识到重访的循环的地方，但也纠正了完整的6自由度循环的姿态实时。BoW 3D基于3D LiDAR特征LinK 3D构建词袋，该特征高效、姿态不变，可用于精确的点对点匹配。此外，我们将我们提出的方法嵌入到三维激光雷达里程计系统，以评估闭环性能。我们在公共数据集上测试了我们的方法，并将其与其他最先进的算法进行了比较。BoW 3D在大多数场景下在F1 max和扩展精度得分方面表现出更好的性能。值得注意的是，BoW 3D在配备英特尔酷睿i7@2.2 GHz处理器的笔记本电脑上执行时，平均需要48 ms来识别和纠正KITTI 00上的环路（包括4K+ 64射线LiDAR扫描）。我们在这里发布方法的实现：https://github.com/YungeCui/BoW3D。

2. 主要内容

2.1 LinK3D特征

在这项工作中，提出的BoW 3D基于LinK 3D [10]功能。LinK 3D由三个部分组成：关键点提取、描述符生成和特征匹配。如图2所示，LinK 3D描述符的核心思想是使用邻域信息来表示当前关键点，其灵感来自2D图像特征SIFT [24]和ORB [25]。LinK 3D描述符由180维向量表示。描述符的每个维度对应于扇区区域。第一维度对应于最接近的关键点所在的扇区区域，而其他维度对应于以逆时针顺序排列的区域。LinK 3D是轻量级的，平均需要32 ms从64射线激光束收集的点云中提取特征，当在配备英特尔酷睿i7@2.2 GHz处理器的笔记本电脑上执行时。我们将BoW 3D嵌入到循环关闭线程中，该线程关闭循环并获得全局一致的SLAM结果。应用于快速3D配准。有关LinK 3D的更多详细信息，请参阅其原始论文[10]。

图 2. 我们的 LinK3D 核心理念以及基于我们的 LinK3D 对两次激光雷达扫描的匹配结果。绿色线条表示有效的匹配。当前关键点的描述符是由其邻近关键点表示的。描述符的每个维度对应一个扇区区域。第一维对应当前关键点最近的关键点所在的扇区区域，其他的则按逆时针顺序对应到各区域。如果一个扇区区域内有关键点，就搜索该扇区区域内最近的关键点，并用它来表示描述符的对应维度。

2.2 词袋模型

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

$tf\text{-i}df=\frac{n_{wi}}{n_{i}}log\frac{N}{n_{w}}$

其中nwi表示图像Ii中单词w的数量，ni表示Ii中单词的总数，N是迄今为止看到的图像的总数，并且nw表示包含单词w的图像的数量。idf的思想是，一个词在所有图像中出现的次数越少，最终得分就越高，这表明这个词有更高的辨别力。总tf-idf得分越高，图像中单词的频率越高。如果单词的得分足够高，则计算该单词与数据库中的单词之间的相似度。如果在数据库中存在相似的词，则使用倒排索引来搜索对应的图像。

词频TF定义
一个特征在一幅图里面的数量较多，那么这个特征对于这个图像来说就有较高的权重。

$\mathrm{TF}=\frac{numberofwordwinimageI}{numberofallwordsinimageI}$

逆向文本频率定义（IDF）

$\mathrm{IDF}=\log\frac{numberofallimages}{numberofimagescontainingwordw}$

如果一个特征在很多图像中出现，那么这个特征的区分度就比较低；相反，如果一个特征在较少的图像中出现，则有较大的区分度。

$\mathrm{TF-IDF=TF*IDF}$

逆向索引主要记录某个单词在哪些图像中出现，以及这些单词的权重。

3. 方法

在本节中，我们将介绍基于BoW 3D的闭环系统。为了在实践中验证BoW 3D的性能，将闭环系统嵌入到最先进的A-LOAM 6 [14]中。如图所示1。提出的系统主要由三部分组成。它首先提取原始点云中的LinK 3D特征。然后，它使用A-LOAM估计机器人的位姿。第三部分使用所提出的BoW 3D来检测环路闭合并校正环路姿势。如果检测到并优化了循环，则循环关闭将向映射算法给予反馈并更新局部映射，这为后续步骤提供了更准确的估计。

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

3.1 BoW3D Algorithm

在本节中，我们介绍所提出的BoW 3D算法。如第III-A节中所介绍的，LinK 3D描述符的每个维度表示对应区域中的特定关键点，这使得LinK 3D描述符具有很强的区分性。内存中数据库的结构如图所示3中，哈希表用于在单词和单词出现的位置之间建立一对一的映射。哈希表的计算成本理论上为O（1），这使得它适合于高效的检索。如图所示3、BoW 3D词汇表中的单词由两部分构成：一是LinK 3D的非零维数;另一个是词所在的对应维度。地点（点云框架）也由两部分组成：一个是帧Id;另一个是帧中的描述符ID。

图3. BoW3D的数据结构（本身就是个哈希表啊）。哈希表用于检索。BoW3D的单词由描述符中的非零值（Dim-value）和相应的维度（Dim-ID）组成。每个单词对应一个地点集，在其中出现了该单词。地点也由两部分组成，一部分是帧ID，另一部分是帧中的描述符ID。

1）检索算法：对于LinK3D描述符的单词，我们检索单词并计数每个位置（点云帧）出现的频率。如果最高的一个大于频率阈值Thf，则该地点被认为是候选地点。此外，为了快速检索，使用逆文档频率（idf）来避免检索出现在多个地方的单词。更具体地说，由于这些词出现的频率明显高于其他词，因此它们的区分度较低，降低了检索效率。因此，我们定义了一个类似于idf的比率因子来衡量当前集合中的位置数与所有集合中的平均值之间的差异。我们用它来确定在计算位置数时是否应该保留当前单词的位置集。该比率定义如下：

$ratio=N_{set}/(\frac{N}{n_{w}})$

其中Nset是当前集合中的位置的数量。N/nw表示一个位置集合的数量平均值，nw是词汇表中的单词数量，N是到目前为止看到的位置的总数。如果位置集合的比率大于阈值Thr，则该位置集合将不被用于计数。检索算法如算法1所示。

2) 回环校正：回环校正用于为后端的姿态图优化提供约束。首先基于LinK3D的匹配结果计算循环的观测约束，然后使用RANSAC [12]来去除mismatches。我们遵循[13Least-squares fitting of
two 3-d point sets]中的方法来计算循环姿态Tlc，其是loop frame回环帧 l 与当前帧 c 之间的估计。

给定当前帧的点集{S }c和循环帧的匹配点集{S }l，我们通过最小化以下成本函数来计算循环：

$r_{l,c}(\boldsymbol{R}_{l,c},\boldsymbol{t}_{l,c})=\frac{1}{2}\sum_{i=1}^{n}\|s_{l}^{i}-\left(\boldsymbol{R}_{l,c}s_{c}^{i}+\boldsymbol{t}_{l,c}\right)\|^{2}$

其中 $s_l^i \in\{S\}_l$ 和 $\boldsymbol{s}_c^i \in\{S\}_c$ 是对应的匹配点。 $\boldsymbol{R}_{l,c}$ 和 $t_{l,c}$ 分别 $T_{l,c}$ 是变换的旋转和平移。 $T_{l,c}$ 定义如下：

$T_{l,c}=\begin{bmatrix}R_{l,c}&t_{l,c}\\\\\mathbf{0}^T&1\end{bmatrix}$

我们首先根据 $\{S\}_{l}$ 和 $\{S\}_{c}$ 计算质心 $s_{\boldsymbol{l}}$ 和 $s_{\boldsymbol{c}}$ 。设 $\hat{s}_l^i$ 和 $\hat{s}_{c}^{i}$ 是对应点 $s_{l}^{i}$ 和 $s_{c}^{i}$ 的坐标，其分别去除了中心点 $s_{\boldsymbol{l}}$ 和 $s_{c}$ 。然后我们计算矩阵：

$\boldsymbol{W}=\sum_{i=1}^n\boldsymbol{\hat{s}}_l^i\boldsymbol{\hat{s}}_c^{iT}$

求W的SVD分解

$W=U\boldsymbol{\Sigma}\boldsymbol{V}^T$

如果W是满秩的，则我们将通过下式得到Rl，c和tl，c的解：

$\begin{aligned}R_{l,c}&=\boldsymbol{VU}^T,\\t_{l,c}&=s_l-R_{l,c}s_{c}.\end{aligned}$

这使我们能够校正回路姿势。此外，循环姿态还用于候选循环帧的几何验证，并且我们设置距离阈值Thdis来验证候选循环是否有效。

3)更新算法：我们提出了一个更新算法来添加新的单词和地方的数据库。为了提高更新和检索的效率，我们只向数据库中添加一定数量的特征。描述符基于其对应关键点与LiDAR中心之间的距离来选择。具体地，Specifically, only a certain number of closer features are added to the database。类似地，当检索环时，使用一定数量的更接近的特征来检索。更新算法在算法2中示出。