【自动驾驶100问】第八问：回环检测方法介绍

视频链接

博主是《学习OpenCV4：基于Python的算法实战》和《深度学习计算机视觉实战》图书的作者，目前从事自动驾驶感知融合算法开发，分享内容包括深度学习、计算机视觉、OpenCV、自动驾驶、SLAM、C++/Python语言开发等方向的内容。
《学习OpenCV4：基于Python的算法实战》：OpenCV4所有模块系统讲解，200+个实战案例，Python/C++双语编写（Python为主），对入门学习很有帮助。
《深度学习计算机视觉实战》：算法理论、算法实战案例到算法模型部署都有系统讲解，特别是模型部署目前学习资源少，但是对于算法学习很重要，本书部署基于Tensorflow
Lite（所有框架部署流程基本一样），还有模型优化，模型转换等方向的详细讲解。

---

在SLAM中，位姿的估计往往是一个递推的过程，即由上一帧位姿解算当前帧位姿，因此其中的误差便这样一帧一帧的传递下去，也就是我们所说的累积误差。一个消除误差有效的办法是进行回环检测。回环检测判断机器人是否回到了先前经过的位置，如果检测到回环，它会把信息传递给后端进行优化处理。回环是一个比后端更加紧凑、准确的约束，这一约束条件可以形成一个拓扑一致的轨迹地图。如果能够检测到闭环，并对其优化，就可以让结果更加准确。
在检测回环时，如果把以前的所有帧都拿过来和当前帧做匹配，匹配足够好的就是回环，但这样会导致计算量太大，匹配速度过慢，而且没有找好初值的情况下，需要匹配的数目非常巨大。因此回环检测是SLAM问题的一个难点，针对这个问题，在这里我们总结几种经典的方法供大家参考。

方法一：词袋模型（Bag Of Words,BOW)

在视觉SLAM系统中，比较流行的回环检测方法是特征点结合词袋的方法（如ORB-SLAM,VINS-Mono）等。基于词袋的方法是预先加载一个词袋字典树，通知这个预加载的字典树将图像中的每一局部特征点的描述子转换为一个单词，字典里包含着所有的单词，通过对整张图像的单词统计一个词袋向量，词袋向量间的距离即代表了两张图像之间的差异性。在图像检索的过程中，会利用倒排索引的方法，先找出与当前帧拥有相同单词的关键帧，并根据它们的词袋向量计算与当前帧的相似度，剔除相似度不够高的图像帧，将剩下的关键帧作为候选关键帧，按照词袋向量距离由近到远排序。
过程：
（1）构建字典：相当于描述子聚类过程，可以用K近邻算法，或者使用已经探索过的环境中的特征在线动态生成词袋模型。
（2）建立字典树：因为字典太过庞大，如果一一查找匹配单词，会产生很大的计算量，因此可以用k叉树的方式来表达字典以建立字典树。
（3）计算词袋向量：关键帧和查询帧的相似度是通过词袋向量之间的距离来衡量的。
（4）相似度计算：一些词在用来识别两个图像是否显示同一个地方比其他词更加有用，而有一些词对识别贡献不大，为了区分这些词的重要性，可以为每个词分配特定权重，常见方案是TF-IDF。它综合了图像中的词的重要性（TF-Term Frequency）和收集过程中词的重要性（IDF-Inverse Document Frequency）,用以评估一个词对于一个文件或者一个语料库中的一个领域文件集的重复程度。
（5）回环验证：
词袋模型的另一个问题是它并不完全精确，会出现假阳性数据。在回环检测检索的后期阶段需要用其他方法加以验证。如果当前跟踪已经完全丢失，需要重定位给出当前帧的位姿来调整。在重定位的验证中，使用空间信息进行筛选，可以使用PnP进行后验校正，或者使用条件随机场。这个验证可以去掉那些和参考图像不符合几何一致性的图像。得到准确的图像匹配后，可以根据匹配结果去求解相机位姿。
如果系统跟踪正常，发现了之前访问过的场景，需要进行回环检测添加新约束。基于词袋的回环检测方法只在乎单词有无，不在乎单词的排列顺序，会容易引发感知偏差，此外，词袋回环完全依赖于外观而没有利用任何的几何信息，会导致外观相似的图像容易被当作回环，因此需要加一个验证步骤，验证主要考虑以下三点：
1）不与过近的帧发生回路闭合如果关键帧选得太近，那么导致两个关键帧之间的相似性过高，检测出的回环意义不大，所以用于回环检测的帧最好是稀疏一些，彼此之间不太相同，又能涵盖整个环境。且为了避免错误的回环，某一位姿附近连续多次（ORB-SLAM中为3次）与历史中某一位姿附近出现回环才判断为回环；回环候选帧仍然要匹配，匹配点足够才为回环。
2）闭合的结果在一定长度的连续帧上都是一致的。如果成功检测到了回环，比如说出现在第1 帧和第n 帧。那么很可能第n1 帧、n2 帧都会和第1 帧构成回环。但是，确认第1 帧和第nnn 帧之间存在回环，对轨迹优化是有帮助的，但再接下去的第n1n 1n1 帧，n2n 2n2 帧都会和第1 帧构成回环，产生的帮助就没那么大了，因为已经用之前的信息消除了累计误差，更多的回环并不会带来更多的信息。所以，我们会把“相近”的回环聚成一类，使算法不要反复地检测同一类的回环。
3）闭合的结果在空间上是一致的。即是对回环检测到的两个帧进行特征匹配，估计相机的运动，再把运动放到之前的位姿图中，检查与之前的估计是否有很大出入。
经典的词袋模型算法：
DBOW/DBOW2/DBOW3、FBOW、FAB-MAP

方法二：随机蕨法（Random ferns）

这种重定位方法将相机的每一帧压缩编码，并且有效的对不同帧之间相似性进行评估。而压缩编码的方式采用随机蕨法。在这个基于关键帧的重定位方法中，采用基于fern的帧编码方式：输入一个RGB-D图片，在图像的随机位置评估简单的二进制测试，将整个帧进行编码，形成编码块，每个fern产生一小块编码，并且编码连接起来可以表达一个紧凑的相机帧。每一个编码块指向一个编码表的一行，和具有等效的编码、存储着关键帧id的fern关联起来，编码表以哈希表的形式存储。

方法三：基于深度学习的方法

基于深度学习的图像检索方法是全局检索方法，需要大量的数据进行预训练，但对场景变化容忍度好。一些端到端的相机位姿估计方法取得了不错的成果。深度学习和视觉定位结合的开创性工作PoseNet就使用的是神经网络直接从图像中得到6自由度的相机位姿。相较于传统的视觉定位方法，省去了复杂的图像匹配过程，并且不需要对相机位姿进行迭代求解，但是输入图像必须在训练场景中。后来在此基础上，他们又在误差函数中使用了投影误差，进一步提高了位姿估计的精度。同样，MapNet使用了传统方法求解两张图象的相对位姿，与网络计算出来的相对位姿对比得到相机的相对位姿误差，将相对位姿误差添加到网络的损失函数中，使得求解出来的相机位姿更加平滑，MapNet还可以将连续多帧的结果进行位姿图优化，使得最终估计出的相机位姿更为准确。
有监督的方法如Places365，无监督的方法例如CALC。