0. 简介

相信大家在面试时候经常被问激光SLAM的回环方法。一般我们回答的比较笼统，我们在《SC-LEGO-LOAM 扩展以及深度解析》一文中提到了回环的策略，但是这两年又出来更多的方。相较于16年谷歌提出的《Real-Time Loop Closure in 2D LIDAR SLAM》2D激光雷达算法，最近文章《BoW3D: Bag of Words for Real-time Loop Closing in 3D LiDAR SLAM》就是提出了将词袋模型应用于3D激光雷达中。以实时构建词袋信息，目前作者并未开源代码，但是这并不影响其价值，可以使用视觉类似的词袋方法，不仅能有效地识别再次访问的回路位置，而且还能实时校正全6自由度回路姿态。BoW3D基于3D特征LinK3D构建词包，该特征高效、姿态不变，可用于精确的点对点匹配。

1. 本文贡献

在视觉SLAM领域，已经提出了许多算法来使用图像检索技术。它们通常通过将图像作为文字包（BoW）空间中2D特征的数值向量进行比较来匹配图像。BoW已被证明能够实现非常有效和快速的特征匹配，并促进了视觉SLAM的进展。但是在三维激光雷达SLAM领域，三维激光雷达点云的不规则性、稀疏性和无序性对三维特征提取和表示提出了挑战。在基于3D激光雷达的SLAM中，将3D特征应用于实时闭环和校正全6自由度环路姿态等问题仍然没有解决。

在本文中提出了一种新的闭环方法，该方法为从激光雷达点云提取的三维特征构建一个词包。我们使用LinK3D作为3D点特征，并为名为BoW3D的3D点云提供词袋BoW。 同时为了实现快速检索，文中提出了一种基于哈希表的三维特征检索算法。找到匹配后，我们使用RANSAC和SVD的精确点对点LinK3D匹配结果计算全6自由度回路姿态。

• 提出了一种基于具有3D特征的文字包的完整闭环系统。它建立了LinK3D特征数据库，可以实时有效地识别重访的地点。
• 所提出的方法还可用于基于LinK3D的匹配结果实时校正全6自由度回路姿态，这使得我们的方法能够与在线操作中的后续回路优化过程一起使用
• 所提出的方法已嵌入到最先进的激光雷达里程计系统中。实验结果表明，该方法可以显著消除漂移，提高三维激光雷达SLAM系统的精度。

2. 主要内容

2.1 LinK3D特征

BoW3D基于LinK3D特征。LinK3D由三部分组成：关键点提取、描述符生成和特征匹配。如下图所示，LinK3D描述符的核心思想是使用邻域信息表示当前关键点，这是受2D图像特征SIFT和ORB的启发。LinK3D描述符由180维向量表示。描述符的每个维度对应于扇区区域。第一个维度对应于最近关键点所在的扇区区域，其他维度对应于按逆时针顺序排列的区域。LinK3D重量轻，可在32毫秒内构建。此外，LinK3D可用于实现精确的点对点匹配，因此适用于快速3D配准。等这篇代码开源后我们再详细的去看

2.2 词袋模型

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

$tf-idf=\frac{n_{w i}}{n_{i}} \log \frac{N}{n_{w}}$

其中 $n\_{wi}$ 为单词w在第i帧图片中出现的次数
$n_i$ 为图片中的单词总数
$N$ 所有的图片数 即当前记录了多少帧
$n_w$ 单词w在所有图片中出现的总次数
意思是如果一个词 $w$ 在所有图像中出现的次数越少，最终得分越高，这表明该词具有更高的辨别力。tf-idf总分越高，则代表图像中单词 $w$ 的频率越高，表明这个word不适合用来分类。

如果一个词的分数足够高，则计算该词与数据库中的词之间的相似度。如果数据库中有相似的词，则使用反转索引搜索相应的图像。

3. 详细方法

所提出的系统主要由三部分组成。它首先提取原始点云中的LinK3D特征。然后，它使用A-LOAM的里程计和SLAM地图算法来估计机器人姿态。第三部分使用所提出的BoW3D来检测环路闭合并校正环路姿态。如果检测到并优化了环路，则环路闭合将向地图算法提供反馈并更新局部地图，这为后续步骤提供了更精确的姿态。

LinK3D描述符的每个维度代表相应区域中的特定关键点，这使得LinK3D描述符非常具有描述性。因此，我们的方法不需要通过在离线步骤中构建树的方法进行额外转换。因此，我们的BoW3D不需要加载额外的词汇文件，与原始的BoW算法相比，更便于实时位置识别。内存中数据库的结构下图所示，哈希表用于建立单词和单词出现的位置之间的一对一映射。哈希表的计算成本理论上是O(1)的复杂度，这使得它适合于高效检索。BoW3D词汇表中的单词由两部分构成：Dim_value为描述子计算得到的非零数，而Dim_ID为word相对应的维度数。对于每一张图片Place Set中的word集合也包含两种变量：FrameID为第几帧，DesID为一帧画面中的描述子的ID号

3.1 词袋存储

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

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

$N_{set}$ 为word对应的place set中包含的place个数， $N$ 为place的总数， $n_w$ 为总的单词数。如果这个数值高于了阈值 那么这个word对应的place set将不会再被计算。流程图如下图所示

3.2 回环矫正

为了有效校正SLAM系统的漂移，后端的姿态图用于优化姿态。首先根据LinK3D的匹配结果计算回路的观测约束，然后使用RANSAC消除误匹配。并计算循环姿态 $T_{l,c}$ ，即循环帧 $l$ 和当前帧 $c$ 之间的相对姿态。

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

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

在文中说的很清晰： $l$ 为回环检测到的历史帧的点云 ， $c$ 为当前帧的点云 ， $s$ 为激光点 ，求解 $R,t$ 的方法为：
$\boldsymbol{W} = \sum_{i = 1}^{n} \hat{\boldsymbol{s}}_{l}^{i} \hat{s}_{c}^{i T}$

$W = U\Sigma V^T$

$R_{l,c}=VU^T$

$t_{l,c}=s^, - R_{l,c}s$

$s_ls_c$ 为去中心化的点云坐标。这允许我们回环校正姿势。此外，回环校正还可用于验证回环帧是否有效，并且我们可以设置距离阈值 $T h_{dis}$ 来确定其有效性。

3.3 更新算法

我们提出了一种向哈希表中添加新词和位置的更新算法。为了提高更新和检索的效率，我们只在哈希表中添加了一定数量的特征。描述子只会基于其对应的关键点与激光雷达中心之间的距离来选择。具体而言，只有特定数量的更接近特征（Na）被添加到哈希表。类似地，当检索循环时，使用一定数量的更接近特征（Nr）进行检索。更新算法如下图所示

3.4 检测回环

在校正环路姿态后，构建环路帧的姿态图，用于后续环路优化。顶点的姿态图是要优化的全局姿态。位姿图的边（顶点之间的连接）是观察约束，由相邻帧之间的相对位姿和循环位姿组成。我们将帧 $i$ 和 $j$ 之间的残差定义为：

$\begin{array}{c} r_{i, j}\left(\boldsymbol{T}_{w, i}, \boldsymbol{T}_{w, i}\right)=\ln \left(\boldsymbol{T}_{i, j}^{-1} \boldsymbol{T}_{w, i}^{-1} \boldsymbol{T}_{w, j}\right)^{\vee} \end{array}$

通过最小化以下成本函数，优化顺序边和环闭合边的整个姿态图：

$\begin{array}{c} \min _{T}\left\{\sum_{(i, j) \in S}\left\|r_{i, j}\right\|^{2}+\sum_{(i, j) \in L}\left\|r_{i, j}\right\|^{2}\right\} \end{array}$

其中 $S$ 是所有连续边的集合， $L$ 是所有环闭合边的集合。我们使用图优化器g2o中实现的Levenberg-Markadt方法来解决优化问题。

姿态优化后，我们更新跟踪线程的局部映射。具体而言，基于当前帧的更新姿态将局部地图中的点投影到全局坐标，这可以确保后续估计的全局一致性。

4. 参考链接

https://blog.csdn.net/weixin_45485946/article/details/126443177