视觉SLAM⑪----回环检测

APS2023

已于 2022-05-29 19:59:58 修改

阅读量2.4k

点赞数 7

分类专栏： SLAM 十四讲读书笔记文章标签：深度学习计算机视觉人工智能

于 2022-05-29 19:59:40 首次发布

本文链接：https://blog.csdn.net/qq_41694024/article/details/125011306

版权

SLAM 十四讲读书笔记专栏收录该内容

12 篇文章 50 订阅

订阅专栏

11.0 本章目标

1．理解回环检测的必要性。
2．掌握基于词袋的外观式回环检测。

本讲中，我们介绍SLAM 中另一个主要模块：回环检测。

SLAM主体（前端、后端）主要的目的在于估计相机运动，而回环检测模块，无论是目标上还是方法上，都与前面讲的内容相差较大，所以通常被认为是一个独立的模块。我们将介绍主流视觉SLAM中检测回环的方式：词袋模型

11.1 概述

11.1.1 回环检测的意义

1.前端+后端建图有什么问题？

前端提供特征点的提取和轨迹、地图的初值；而后端负责对所有这些数据进行优化。

        然而，如果像视觉里程计那样仅考虑相邻时间上的关键帧，那么，之前产生的误差将不可避免地累积到下一个时刻，使得整个SLAM出现累积误差，长期估计的结果将不可靠，或者说，我们无法构建全局一致的轨迹和地图。
        举个简单的例子：在自动驾驶的建图阶段，我们通常会指定采集车在某个给定区域绕若干圈以覆盖所有采集范围。

        假设我们在前端提取了特征，然后忽略特征点，在后端使用位姿图优化整个轨迹，如图11-1( a）所示。前端给出的只是局部的位姿间约束，例如，可能是 $x_{1}-x_{2},x_{2}-x_{3}$ ，等等。但是，由于 $x_{1}$ 的估计存在误差，而 $x_{2}$ 是根据 $x_{1}$ 决定的， $x_{3}$ 又是由 $x_{2}$ 决定的。依此类推，误差就会被累积起来，使得后端优化的结果如图11-1（b）所示，慢慢地趋向不准确。在这种应用场景下，我们应该保证，优化的轨迹和实际地点一致。当我们实际经过同一个地点时，估计轨迹也必定经过同一点。（累计误差）

图11-1 漂移示意图（a）真实轨迹（b）由于前端只给出相邻帧间的估计，优化后的位姿图出现漂移（c）添加回环检测后的位姿图可以消除累积误差

  虽然后端能够估计最大后验误差，但所谓“好模型架不住烂数据”，只有相邻关键帧数据时，我们能做的事情并不多，也无从消除累积误差。

        但是，回环检测模块能够给出除了相邻帧的一些时隔更加久远的约束：例如 $x_{1}\sim x_{100}$ 之间的位姿变换。

        为什么它们之间会有约束呢？这是因为我们察觉到相机经过了同一个地方，采集到了相似的数据。（不可能采集数据仅仅采集一次，肯定多次采集，就会回到原处，我们通过某种方法检测其是否回到原处）而回环检测的关键，就是如何有效地检测出相机经过同一个地方这件事。如果我们能够成功地检测到这件事，就可以为后端的位姿图提供更多的有效数据，使之得到更好的估计，特别是得到一个全局一致的估计。由于位姿图可以看成一个质点——弹簧系统，所以回环检测相当于在图像中加入了额外的弹簧，提高了系统稳定性。也可直观地想象成回环边把带有累积误差的边“拉”到了正确的位置——如果回环本身正确的话。

2.回环检测的意义

        回环检测对于SLAM系统意义重大。一方面，它关系到我们估计的轨迹和地图在长时间下的正确性。另一方面，由于回环检测提供了当前数据与所有历史数据的关联，我们还可以利用回环检测进行重定位。重定位的用处就更多一些。

        例如，如果我们事先对某个场景录制了一条轨迹并建立了地图，那么之后在该场景中就可以一直跟随这条轨迹进行导航，而重定位可以帮助我们确定自身在这条轨迹上的位置。

        因此，回环检测对整个SLAM系统精度与稳健性的提升是非常明显的。甚至在某些时候，我们把仅有前端和局部后端的系统称为视觉里程计，而把带有回环检测和全局后端的系统称为SLAM。

11.1.2 回环检测的方法

1.简单的两种回环检测方法及其局限性

        下面我们来考虑回环检测如何实现的问题。事实上存在若干种不同的思路来看待这个问题，包括理论上的和工程上的。
        最简单的方式就是对任意两幅图像都做一遍特征匹配，根据正确匹配的数量确定哪两幅图像存在关联——这确实是一种简单的思想。

        缺点在于，我们盲目地假设了“任意两幅图像都可能存在回环”，使得要检测的数量实在太大：对于N个可能的回环，我们要检测 $C_{N}^{2}$ 那么多次，这是 $O(N^2)$ 的复杂度，随着轨迹变长增长太快，在大多数实时系统中是不实用的。

        另一种简单的方式是，随机抽取历史数据并进行回环检测，例如在n帧中随机抽5帧与当前帧比较。这种做法能够维持常数时间的运算量，但是这种盲目试探方法在帧数N增长时，抽到回环的概率又大幅下降，使得检测效率不高。

2.基于里程计和外观的回环检测方法

        上面说的思路都过于粗糙。尽管随机检测在有些实现中确实有用，但我们至少希望有一个“哪处可能出现回环”的预计，才好不那么盲目地去检测。这样的方式大体有两种思路:
基于里程计（Odometry based）的几何关系，或基于外观（Appearance based）的几何关系。

基于里程计的几何关系是说，当我们发现当前相机运动到了之前的某个位置附近时，检测它们有没有回环关系——这自然是一种直观的想法，但是由于累积误差的存在，我们往往没法正确地发现“运动到了之前的某个位置附近”这件事实，回环检测也无从谈起。因此，这种做法在逻辑上存在一点问题，因为回环检测的目标在于发现“相机回到之前位置”的事实，从而消除累积误差。而基于里程计的几何关系的做法假设了“相机回到之前位置附近”，这样才能检测回环。这是有倒果为因的嫌疑的。因而也无法在累积误差较大时工作。

        另一种方法是基于外观的。它和前端、后端的估计都无关，仅根据两幅图像的相似性确定回环检测关系。这种做法摆脱了累积误差，使回环检测模块成为SLAM系统中一个相对独立的模块（当然前端可以为它提供特征点)。自提出以来，基于外观的回环检测方式能够有效地在不同场景下工作，成了视觉SLAM中主流的做法，并被应用于实际的系统中。

        除此之外，从工程角度我们也能提出一些解决回环检测的办法。例如，室外的无人车通常会配备GPS，可以提供全局的位置信息。利用GPS信息可以很轻松地判断汽车是否回到某个经过的点，但这类方法在室内就不怎么好用。

3.基于外观的回环检测方法

        在基于外观的回环检测算法中，核心问题是如何计算图像间的相似性。例如，对于图像A和图像B，我们要设计一种方法，计算它们之间的相似性评分： $s(A,B)$ 。当然，这个评分会在某个区间内取值，当它大于一定量后我们认为出现了一个回环。

        计算两幅图像之间的相似性很困难吗？例如直观上看，图像能够表示成矩阵，那么直接让两幅图像相减，然后取某种范数行不行呢?

$s(A,B) = ||A-B||$

为什么我们不这样做?
1.前面也说过，像素灰度是一种不稳定的测量值，它严重地受环境光照和相机曝光的影响。
假设相机未动，我们打开了一支电灯，那么图像会整体变亮。这样，即使对于同样的数据，我们也会得到一个很大的差异值。
2.当相机视角发生少量变化时，即使每个物体的光度不变，它们的像素也会在图像中发生
位移，造成一个很大的差异值。

        由于这两种情况的存在，实际中，即使对于非常相似的图像，A-B也会经常得到一个（不符合实际的）很大的值。所以我们说，这个函数不能很好地反映图像间的相似关系。这里牵涉到一个“好”和“不好”的定义问题。我们要问,怎样的函数能够更好地反映相似关系，而怎样的函数不够好呢?从这里可以引出感知偏差（Perceptual Aliasing）和感知变异（Perceptual Variability）两个概念。

11.1.3 准确率和召回率

1.准确率和召回率

        从人类的角度看，我们能够以很高的精确度，感觉到“两幅图像是否相似”或“这两张照片是从同一个地方拍摄的”这一事实，但由于目前尚未掌握人脑的工作原理，我们无法清楚地描述自己是如何完成这个判断的。

        从程序角度看，我们希望程序算法能够得出和人类，或者和事实一致的判断。当我们觉得，或者事实上就是，两幅图像从同一个地方拍摄，那么回环检测算法也应该给出“这是回环”的结果。反之，如果我们觉得，或事实上，两幅图像是从不同地方拍摄的，那么程序也应该给出“这不是回环”的判断。当然，程序的判断并不总是与我们人类的想法一致，所以可能出现表11-1中的4种情况。

表11-1 回环检测的结果分类

        假阳性（False Positive）又称为感知偏差，而假阴性（False Negative）称为感知变异（如图11-2所示）。

        用缩写TP代表TruePositive（真阳性），用TN代表True Negative（真阴性）。由于我们希望算法和人类的判断一致，所以希望TP和TN尽量高，而FP和FN尽可能低。所以，对于某种特定算法，我们可以统计它在某个数据集上的TP、TN、FP、FN的出现次数，并计算两个统计量：准确率和召回率（Precision & Recall）。

$Precision =TP/(TP+FP),\ \ \ Recall = TP/(TP+FN)$ (11.2)

图11-2 假阳性与假阴性的例子。左侧为假阳性，两幅图像看起来很像，但并非同一走廊；右侧为假阴性，由于光照变化，同一地点不同时刻的照片看起来很不一样

准确率描述的是算法提取的所有回环中确实是真实回环的概率。而召回率则是指，在所有真实回环中被正确检测出来的概率。

2.为什么要取这两个统计量

        为什么取这两个统计量呢？因为它们有一定的代表性，并且通常是一对矛盾。
        一个算法往往有许多的设置参数。例如，当提高某个阈值时，算法可能变得更加“严格”它检出更少的回环，使准确率得以提高。同时，由于检出的数量变少了，许多原本是回环的地方就可能被漏掉，导致召回率下降。反之，如果我们选择更加宽松的配置，那么检出的回环数量将增加，得到更高的召回率，但其中可能混杂一些不是回环的情况，于是准确率下降。

        为了评价算法的好坏，我们会测试它在各种配置下的P和R值，然后做Precision-Recall曲线（如图11-3所示）。当用召回率为横轴，用准确率为纵轴时，我们会关心整条曲线偏向右上方的程度、100%准确率下的召回率或者50%召回率时的准确率，作为评价算法的指标。不过请注意，除了一些“天壤之别”的算法，我们通常不能一概而论地说算法A就是优于算法B的。我们可能会说A在准确率较高时还有很好的召回率，而B在70%召回率的情况下还能保证较好的准确率，诸如此类。

        值得一提的是，在SLAM中，我们对准确率的要求更高，而对召回率则相对宽容一些。由于假阳性的（检测结果是而实际不是的）回环将在后端的位姿图中添加根本错误的边，有些时候会导致优化算法给出完全错误的结果。想象一下，如果SLAM程序错误地将所有的办公桌当成了同一张，那建出来的图会怎么样呢？你可能会看到走廊不直了，墙壁被交错在一起了，最后整个地图都失效了。相比之下，召回率低一些，顶多有部分的回环没有被检测到，地图可能受一些累积误差的影响——然而仅需一两次回环就可以完全消除它们了。所以在选择回环检测算法时我们更倾向于把参数设置得更严格,或者在检测之后再加上回环验证的步骤。

图11-3 准确率–召回率曲线的例子。随着召回率的上升，检测条件变得宽松，准确率随之
下降。好的算法在较高召回率情况下仍能保证较好的准确率。

         那么，回到之前的问题，为什么不用A-B来计算相似性呢？我们会发现它的准确率和召回率都很差，可能出现大量的假阳性或假阴性的情况，所以说这样做“不好”。那么，什么方法更好一些呢?

11.2 词袋模型

1.特征点法做回环检测的局限性

        既然直接用两张图像相减的方式不够好，我们就需要一种更可靠的方式。结合前面几讲的内容，一种思路是：为何不像视觉里程计那样使用特征点来做回环检测呢？

        和视觉里程计一样，我们对两幅图像的特征点进行匹配，只要匹配数量大于一定值，就认为出现了回环。

        根据特征点匹配，我们还能计算出这两幅图像之间的运动关系。当然这种做法存在一些问题，例如，特征的匹配会比较费时、当光照变化时特征描述可能不稳定等，但离我们要介绍的词袋模型已经很相近了。下面我们先来介绍词袋的做法，再来讨论数据结构之类的实现细节。

2.词袋做特征检测

        词袋，也就是Bag-of-Words（BoW），目的是用“图像上有哪几种特征”来描述一幅图像。例如，我们说某张照片中有一个人、一辆车；而另一张中有两个人、一只狗。根据这样的描述，就可以度量这两幅图像的相似性。再具体一些，我们要做以下三步：

1.确定“人”“车”“狗”等概念——对应于BoW中的“单词”（Word），许多单词放在一起，组成了“字典”（Dictionary）。
2.确定一幅图像中出现了哪些在字典中定义的概念——我们用单词出现的情况（或直方图）描述整幅图像。这就把一幅图像转换成了一个向量的描述。
3.比较上一步中的描述的相似程度。
        以上面举的例子来说，首先我们通过某种方式得到了一本“字典”。字典上记录了许多单词，每个单词都有一定意义，例如“人”“车”“狗”都是记录在字典中的单词，我们不妨记为 $w_{1},w_{2},w_{3}$ 。然后，对于任意图像A，根据它们含有的单词，可记为：

$A = 1\cdot w_{1} +1\cdot w_{2}+0\cdot w_{3}$
式11-3 图像A有人和车

         字典是固定的，所以只要用 $[1,1,0]^T$ 这个向量就可以表达 $A$ 的意义。通过字典和单词，只需一个向量就可以描述整幅图像。该向量描述的是“图像是否含有某类特征”的信息，比单纯的灰度值更稳定。又因为描述向量说的是“是否出现”，而不管它们“在哪儿出现”，所以与物体的空间位置和排列顺序无关，因此在相机发生少量运动时，只要物体仍在视野中出现，我们就仍然保证描述向量不发生变化。

        基于这种特性，我们称它为Bag-of-Words而不是什么List-of-Words，强调的是Words的有无，而无关其顺序。因此，可以说字典类似于单词的一个集合。

        回到上面的例子，同理，用 $[2,0,1]^T$ 可以描述图像 $B$ 。如果只考虑“是否出现”而不考虑数量，也可以是 $[1,0,1]^T$ ，这时候这个向量就是二值的。于是，根据这两个向量，设计一定的计算方式，就能确定图像间的相似性。当然，对两个向量求差仍然有一些不同的做法，例如对于 $a,b \epsilon \Re ^w$ 可以计算：



$s(a,b) = 1- \frac{1}{W}||a-b||_1$
标题

         其中范数取 $L_1$ 范数，即各元素绝对值之和。请注意在两个向量完全一样时，我们将得到1；完全相反时（a为0的地方b为1）得到0。这样就定义了两个描述向量的相似性，也就定义了图像之间的相似程度。

        接下来的问题是什么呢?
1.我们虽然清楚了字典的定义方式，但它到底是怎么来的呢?
2.如果我们能够计算两幅图像间的相似程度评分，是否就足够判断回环了呢?
所以接下来，我们首先介绍字典的生成方式，然后介绍如何利用字典实际地计算两幅图像间的相似性。

11.3 字典

11.3.1 字典的结构

        按照前面的介绍，字典由很多单词组成，而每一个单词代表了一个概念。一个单词与一个单独的特征点不同，它不是从单幅图像上提取出来的，而是某一类特征的组合。所以，字典生成问题类似于一个聚类（Clustering）问题。
        聚类问题在无监督机器学习（Unsupervised ML）中特别常见，用于让机器自行寻找数据中的规律。BoW的字典生成问题也属于其中之一。首先，假设我们对大量的图像提取了特征点，例如有N个。现在，我们想找一个有k个单词的字典，每个单词可以看作局部相邻特征点的集合，应该怎么做呢？这可以用经典的K-means（K均值）算法解决。
        K-means是一个非常简单有效的方法，因此在无监督学习中广为使用，下面对其原理稍做介绍。简单的说，当有N个数据，想要归成k个类，那么用K-means来做主要包括如下步骤：

1.随机选取k个中心点： $c_{1},c_{2},...,c_{k}$
2.对每一个样本，计算它与每个中心点之间的距离，取最小的作为它的归类。

3.重新计算每个类的中心点。
4.如果每个中心点都变化很小，则算法收敛,退出；否则返回第2步。

        K-means 的做法是朴素且简单有效的，不过也存在一些问题，例如，需要指定聚类数量、随机选取中心点使得每次聚类结果都不相同，以及一些效率上的问题。随后，研究者们也开发出了层次聚类法、K-means++等算法以弥补它的不足，不过这都是后话，我们就不详细讨论了。总之，根据K-means，我们可以把已经提取的大量特征点聚类成一个含有k个单词的字典。现在的问题变成了如何根据图像中某个特征点查找字典中相应的单词。
        仍然有朴素的思想：只要和每个单词进行比对，取最相似的那个就可以了——这当然是简单有效的做法。然而，考虑到字典的通用性，我们通常会使用一个较大规模的字典，以保证当前使用环境中的图像特征都曾在字典里出现，或至少有相近的表达。如果你觉得对十个单词一比较不是什么麻烦事，那么对于一万个呢？十万个呢？

        也许读者学过数据结构，这种 $O(n)$ 的查找算法显然不是我们想要的。如果字典排过序,
那么二分查找显然可以提升查找效率，达到对数级别的复杂度。而实践中，我们可能会用更复杂的数据结构，例如 Fabmap中的Chou-Liu tree等。

使用一种k叉树来表达字典。它的思路很简单，类似于层次聚类，是K-means的直接扩展。假定我们有N个特征点，希望构建一个深度为d、每次分叉为k的树，那么做法如下：
1.在根节点，用 K-means把所有样本聚成k类（实际中为保证聚类均匀性会使用K-means++）。这样就得到了第一层。
2.对第一层的每个节点，把属于该节点的样本再聚成k类，得到下一层。

3.依此类推，最后得到叶子层。叶子层即为所谓的Words。

图11-4 K叉树字典示意图。训练字典时，逐层使用K-means 聚类。根据已知特征查找单词时,可逐层比对，找到对应的单词

         实际上，最终我们仍在叶子层构建了单词，而树结构中的中间节点仅供快速查找时使用这样一个k分支、深度为d的树，可以容纳 $k^d$ 个单词。另外，在查找某个给定特征对应的单词时，只需将它与每个中间节点的聚类中心比较（一共d次），即可找到最后的单词，保证了对数级别的查找效率。

11.3.2 创建字典

1.代码问题

Ⅰ.出现这个问题：

liuhongwei@liuhongwei-virtual-machine:~/桌面/slambook2/ch11/build$ make
make[2]: *** 没有规则可制作目标“/usr/local/lib/libDBoW3.a”，由“gen_vocab” 需求。 停止。
CMakeFiles/Makefile2:67: recipe for target 'CMakeFiles/gen_vocab.dir/all' failed
make[1]: *** [CMakeFiles/gen_vocab.dir/all] Error 2
Makefile:83: recipe for target 'all' failed
make: *** [all] Error 2

解决方法：修改CMakeLists.txt，

将这个地方的.a换成.so。

Ⅱ.出现这个问题：

liuhongwei@liuhongwei-virtual-machine:~/桌面/slambook2/ch11/build$ ./feature_training 
reading images... 
detecting ORB features ... 
creating vocabulary ... 
vocabulary info: Vocabulary: k = 10, L = 5, Weighting = tf-idf, Scoring = L1-norm, Number of words = 0
段错误 (核心已转储)

修改程序中的路径为绝对路径：

2.实践

        既然讲到了字典生成，我们就来实际演示一下。前面的视觉里程计部分大量使用了ORB特征描述，所以这里就来演示如何生成及使用ORB字典。

        本实验中，我们选取TUM数据集中的10帽图像，它们来自一组实际的相机运动轨迹。可以看出，第一幅图像与最后一幅图像明显采自同一个地方，我们来看算法能否检测到这个回环。根据词袋模型，我们先来生成这10张图像对应的字典。

图11-5 演示实验中使用的10幅图像，采集自不同时刻的轨迹

        需要声明的是，实用的字典往往是在更大的数据集上训练而成的，并且数据应来自与目标环境类似的地方。我们通常使用较大规模的字典——越大代表字典单词量越丰富，越容易找到与当前图像对应的单词，但也不能大到超过我们的计算能力和内存。我们暂时从10幅图像训练一个小的字典。如果想追求更好的效果，应该下载更多的数据，训练更大的字典，这样程序才会实用。也可以使用别人训练好的字典，但请注意字典使用的特征类型是否一致。

        接下来考虑训练字典：
#include "DBoW3/DBoW3.h"
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <iostream>
#include <vector>
#include <string>

using namespace cv;
using namespace std;

/***************************************************
 * 本节演示了如何根据data/目录下的十张图训练字典
 * ************************************************/

int main( int argc, char** argv ) {
    // read the image 
    cout<<"reading images... "<<endl;
    vector<Mat> images; 
    for ( int i=0; i<10; i++ )
    {
        string path = "/home/liuhongwei/桌面/slambook2/ch11/data/"+to_string(i+1)+".png";
        images.push_back( imread(path) );
    }
    // detect ORB features
    cout<<"detecting ORB features ... "<<endl;
    Ptr< Feature2D > detector = ORB::create();
    vector<Mat> descriptors;
    for ( Mat& image:images )
    {
        vector<KeyPoint> keypoints; 
        Mat descriptor;
        detector->detectAndCompute( image, Mat(), keypoints, descriptor );
        descriptors.push_back( descriptor );
    }
    
    // create vocabulary 
    cout<<"creating vocabulary ... "<<endl;
    DBoW3::Vocabulary vocab;
    vocab.create( descriptors );
    cout<<"vocabulary info: "<<vocab<<endl;
    vocab.save( "vocabulary.yml.gz" );
    cout<<"done"<<endl;
    
    return 0;
}
DBoW3的使用非常容易。我们对10张目标图像提取ORB特征并存放至vector容器中，然后调用DBoW3的字典生成接口即可。在DBoW3::Vocabulary对象的构造函数中，我们能够指定树的分叉数量及深度，不过这里使用了默认构造函数，也就是 $k = 10,d = 5$ 。这是一个小规模的字典，最大能容纳100,000个单词。对于图像特征，我们亦使用默认参数，即每幅图像500个特征点。最后，我们把字典存储为一个压缩文件。

运行此程序，将看到如下字典信息输出：
liuhongwei@liuhongwei-virtual-machine:~/桌面/slambook2/ch11/build$ ./feature_training 
reading images... 
detecting ORB features ... 
creating vocabulary ... 
vocabulary info: Vocabulary: k = 10, L = 5, Weighting = tf-idf, Scoring = L1-norm, Number of words = 4983
我们看到：分支数量k为10，深度L为5，单词数量为4983，没有达到最大容量。但是，剩下的 Weighting 和Scoring是什么呢？从字面上看，Weighting是权重，Scoring指的是评分。