任何一个SLAM系统，累积误差都是不可回避的问题。所以学者们想出了各种各样的办法尝试消除这种累积误差。对于视觉SLAM系统而言，主要通过视觉回环实现。更进一步来说，可以通过词袋模型实现。通过视觉词袋来描述场景，当两个场景词袋相似度较高，也就是说可能是同一个场景的时候，就认为平台在此时回到了自己曾经去过的某个地方。这样便可以构成一种约束。根据这种约束，将累积误差反推，就可以在一定程度上消除累积误差。在ORB-SLAM系统中回环具体是怎么实现的呢？回环检测与重定位又有什么关系？这也就是本篇笔记研究的问题。
1.词袋模型基础
在继续介绍之前，先介绍一下ORB-SLAM系统中的词袋模型。这里不涉及原理，只介绍具体实现过程。
1.1 字典的加载
只要你跑过ORB-SLAM系统，一定对”Loading ORB Vocabulary. This could take a while…“这句话不陌生，如下所示。
这其实就是在加载之前已经建好的ORB字典。对应System()构造函数中的相关部分，如下。
可以看到，新建了一个ORBVocabulary对象并赋给了System的成员变量mpVocabulary。然后，将传入的字典文件路径给成员函数loadFromTextFile()，从而加载字典文件。返回值为bool类型变量，指示加载是否成功。
1.2 词袋的计算
在加载好字典以后，当新进来一帧影像时，按照上面的逻辑就应该对这个影像计算词袋(可以看做是一种对影像的描述子)，然后和先前已有帧的词袋进行比较。那么计算影像的词袋这一步在ORB-SLAM系统里是在哪里实现的呢？
首先，在ORB-SLAM系统中有Frame和KeyFrame两种数据类型。它们都有ComputeBoW()成员函数，用于计算当前帧的ORB词袋。如下是Frame类中的ComputeBoW()函数。
可以看到，首先如果mBowVec成员变量为空，就尝试计算BoW向量。首先，我们调用Converter::toDescriptorVector()函数，将当前帧中ORB特征的描述子转成vector形式(形式转换)。然后，调用transform()函数，将描述子转换成对应的BoW词袋向量mBowVec、特征向量mFeatVec。这里简单解释一下词袋向量和特征向量的意义。
词袋向量比较好理解，可以看作是用来描述影像的向量，构成为：(字典单词索引1，权重1),(字典单词索引2，权重2),(字典单词索引3，权重3)……。计算影像间的相似度其实就是计算影像间BoW向量的距离。在ORB-SLAM系统中，BoWVector的构成为std::map<WordId, WordValue>。
特征向量则是用于加速图像的匹配。理解特征向量的作用之前，需要先了解一些思路。我们可以把一幅影像中的多个特征点按照描述子分成很多簇，每个簇都有ID，也就是NodeID。这个分割描述子的标准由字典提供，所以在各个影像之间是统一的。这样，比如我们有影像1，提取了100个特征点。按照描述子可以将其分为：节点1包含20个特征点，节点2包含50个特征点，节点3包含10个特征点，节点4包含20个特征点。同理，我们有影像2，提取了200个特征点。按照描述子分为：节点1包含50个特征点，节点2包含70个特征点，节点3包含80个特征点，节点4包含0个特征点。这样，当我们要匹配这两个影像时，从节点1开始，获取影像1中属于节点1的特征点(这里是20个)，再获得影像2中属于节点1的特征点(这里是50个)。我们只要做20对50个特征点的匹配就可以了。做完以后，同理再看节点2，以此类推。这样做的好处就是，可以有效减少匹配计算量，提升匹配效率。举个简单的例子，如果我们采用暴力匹配。对于100对200个特征点的匹配，我们要计算100×200=20000次。而如果对特征点提前分簇，我们要做的计算次数就是20×50+50×70+10×80+20×0=5300。可以看到，减少了几乎四分之三。所以这个策略还是很有效果的。那么回到对特征向量的介绍上。FeatureVector类型本质上是一个std::map，映射的是uint类型的NodeId和vector<unsigned int>。换句话说，对于一张影像中的特征点而言，它保存的是节点ID和特征点在影像中对应的索引。举个例子，比如我们提取了10个特征点，有4个节点。那么mFeatVec包含的内容就是这个样子：
0:{5,2,9}
1:{3,1}
2:{4,8}
3:{6,7}
当我们需要的时候，就可以根据索引直接获取到对应特征点。比如我们要获取到Node0节点下的特征点，那么直接拿着5、2、9这三个索引去mDescriptors里找就能获得对应的描述子了。
至此，我们基本上就把ORB-SLAM系统中的ComputeBoW()函数、BoW向量和特征向量的组成和计算基本说清楚了。
所以回答本部分开始的问题，我们只需要分别找到Frame和KeyFrame的ComputeBoW()在哪里被调用了即可。
1.2.1 Frame类
首先是Frame类的ComputeBoW()函数，调用如下。
可以看到，主要有两处调用，都在Tracking类中。一个是在Relocalization()函数，一个是在TrackReferenceKeyFrame()函数。当然了，如果你点进去这两个函数就会发现，其实它们计算ORB词袋和回环没多大关系。倒不如说这里ORB词袋是用来“查找”或者说“搜索”的。下面分别解释。
(a) TrackReferenceKeyFrame()
如下图所示，在TrackReferenceKeyFrame()函数中先计算了当前帧的词袋，然后将其和参考关键帧的词袋进行匹配，企图得到匹配点对。
(b) Relocalization()
如下图所示，在Relocalization()函数中，先计算了当前帧的ORB词袋，然后拿着这个词袋去关键帧数据库中取匹配，找到相似度最高的几个，尝试重定位。关于重定位，会在后续部分进一步介绍。
1.2.2 KeyFrame类
KeyFrame类的ComputeBoW()函数，调用如下。
可以看到主要在LocalMapping线程和Tracking线程中被调用，下面分解介绍。
(a) LocalMapping
在LocalMapping中，主要是在ProcessNewKeyFrame()函数调用，如下图所示。
可以看到，对当前关键帧计算了ORB词袋。而ProcessNewKeyFrame()函数则是在LocalMapping的Run()函数调用。
(b) Tracking
在Tracking中，主要在CreateIntialMapMonocular()函数中调用，如下图所示。
可以看到，在函数中分别对初始关键帧和当前关键帧计算了ORB词袋。
可以看到，对于关键帧而言，主要是在LocalMapping的ProcessNewKeyFrame()函数中做的。
事实上，在ORB-SLAM系统中，回环检测中比较的关键帧之间的词袋，而非每一帧的词袋。换句话说，回环检测是针对关键帧而言的，对每个新的关键帧都会做，而非普通帧。这点需要明确一下。这一点在后面的部分也会进一步介绍。
但不管怎么说，经过上面的分析，我们就找到了ORB-SLAM系统中计算ORB词袋的实现。
2.回环检测
在介绍完上面的词袋模型基础后，我们就要进入回环检测正题了。其实SLAM中的回环可以分为两个方面：回环检测和回环校正。回环检测用于检测是否构成回环，回环校正则是检测到回环以后后续的优化步骤。本篇笔记重点放在前半部分。
2.1 回环检测线程的创建与启动
在ORB-SLAM系统中，回环检测由LoopClosing线程单独负责。该线程和Tracking、LocalMapping线程一样，在System类的构造函数中创建，如下。
可以看到，同样的，回环检测核心的操作都是在Run()函数中做的，如下图所示。
可以看到，在ORB-SLAM系统中，通过DetectLoop()函数实现回环的检测，通过CorrectLoop()函数实现回环的校正。下面进一步分析前者。
2.2 回环的初步判断
如果你实际跑过ORB-SLAM，可能会有一些疑惑。为什么有些场景明明看着很像，却没有检测到回环；而有些场景看起来不是那么相似，系统却检测到了回环。事实上，在ORB-SLAM系统中，是否成功回环不仅仅考虑了场景的相似性，还有其它一些因素。这个问题的所有答案都在DetectLoop()函数中。所以这一部分对于该函数重点分析。
2.2.1 步骤1：统计地图中关键帧个数
如下图所示，如果当前地图包含小于10个关键帧或者距离上一次回环少于10个关键帧，函数直接返回false，也就是不构成回环。
换句话说，即使某两个关键帧很相似，但是地图中关键帧个数小于10个，那么也不认为是回环。这样做其实也可以理解，对于回环检测而言，我们想知道的是当前场景是不是在很久之前见过。而对于刚见过不久的场景，其实对整个场景的误差校正作用是有限的。
2.2.2 步骤2：计算BoW相似得分
如下图所示，我们首先获得当前关键帧的共视关键帧，并且获得当前关键帧的ORB词袋。
然后依次遍历共视关键帧，计算当前关键帧与共视关键帧ORB词袋的相似得分，记录下最小的得分。这个得分其实可以看做一个动态阈值。只有相似度大于这个得分的关键帧才有可能被作为回环关键帧。之后再调用DetectLoopCandidates()函数寻找潜在的回环关键帧。如果返回值为空，也就是说一个相似的都没有(或者没有相似度大于最小得分的关键帧)。返回为false，否则继续判断。这里涉及到了KeyFameDB，这里暂时可以不用管，在后面会进一步介绍。
2.2.3 步骤3：共视一致性检查
如果DetectLoopCandidates()函数返回了几个潜在的回环关键帧，下面就对它们进一步判断，如下所示。
下面详细解释。
首先，在上一步我们获得了当前关键帧mpCurrentKF的潜在回环关键帧并放到了vpCandidateKFs中。如果其为空，则认为不构成回环，如果不为空，则迭代判断。事实上后续所有的操作都是在围绕vpCandidateKFs展开的。
如上图所示，首先，我们新建vCurrentConsistentGroups和vbConsistentGroup这两个vector。这里涉及到两个需要说明的地方。第一个是ConsistentGroup是个什么类型，第二个是mvConsistentGroups从哪里来。先回答第一个问题。ConsistentGroup是在LoopClosing的头文件中自定义的数据类型，原始形式为pair<set<KeyFrame*>,int>。第二个问题。其实我们会在遍历完vpCandidateKFs后将新建的vCurrentConsistentGroups赋给mvConsistentGroups。所以对于第一次运行而言，mvConsistentGroups是空的，也就对应着vbConsistentGroup长度为0。
如上图所示，新建完了vCurrentConsistentGroups和vbConsistentGroup这两个vector以后，我们就开始遍历vpCandidateKFs。首先，我们根据索引获取到某个潜在回环关键帧并赋给pCandidateKF。然后又调用KeyFrame的成员函数GetConnectedKeyFrames()获取到与其相关联的关键帧，并将其赋给set类型的spCandidateGroup。然后我们把它本身也放入这个set。
如上图所示。然后我们新建了bEnoughConsistent和bConsistentForSomeGroup两个变量用于指示状态，默认都为false。
然后开始遍历上面提到的mvConsistentGroups，根据索引依次获得上一帧对应的一致组，将其赋给sPreviousGroup。然后，再遍历当前潜在回环关键帧所对应的一致组spCandidateGroup。看一看当前一致组中的某个关键帧在上一个一致组中是否存在。如果存在的话，bConsistent和bConsistentForSomeGroup都设为true，并且结束本次对于当前一致组spCandidateGroup的迭代。然后，根据bConsistent的状态判断是否执行后续步骤。如果其为false，则直接结束本次对mvConsistentGroups的迭代，进入下一次迭代。而如果其为true的话，则根据索引获取到一致性分数赋给nPreviousConsistency，并在此基础上加一，赋给nCurrentConsistency。如果我们之前新建的vbConsistentGroup中对应位置没有元素或者为false，则将当前潜在回环关键帧的spCandidateGroup以及一致性分数放到vCurrentConsistentGroups中，并且将vbConsistentGroup中对应位置设为true。进一步，如果当前一致性分数大于给定的阈值(ORB-SLAM系统中为3)，并且bEnoughConsistent为false(默认值)，那么就将这个潜在回环关键帧放到LoopClosing的成员变量mvpEnoughConsistentCandidates中，并且将bEnoughConsistent设为true。最后，如果bConsistentForSomeGroup为false，那么就把当前潜在回环关键帧的spCandidateGroup以及一致性分数(0分)放到vCurrentConsistentGroups中。结束本次对于vpCandidateKFs的迭代。最后，我们把vCurrentConsistentGroups的内容赋给mvConsistentGroups，更新一下。这里其实可以看到，如果mvConsistentGroups为空，那么所有迭代其实都不会执行。
上面说的过程，嵌套、迭代比较多，所以需要仔细理解一下才能明白大致意思。为了更便于理解，制作了一个小动画，如下图所示。
上图展示了添加潜在回环关键帧的过程。
最后，我们将当前关键帧添加到关键帧数据库中。并且判断mvpEnoughConsistentCandidates是否为空。如果不为空，就说明存在经过筛选的回环关键帧，返回true，否则就返回false。对于回环检测而言，如果上面的三个步骤有任何一个不通过，那么DetectLoop()就返回false，那么在Run()函数中，就不会执行后续的回环校正流程，直接进入下次迭代了。而如果DetectLoop()成功了，也就是说，我们获得了几个可能的回环关键帧(mvpEnoughConsistentCandidates不为空)，我们就从几何角度进行进一步判断，对应的也就是ComputeSim3()函数。
2.3 回环的进一步判断
上面说了，通过一些初步筛选，我们获得了几个潜在的回环关键帧。在这一步，我们就通过几何的方式看看这几个关键帧是否真的可以担负起“回环”的责任。这些都是在ComputeSim3()中做的，如下图所示。
可以看到，只有DetectLoop()和ComputeSim3()两个函数都通过了，才会进入CorrectLoop()进行最终的回环校正。前面分析了DetectLoop()，下面则对ComputeSim3()函数进行分析。
完整的ComputeSim3()函数如下所示。
2.3.1 准备操作
首先，我们获取DetectLoop()函数输出的潜在回环关键帧的个数，并且为每一个关键帧都配一个SimeSolver、地图点vector、是否抛弃的flag变量。进行一系列准备工作。如下图所示。
2.3.2 基于匹配点筛选潜在回环关键帧
然后，我们遍历潜在回环关键帧vector，也就是DetectLoop()函数修改过的mvpEnoughConsistentCandidates，如下图所示。
我们根据索引获取对应的潜在回环关键帧，并建立一个临时变量pKF存储。如果这个关键帧是坏的，那么直接跳过后续步骤(continue)，进行下一次迭代。否则，就调用SearchByBoW()函数对当前关键帧和潜在回环关键帧进行匹配。返回值包含匹配到的地图点(vector形式)以及个数nmatches。如果个数小于20，这个潜在回环关键帧就垮掉，执行下一次循环(continue)。如果多于20个的话，则构造相似变换求解器(Sim3Solver类的对象)、设置RANSAC参数，并把这个求解器放到vpSim3Solvers中，方便之后调用。当然了，最后把累计变量nCandidates加一，表示剩余的潜在回环关键帧个数。
2.3.3 多次RANSAC筛选潜在回环关键帧
到上面一步，我们还是有可能有多个潜在回环关键帧。所以这一步的核心目标就是从剩下的多个关键帧中选择一个最好的。我们对每一个关键帧计算相似变换，如下。
可以看到，我们遍历mvpEnoughConsistentCandidates。首先，看一看某一关键帧在上一步的检测中有没有通过(如果没通过，那么对应的vbDiscarded被赋值为false)。如果没有通过，就直接跳过本次循环后面的步骤，否则继续执行。取出某个关键帧并赋给pKF。并且取出在上一步中构造好并保存到vpSime3Solvers中的相似变换求解器，赋给pSolver。利用该求解器，迭代5次，求解相似变换矩阵Scm。当然了，除了相似变换矩阵，还返回了是否达到了最大迭代次数(有一种情况是，迭代了5次，但是还是不收敛，求解的变换矩阵有问题)。如果真的出现了这种情况，bNoMore就为true。这个时候，我们也直接丢弃这一个潜在回环关键帧，对应累计变量减一，如下图所示。
而如果上述流程一切正常并且求解的相似变换矩阵Scm也不为空，尝试用这个变换进行检查，如下。
简单来说就是，我们获取估计的旋转、平移、尺度，然后利用SearchBySim3()函数对当前关键帧和待选回环关键帧进行地图点匹配，结果返回给vpMapPointMatches。并且调用OptimizeSim3()函数对相似变换矩阵进行优化，结果返回给gScm，并返回优化后剩余的地图点个数。如果剩余的地图点个数大于20个，即认为成功，将bMatch设为true，将当前潜在回环关键帧pKF赋给mpMatchedKF，匹配地图点vpMapPointMatches赋给mvpCurrentMatchedPoints，并计算全局相似变换矩阵mScw。可以看到，最终以break退出for循环。此时，会到while循环，但是由于不再满足bMatch条件，所以也会退出while循环。这样，就相当于我们终于从几个潜在回环关键帧中找到了最好的关键帧，作为回环关键帧。
2.3.4 几何检测
在上一步，我们终于不断缩小范围，找到了一个回环关键帧，也就是mpMatchedKF，如下图所示。
我们调用GetVectorCovisibleKeyFrames()函数获取连接关键帧，放到vpLoopConnectedKFs中。同时也把它自己加进来。然后遍历每个关键帧，把关联的地图点添加到mvpLoopMapPoints中。
如上图所示，然后，我们尝试用SearchByProjection()函数基于求解的相似变换进行投影，找到更多的匹配地图点。统计地图点的个数。
最终，我们判断总共匹配的个数，如下图所示。
如果匹配的地图点个数大于40，即认为回环成功，返回true；否则回环失败，返回false。
2.4 什么是KeyFrameDB
在前面，我们多次提到了关键帧数据库这个概念。它到底是什么东西呢？它也是一个类，叫做KeyFrameDatabase，有对应的头文件和源码文件，如下。
简单来说，它是一个用于存放关键帧的类，而且主要用于回环检测与重定位。通过查找函数的调用也可以看出来，如下。
添加关键帧到数据库的操作只在DetectLoop()函数中执行了。而根据上面的分析可以发现，对于每一个传给LoopClosing的关键帧，都会执行DetectLoop()函数。在DetectLoop()函数中，不管成功与否，都会将当前关键帧添加到数据库中。
回到一开始的问题：为什么一定要建个所谓的数据库呢？答案是为了更方便的检索与匹配。如果我们建了个数据库，将每一个关键帧都添加进来，这样当有新的关键帧来的时候，我们就将其与数据库中的其它关键帧进行匹配，就可以比较容易地找到相似关键帧。这也是一种“面向对象”设计思想的体现。
这里我们不妨稍微再深入研究一下是如何添加关键帧的？就是建了一个vector然后把KeyFrame对象放进去吗？答案在KeyFrameDatabase::add()函数中，如下。
可以看到，这里我们有一个成员变量mvInvertedFile用于接收传入的pKF。它是什么呢？它的类型是std::vector<list<KeyFrame*>>。对于传入的关键帧pKF，我们遍历它的BoW向量的每个Word。如上面1.2部分所说，每一个Word由字典单词索引+权重构成。这里我们通过first，获取到的就是字典单词的索引。也就是说，这里字典单词的索引被作为了一种ID，传给mvInvertedFile。系统根据不同的ID，找到不同的list，将传入的pKF放进去。所以可以发现，对于某个关键帧，它会被添加多次，放到数据库中。次数等于它对应的BoW向量包含的单词个数，它在数据库中的位置等于单词在字典中的索引(稍微有点绕，理解一下)。所以可以看到，关键帧数据库并不是简单地将输入关键帧push_back()进来，而是根据关键帧BoW向量包含的单词进行添加。
那么这样你可能就会有个疑问，这样构建出来的是个什么东西？具体而言mvInvertedFile的组织形式是什么样的。答案是它就变成了一个类似于灰度直方图的东西。横轴是字典单词的索引，纵轴是包含这个索引单词的关键帧。为什么要这样“麻烦”做呢？答案是为了之后检索的方便。因为前面也说了，构造这个数据库就是为了回环或者重定位，而对于ORB-SLAM系统而言。回环或者重定位的本质其实就是根据视觉描述子来判断两个场景相不相似。所以视觉描述子或者说单词就至关重要。在实际回环或者重定位过程中，我们只需要拿着某个单词，去数据库中查找，系统就会返回给我多个包含该单词的关键帧。不仅如此，如果我们查询某个关键帧中包含的多个单词，并且找到所有返回的关键帧中次数最多的那个，那就等于是与查询向量最相匹配的关键帧。这也就完成了基于相似性的查找。这样不是很方便吗？
这里最后还有个小知识点，vector和list的区别。上述基于vector和list构建的这种二维数据结构，利用两个vector可以做吗？答案是肯定的。但效率会低很多。简单来说，vector适合对象数量变化少、对象简单、随机访问元素频繁的场合；list适合对象数量变化大、对象复杂、插入和删除操作频繁的场合。基于这两个特性，也就可以理解为什么这里用vector和list的组合了。因为字典单词个数基本是不会变的，而且我们将其作为查询索引，随机访问是很常见的，所以使用vector十分适合。而对于某个索引的所包含的关键帧，由于需要频繁的插入操作，这十分适合list。所以最终作者选择了vector和list的组合。
2.5 如何挑选潜在回环关键帧
在前面，我们多次提到通过调用KeyFrameDatabase::DetectLoopCandidates()函数挑选潜在回环关键帧。怎么做的呢？是这一小节主要讨论的问题。函数的内容如下。
下面对具体步骤进行简单分析。
2.5.1 获取具有共视关系的关键帧
首先需要明确的是，我们的目的是返回查询关键帧的潜在回环关键帧，那么基本原则是相似但时间上间隔较远。所以对于那些直接和查询关键帧相关联的关键帧可以直接“抛弃”了(因为太近了)。另外，既然是“相似”，那么两个关键帧的ORB词袋之间至少有一点点的关联，不然也不叫相似了。上面说的也就是这个函数首先做的事情，如下图所示。
首先，我们获取查询关键帧pKF的BoW向量，并且遍历其中的每个单词。获取这个单词在字典中的索引，然后根据这个索引在关键帧数据库中找到所有包含这个单词的关键帧并放到lKFs中。然后，我们遍历这些包含共同单词的关键帧pKFi。首先判断这个关键帧的mnLoopQuery是不是和pKF的ID相同。如果不相同的话，就首先将pKFi的mnLoopWords设为0。然后，看看这个关键帧在不在查询帧pKF的连接关键帧中。如果不在的话，就该关键帧的mnLoopQuery设为pKF的mnId。并且，将这个关键帧添加到lKFsSharingWords中。这里面还有个比较重要的变量mnLoopQuery。这个变量在哪里被赋值的？答案是，只在上图中的地方被赋值了。它是KeyFrame类的成员变量，初始值为0。
2.5.2 根据BoW相似性进行筛选
在获取到了潜在回环关键帧列表lKFsSharingWords后，下面就开始遍历它进一步筛选了。如下图所示。
如果一个都没有，那么直接返回空vector，否则继续判断。首先，可以看到，我们遍历这些潜在关键帧，获取mnLoopWords属性。找到最大的值，赋给maxCommonWords。并且将其乘以0.8，得到minCommonWords。下面就开始遍历这些关键帧，计算得分。
首先，我们看遍历的某个关键帧pKFi的mnLoopWords属性是否大于minCommonWords。只有大于这个值才进一步判断。我们调用score()函数，计算查询关键帧pKF的BoW向量和遍历关键帧pKFiBoW向量之间的相似得分，赋给si，进一步赋给pKFi的mLoopScore属性。如果得分大于minScore，则认为是一个比较可靠的关键帧，和得分一起，添加到lScoreAndMatch列表中。并且类似的，如果它为空，也是直接返回一个空的vector。那么这里这个最小得分阈值是从哪来的呢？答案是DetectLoopCandidates()函数的传入参数之一。进一步，我们可以查找调用它的DetectLoop()函数。可以发现，它的默认值为1。
2.5.3 根据共视关系进行筛选
到这里为止，我们通过与输入关键帧的BoW向量相似性进行了筛选。进一步，我们通过共视关系再筛选，如下图所示。筛选的结果放到lAccScoreAndMatch。
具体来说，我们遍历上一步得到的lScoreAndMatch，获得某个关键帧pKFi。然后调用成员函数GetBestCovisibilityKeyFrames()获得其最优的10个共视关键帧，放到vpNeighs中。然后我们遍历这些vpNeighs中的共视关键帧pKF2，并且进行判断，并累加回环得分。这里在做的事情其实是，拿到一个关键帧，并且获得和它最共视的10个关键帧，共11个关键帧。计算回环得分，这11个里面得分最高的那个关键帧作为最优关键帧pBestKF并记录对应的得分accScore，放到lAccScoreAndMatch中。那么这一步结束以后，其实可以发现，待选回环关键帧的数量是不会增加或者减少的，但可能会存在被替换的情况。
最后，我们根据回环得分，再筛选一轮，如下图所示。
我们计算最好得分的0.75倍，作为筛选阈值minScoreToRetain。然后，遍历上面更新过的lAccScoreAndMatch。大于阈值的会保留下来，添加到vpLoopCandidates中。这样潜在回环关键帧的搜索就完成了。当然可以看到，这里为了避免重复进一步做了个判断。
2.6 小结
至此，我们就完整介绍了ORB-SLAM系统中的回环检测相关实现。我们不妨更仔细地回答一下一开始的问题：ORB-SLAM系统中回环检测是如何实现的？什么情况下会判定为回环？
针对第一个问题，大体来说，ORB-SLAM系统中主要利用BoW词袋模型来进行回环检测，并且辅以几何检查进一步确保回环的可靠性。稍微细节来说，由DetectLoop()、ComputeSim3()两个函数负责。前者主要基于词袋进行关键帧筛选，后者则在前者的基础上，基于几何检验进一步筛选待选关键帧，并最终确定一个关键帧作为回环关键帧(或者一个都没有)。
针对第二个问题，我们还是从DetectLoop()、ComputeSim3()两个函数中寻找答案。大体来说，可以总结为：视觉场景相似并且几何变换稳定的关键帧才能构成回环。为了更加直观地展示回环检测的逻辑，结合上面的分析绘制了下面的流程图。
可以看到，在ORB-SLAM系统中，要想真正形成一个回环要满足的条件还是很多的，并非只是单纯的“长的像”就可以了，为了鲁棒的性能,还是要考虑到多种情况。
3.重定位
在前面的部分，我们详细分析了ORB-SLAM系统中回环检测相关内容。而这一部分则是回答本文开头的第二个问题：什么是重定位？和回环检测有什么不同。
3.1 什么是重定位
重定位顾名思义就是“重新定位”。在ORB-SLAM系统中，重定位在LOST状态的时候才会尝试执行。其作用也是显而易见的：如果某一时刻逐帧的Tracking失败了，我们就会利用构建的关键帧数据库尝试找到与当前帧最相似的几个关键帧，然后逐个遍历尝试与当前帧匹配。如果匹配成功，即表示重定位成功，否则就一致尝试重定位。具体而言，在Tracking类中有专门的Relocalization()用于重定位，如下图所示。
3.2 重定位的流程
3.2.1 计算BoW向量
在重定位函数中，首先就调用ComputeBoW()计算了当前帧的BoW向量。关于ComputeBoW()函数，在1.2部分已经说过了，这里就不再赘述。
3.2.2 获得潜在重定位关键帧
然后，调用KeyFrameDatabase::DetectRelocalizationCandidates()函数获取当前帧的潜在重定位关键帧，并放到vpCandidateKFs中。当然了，如果一个都没有，直接返回false，结束函数，否则就获得潜在重定位关键帧的个数，并赋给nKFs，如下图所示。
当然也可以看到，这里的核心函数就是这个DetectRelocalizationCandidates()，它其实和前面提到过的DetectLoopCandidates()是类似的。本质上来说都是通过BoW向量的相似性在关键帧数据库中查找关键帧，如下所示。
事实上，你会发现整个函数的流程和DetectLoopCandidates()几乎相同，还是遵循三个大的步骤：获取具有共视关系的关键帧、根据BoW相似性进行筛选、根据共视关系进行筛选。所以更多细节的部分可以参考2.5部分，此处就不再赘述。主要有两点不一样的地方。第一点是在回环函数中，我们通过KeyFrame的成员函数GetConnectedKeyFrames()获取了与当前帧连接的其它关键帧并赋给spConnectedKeyFrames，而这个操作在重定位函数中则是没有的。第二点是，重定位函数不需要传入minScore这个参数。这个参数在回环函数中是用于过滤掉一些相似性得分较低的关键帧的。但在重定位函数中则没有这种阈值判断。这就是这两个函数最重要的不同之处。
这里额外简单对第一点做一下补充。在回环函数中，之所以把和当前关键帧直接连接的关键帧去掉，其目的在于保证时间上的间隔尽可能长一点，不然很容易就有很多可能的关键帧。但在重定位中，则并没有这种时间上的限制。只要关键帧数据库中有和当前帧相似的关键帧，我就先拿过来作为备选。
3.2.3 根据BoW匹配筛选潜在关键帧
在获得了一堆潜在重定位关键帧以后，我们就逐个遍历它们，并尝试用SearchByBoW()函数对潜在关键帧与当前帧进行匹配，匹配的地图点个数返回给nmatches，如下图所示。
可以看到，这里其实和ComputeSim3()函数的匹配(2.3.2部分)有一定相似性。都是通过BoW进行匹配，并且看匹配点的个数，如果小于阈值跳过本次循环，否则就构造一个PnP求解器。这个所谓的PnP求解器是PnPsolver类的对象。事实上，ORB-SLAM系统中的Sim3Solver和PnPSolver这两个类，一个用于回环检测中的ComputeSim3()函数，另一个用于重定位的Relocalization()函数。除此之外，就没有在其它地方被调用了。
3.2.4 尝试求解位姿并验证
这一步和ComputeSim3()函数的匹配(2.3.3部分)有一定相似性，但简单一些。简单来说，我们上面对于潜在的重定位关键帧构造好了PnP求解器。所以这里我们就尝试使用该求解器对每个关键帧求解位姿。如果某个位姿经过优化过滤对应的地图点较多，就认为重定位成功，否则判定为失败，如下图所示。
可以看到，对于每一个潜在重定位关键帧，我们执行5次迭代，得到变换矩阵Tcw。当然这里采用了和2.3.3部分一样的思想，如果迭代了5次还不收敛，就抛弃这个关键帧。假设我们在5次以内顺利地算出了位姿，然后就是对位姿进行验证，如下。
调用PoseOptimization()函数尝试对位姿进行优化，并返回优化后的地图点个数nGood，如果其小于阈值(如10)，则跳过本次循环，认为这个关键帧不可靠。而如果其大于10，小于50，我们就会用一个相对宽松的范围再计算一遍位姿。如果最终地图点的个数大于等于50，就认为该关键帧比较可靠，并且停止迭代(bMatch设为true，break)。
最终，判断bMatch。如果其为true的话，即认为重定位成功，函数返回true，并把当前帧的ID记录到mnLastRelocFrameId中。
至此，我们就把重定位的基本流程介绍完了。从上面的分析也可以回答什么条件下重定位才能成功。
3.3 重定位和回环检测有什么异同
那么重定位和回环检测有什么不同呢？我们可以从两个方面来回答。
首先是目的。重定位的目的是为了在LOST以后可以继续Tracking，而回环检测则是为了检测是不是到了曾经去过的地方，构成回环。不同的目的导致了它们对于“成功”的判断标准不同。总体而言，重定位的标准要松一些。而回环检测为了保证在时序上的间隔，增加了额外的约束(把与查询帧直接连接的关键帧剔除)。此外，重定位也没有把当前关键帧数据库中关键帧的个数作为评判指标。因为重定位随时都有可能发生，可能是在程序刚启动的时候。而回环检测必须要有一定数量的关键帧后才能构成回环，不然回环就不是特别有意义。
第二，从实现上来说。在ORB-SLAM系统中，重定位函数Relocalization()在Tracking类中定义，而回环检测函数DetectLoop()则是在LoopClosing中定义。可以看到，重定位其实是作为Tracking线程的一部分在运行，而回环则是LoopClosing线程的核心函数之一。重定位函数可以把它看做是和TrackWithMotionModel()、TrackReferenceKeyFrame()函数同级的用于位姿估计的函数。
4.参考资料
- [1] https://blog.csdn.net/potxxx/article/details/87888518
- [2] https://blog.csdn.net/u010821666/article/details/52915238
- [3] https://blog.csdn.net/icameling/article/details/80923777
- [4] https://blog.csdn.net/zhaodeming000/article/details/105235271
本文作者原创,未经许可不得转载,谢谢配合
