Orb-SLAM回环检测的代码思路

部分转自博客链接．

一. DetectLoop() (检测回环)：

LoopClosing.cc中DetectLoop()函数

（1）首先从队列中取出一个关键帧，判断：如果距离上次闭环没多久（小于10帧），或者map中关键帧总共还没有10帧．如果是的话，则不进行闭环检测。在数据库里把当前关键帧加上就好了．

if(mpCurrentKF->mnIdadd(mpCurrentKF);
｝

否则就开始回环检测，遍历所有的共视关键帧计算所有共视关键帧的得分。步骤如下：

（2）mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore)这个函数就是用于选择出候选关键帧的，具体操作（包括了第3 步骤）：将与当前帧相连的局部关键帧剔除然后遍历所有关键帧，找出与当前关键帧具有相同单词的关键帧，然后统计所有闭环候选帧中与当前关键帧具有共同单词最多的单词数，将最多单词的80%设置为阈值，然后找出所有单词数超过阈值，且相似度检测大于相邻关键帧最低分数的关键帧。

　　　在这之前的工作：

　　　第一小步：检测与当前帧相连的局部关键帧．保存在vector　vpConnectedKeyFrames里面．（为了找出阈值minScore）（vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames();）这里跟后面不太一样

　　　第二小步：计算当前帧与相邻帧局部关键帧的分数找出最低值存在float minScore里面．

　　　第三步：跳转DetectLoopCandidates(mpCurrentKF, minScore)函数，其中mpCurrentKF是当前帧

（3）KeyFrameDatabase.cc中的　DetectLoopCandidates(mpCurrentKF, minScore)：

第一步：遍历mvInvertedFile的单词，将所有跟当前帧有共同单词的帧都保存在list lKFsSharingWords;里面（备注一）除去保存在　set spConnectedKeyFrames = pKF->GetConnectedKeyFrames();　里的相邻帧．

　　　其中int mnLoopQuery保存的是一个关键帧当前是谁的回环候选帧，保存这个谁的mnID　　mnLoopWords表明该帧跟当前帧有多少个共同单词

第二步：从lKFsSharingWord找出符合要求的帧　保存在list > lScoreAndMatch;　　pair对是KF和它的分数．　

要求：关键帧的单词数超过阈值且相似度检测大于相邻关键帧最低分数minScore

准备工作：先找到跟当前帧共视单词最多的单词数，将最多单词的80%设置为阈值

（４）将这些关键帧和与他自己相邻最紧密的前10个关键帧设定为一组（注意这里的组是以每一个关键帧为中心，加上与其相邻的关键帧所形成的，一个关键帧可以在多个组里面）。然后计算每组的总得分以及每组得分最高的关键帧．

bestAccScore保存最好的那一组的分数和．

pKFi->GetBestCovisibilityKeyFrames(10)获取相邻最紧密的前10个关键帧

计算每组的总得分和最高的关键帧，意思就是最高分的关键帧可能不是原候选帧．最高分的帧满足三个条件如下：

pKF2->mnLoopQuery==pKF->mnId //该帧是跟当前帧有共同word的
pKF2->mnLoopWords>minCommonWords　//共同word要超过0.8倍的最大值
pKF2->mLoopScore>bestScore　//比当前已知的最高分要高．

最后保存到lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));里面，得到每组的总得分以及每组得分最高的关键帧．

（５）时间一致性检验：以组得分最高的0.75作为阈值，找出高于这个阈值的所有组里面得分最高的帧，作为候选帧．此时这个函数结束，返回到detectLoop中

 set spAlreadyAddedKF; //用来排除相同的帧．因为set有唯一性
 vector vpLoopCandidates;　//保存候选结果

（６）然后进行几何一致性检验。一致性检验通过维护一个连续性的Groups，每次检测到回环就检查当前候选关键帧是不是跟之前连续，如果连续，那么连续性＋＋，否则Groups全部清零，重新再来．

所谓的连续性，就是当前的候选关键帧，获取它全部的相连帧包括他自己，形成一个组．看之前的组跟现在这个组有没有重合，有一帧重合就ok 

mvpEnoughConsistentCandidates	vector	存放一致性超过阈值的帧 当它不为空的时候　闭环就可以进入下一个阶段了　传递给compute sim3的下一个对象
vCurrentConsistentGroups	vector	当前这个循环中保留有一致性的帧．１）一致性从上一个循环维护的mvConsistentGroups继承，一致性＋１ ２）如果循环中没有一致性的帧，那么一致性被置０，候选帧还是会被放进去留作下一次的用途．
mvConsistentGroups	vector	始终被维护更新：1)刚开始被拿来比较，看当前的候选关键帧是否保持有一致性． 2)最后被vCurrentConsistentGroups更新，保存新的连续性帧

bool型工具

bEnoughConsistent	现在一致性已经满足条件了 可以去闭合回环了
bConsistentForSomeGroup	当前帧的组跟之前的组有一致性（用作稍大循环）
bConsistent	当前帧的组跟之前的组有一致性（用作小循环）
vector vbConsistentGroup	每一位代表一个组是否被检测到有新的连续性

参考博客说这一部分就是＂通过两个for循环将当前帧及其相邻帧与候选关键帧及其相邻帧进行匹配(??)，也就是是否有相同的相邻关键帧，如果大于3那么就认为当前帧通过检测。但是此时还是有一些候选的关键帧。＂也是对的，但是会让第一次看的人以为是直接对当前帧的相邻帧来做匹配，其实不是的，整个操作是一个连续的时间行为．需要三次走这个程序才能完成．

 

 

ComputeSim3()(Sim3求解):对于每一个候选帧都进行求解。首先通过BOW进行匹配，如果改帧的匹配特征点少于20，则直接删除，然后进行sim3求解，求解之后进行inliner检测，只要有一次的是合格的就可以返回，最多只能迭代5次。然后通过求得的Sim3完善通过BOW进行的匹配，然后算出大致的匹配区域再进行Sim3优化，在优化过程中，先迭代5次，然后剔除误差过大的边（卡方检验，这里设定的阈值是10），然后继续对剩下的边进行优化，然后得到优化结果，只要优化后的nInliers大于等于20就认为通过回环检验。同时停止对其他候选帧的优化。然后取出闭环匹配上关键帧（也就是刚刚通过回环检验的这一帧）的相邻关键帧提取出来得到一个集合（同时把匹配上的关键帧也加入到这个集合中），再把它们所对应的Map Point取出来（这里也包括匹配上的关键帧的Map Point，不过这个需要标记一下，避免重复添加），得到一个集合。然后把这些Map Point全部投影到当前关键帧上，进行匹配，根据Sim3确定一个大致区域，然后在附近区域搜索，得到匹配。然后根据匹配判断是否超过40个匹配点，如果超过则认为匹配成功，否则将有Local Mapping送进来的队列清空，等待下一次。 

CorrectLoop()（闭环校正）：首先通知局部地图，让他停止关键帧的插入。然后根据更新之后的共视关系更新当前帧与其它关键帧的联系，根据位姿传播得到与当前帧相连关键帧闭环后的Sim3，然后根据得到的闭环Sim3（也就是每一个关键帧需要调整的位姿）更新Map Point的位置，将sim3转换为SE3，修正闭环帧的位姿。然后再根据共视关系，重新更新各个关键帧的连接关系。然后再检查当前帧的Map Point与闭环匹配帧的MapPoint是否存在冲突，对冲突的Map Point进行替换或填补。然后通过将闭环时相连关键帧的Map Point投影到这些关键帧中，进行Map Point检查与替换（如有冲突以闭环帧及其相邻帧的Map Point为准，这是因为前期可以认为没有误差积累，后期这些冲突是由于在运行中产生的积累误差）。再次更新图关系，得到了因闭环Map Point融合之后得到的图关系。这里面需要注意一下，更新后的连接关系分为两个部分（一部分是本来就有的连接关系，另外一部分是由于闭环之后所产生的连接关系） 

OptimizeEssentialGraph()(EssentialGraph优化)：这里的优化对象由三个部分组成，扩展树连接关系、闭环产生的连接关系和一些共识关系非常好的边（这里设定的是共识权重超过100的），由着三部分组成的图进行优化。

GlobalBundleAdjustemnt()（全局优化）:最后建立一个全局优化，优化所有的关键帧和Map Point.

备注

std::vector > mvInvertedFile;　每个成员都是KeyFrame*的链表．成员长度为字典的单词长度，成员就是能检测到字典的那个对应word的关键帧．wordID对应这里vector的索引．