​综述 | SLAM回环检测方法

本文作者任旭倩，公众号：计算机视觉life成员，由于格式原因，公式显示可能出问题，建议阅读原文链接：综述 | SLAM回环检测方法
在视觉SLAM问题中，位姿的估计往往是一个递推的过程，即由上一帧位姿解算当前帧位姿，因此其中的误差便这样一帧一帧的传递下去，也就是我们所说的累积误差。一个消除误差有效的办法是进行回环检测。回环检测判断机器人是否回到了先前经过的位置，如果检测到回环，它会把信息传递给后端进行优化处理。回环是一个比后端更加紧凑、准确的约束，这一约束条件可以形成一个拓扑一致的轨迹地图。如果能够检测到闭环，并对其优化，就可以让结果更加准确。

在检测回环时，如果把以前的所有帧都拿过来和当前帧做匹配，匹配足够好的就是回环，但这样会导致计算量太大，匹配速度过慢，而且没有找好初值的情况下，需要匹配的数目非常巨大。因此回环检测是SLAM问题的一个难点，针对这个问题，在这里我们总结几种经典的方法供大家参考。

词袋模型（Bag Of Words,BOW)

原理

简介：现有的SLAM系统中比较流行的回环检测方法是特征点结合词袋的方法（如ORB-SLAM,VINS-Mono）等。基于词袋的方法是预先加载一个词袋字典树，通知这个预加载的字典树将图像中的每一局部特征点的描述子转换为一个单词，字典里包含着所有的单词，通过对整张图像的单词统计一个词袋向量，词袋向量间的距离即代表了两张图像之间的差异性。在图像检索的过程中，会利用倒排索引的方法，先找出与当前帧拥有相同单词的关键帧，并根据它们的词袋向量计算与当前帧的相似度，剔除相似度不够高的图像帧，将剩下的关键帧作为候选关键帧，按照词袋向量距离由近到远排序[1]。

字典、单词、描述子之间的关系是：
$$字典\supset 单词\supset 差距较小的描述子的集合$$
因此，可将基于词袋模型的回环检测方法分为以下三个步骤[2]：

1.提取特征

2.构建字典（所有单词的集合)
$$D=\left({\omega }_1,{\omega }_2,{\omega }_3\dots {\omega }_{n-1},{\omega }_n\right)$$
3.确定一帧中具有哪些单词，形成词袋向量 (1表示具有该单词，0表示没有)
$$F=1\cdot {\omega }_{1}0\cdot {\omega }_{2}0\cdot {\omega }_3\dots 1\cdot {\omega }_{n-1}0\cdot {\omega }_n$$
4.比较两帧描述向量的差异。

下面分模块逐个介绍：

构建字典

相当于描述子聚类过程，可以用K近邻算法，或者使用已经探索过的环境中的特征在线动态生成词袋模型[3]。

（1）k近邻算法

根据已经离线采集的图像，提取特征描述子，用k近邻算法形成字典的流程是：

1.在字典中的多个描述子中随机选取k个中心点：
$$c_1,\dots ,c_k$$
2.对于每一个样本，计算它与每个中心点之间的距离，取最小的中心点作为它的归类。

3.重新计算每个类的中心点。

4.如果每个中心点都变化很小，则算法收敛，退出，否则继续迭代寻找。

每个归好的类就是一个单词，每个单词由聚类后距离相近的描述子组成。

其他类似方法还有层次聚类法、K-means 等。

Kmeans 算法是基于Kmeans改进而来，主要改进点在于中心点的初始化上，不像原始版本算法的随机生成，它通过一些策略使得k个初始中心点彼此间距离尽量地远，以期获得这些中心点具有更好的代表性，有利于后面的分类操作的效果[8]。

Kmeans 算法中中心点初始化的流程如下：

1.从n个样本中随机选取一个点作为第一个中心点；

2.计算样本中每个点和距离它最近的中心点之间的距离$D_i$，根据策略选择新的中心点

3.重复2直至得到k个中心点。

（2）在线动态生成词袋模型：

传统的BOW模型生成离线的字典，更灵活的方法是动态地创建一个字典，这样没有在训练集中出现地特征可以被有效地识别出来。典型论文有[4],[5]。
在论文中将图像识别中词袋模型进行了扩展，并用贝叶斯滤波来估计回环概率。回环检测问题涉及识别已建图区域的困难，而全局定位问题涉及在现有地图中检索机器人位置的困难。当在当前图像中找到一个单词时，之前看到过这个单词的图片的tf-idf 分数将会更新。该方法根据探索环境时遇到的特征动态地构建字典，以便可以有效识别训练集中未表示的特征的环境。

字典树

因为字典太过庞大，如果一一查找匹配单词，会产生很大的计算量，因此可以用k叉树的方式来表达字典，建立字典树流程是这样的[6]：

1. 对应用场景下的大量训练图像离线提取局部描述符(words)（每张图像可能会有多个描述符）

2. 将这些描述符KNN聚成k类；

3. 对于第一层的每一节点，继续KNN聚成k类，得到下一层；

4. 按这个循环，直到聚类的层次数达到阈值d，叶子节点表示一个word，中间节点则是聚类的中心。

   

​ （图源:视觉SLAM十四讲）

然而建立词袋的一个主要缺点是，它需要预先载入一个训练好的词袋字典树，这个字典树一般包含大量的特征单词，为了保证有良好的区分能力，否则对图像检索结果有较大影响，但是这会导致这个字典文件比较大，对于一些移动应用来说会是一个很大的负担。为了解决这个问题，可以通过动态建立k-d树来避免预载入字典的麻烦。在添加关键帧的过程中维护一个全局的k-d树，将每个特征点以帧为单位添加到这个k-d树中。在图像检索过程中，寻找最接近的节点进行匹配，根据匹配结果对每个关键帧进行投票，获得的票数即可作为该帧的分数，从而生成与当前帧相似的关键帧候选集[1]。

词袋向量

关键帧和查询帧的相似度是通过词袋向量之间的距离来衡量的。假定一幅图像I的局部描述符集合是[6]
D I = { d 1 , d 2 , ⋯   , d n } D_{I}=\left\{d_{1}, d_{2}, \cdots, d_{n}\right\} DI​={ d1​,d2​,⋯,dn​}
每个描述符$d_i$在字典树中查找距离最近的word，假定其word_id为$j$，其对应的权重为$w_j$。查找过程从字典树的根节点开始，每一层都找距离最近的节点，然后下一层中继续，直至到达叶子节点。
记词表的大小为$|V|$，定义这个在字典树中查找的映射为
$$VT(\cdot ):D\to$$