ORB-SLAM2中LoopClosing线程主要流程梳理

LoopClosing线程主要工作是两个：一个是对当前新到来的关键帧进行闭环检测，看看之前的关键帧中哪个和当前关键帧形成闭环；另一个是纠正闭环，就是在检测到闭环的时候要将当前关键帧和闭环关键帧的位姿拉到一起，形象点说就是进行融合（既然都存在闭环了，那肯定得合并啊）。

一、闭环检测DetectLoop

1.从关键帧列表中获取列表头的关键帧，也就是获取最新插入到LoopClosing线程的关键帧，并判断当前关键帧是否距离上次关键帧闭环检测的帧之间大于10帧，大于10帧才进行后边的闭环检测，否则将新的关键帧插入到关键帧数据库中。

2.获取当前关键帧和与其共视的所有关键帧词袋向量比较的最小分值minScore；

3.在关键帧数据库中查找和当前关键帧存在闭环的候选关键帧(DetectLoopCandidates)。步骤如下：
     1）获取当前关键帧在共视图中和他连接的关键帧，并遍历查找这些关键帧当中的词向量都在哪些关键帧中出现过，查到的共享词向量的关键帧存放在lKFsSharingWords当中；
     2）遍历lKFsSharingWords，统计候选帧中与pKF具有共同单词最多的单词数maxCommonWords；
     3）再次遍历lKFsSharingWords，得到和当前关键帧共享单词数大于0.8×maxCommonWords，并且和当前关键帧的BoW得分大于minScore的关键帧，存入lScoreAndMatch当中；
     4）遍历lScoreAndMatch当中的关键帧，对其中每个关键帧计算和其共视关系最好的10个关键帧中的最高得分的关键帧和他们的累计分值bestAccScore，存入lAccScoreAndMatch当中；
     5）遍历lAccScoreAndMatch，对其中每个关键帧判断其得分大于0.75×bestAccScore的话，将该关键帧存入闭环候选帧spAlreadyAddedKF当中返回；

4.判断连续性：一个关键帧和其所有的相连关键帧一起构成了一个关键帧组，判断连续性的根据就是判断当前关键帧组当中的关键帧是否在上一次的关键帧组中出现过足够的帧数。
     1）遍历闭环候选帧，将每一个候选帧和其所有相连的关键帧放在一个set集合当中，构成了当前的候选连续性关键帧组；
     2）遍历上一次的连续性关键帧组，判断当前的候选连续性关键帧组中的每一个关键帧是否在上一次的连续性关键帧组中出现过。
        如果出现过，则将该关键帧的连续性个数在上一次的基础上加1存储在nCurrentConsistency中，当nCurrentConsistency大于阈值3的时候并且还没有足够的连续性则认为当前候选关键帧具有足够的连续性，将其加入到mvpEnoughConsistentCandidates当中。
     3）mvpEnoughConsistentCandidates中有加入的关键帧，则认为具有连续性。

     此时mvpEnoughConsistentCandidates就是检测所得的闭环候选关键帧。

5.用当前的连续性共视关键帧组更新上一次的连续性共视关键帧组（mvConsistentGroups = vCurrentConsistentGroups;）。

6.将当前关键帧加入到关键帧数据库当中。

二、计算相似变换ComputeSim3

sim3的引入：对于单目相机来说，具有尺度不确定性。如果在单目SLAM当中使用欧式变换SE3表示位姿，由于尺度不确定性和累积漂移的存在，整个运动过程中的尺度会发生变化，而SE3体现不出尺度的变化，所以此时就需要借用相似变换将尺度因子表达出来。对于空间中的一个点p，要投影到相机坐标系当中，相似变换表达如下：

                                                                  

相似变换的尺度s作用在p的3个坐标上，相当于对p进行了一次缩放。相似变换对矩阵乘法构成群，称为相似变换群sim3。

以上引入描述来自高博视觉slam十四讲当中。另外sim3计算也可以参考笔者之前的一篇博文：ORB-SLAM2代码阅读笔记（十）：sim3求解

ComputeSim3流程如下：

1.遍历候选闭环关键帧中每个关键帧，对每个候选关键帧和当前关键帧之间求解sim3。
      1）这里先对每个候选关键帧和当前关键帧进行BOW搜索匹配，只有当匹配的特征点数>=20的时候才为该候选帧构建sim3求解器，否则剔除掉该候选帧；
      2）对1）中为每个匹配成功的候选帧进行sim3迭代优化求解，根据求解出的s,R,t再次对当前关键帧和候选关键帧进行匹配(函数为SearchBySim3)；
      3）根据sim3求解结果构造当前关键帧到候选关键帧的sim3变换（这里的尺度s是sim3求解出来的）gScm，并遍历2）中的匹配点进行SIM3优化；
         如果优化后的inlier数>=20，则该候选帧就是匹配关键帧，并计算当前帧位姿经过sim3修正后的位姿（这里构造闭环帧的sim3位姿gSmw时，尺度s为1）mg2oScw = gScm*gSmw，至此也就计算出了当前关键帧的位姿；
         如果优化后的inlier数<20，则继续遍历下一个候选关键帧进行sim3变换求解；

2.获取当前关键帧的匹配闭环关键帧及其相连关键帧对应的地图点。将这些地图点通过上面优化得到的Sim3(位姿mScw) 变换投影到当前关键帧进行匹配SearchByProjection：
     如果匹配点数>=40，则判定当前关键帧和闭环候选关键帧存在闭环关系；
     如果匹配点数<40，则判定当前关键这暖和闭环候选帧之间没有闭环关系；

---

注意，在LoopClosing::ComputeSim3()函数中一共进行了下面三次特征点匹配：
 1.第一次匹配
int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);
这里主要是通过SearchByBow搜索当前关键帧mpCurrentKF中和闭环候选帧pKF匹配的地图点，BoW通过将单词聚类到树结构node的方法，这样可以加快搜索匹配速度，vvpMapPointMatches[i]用于存储当前关键帧和候选关键帧之间匹配的地图点。
 
2.第二次匹配
  matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5);
  使用sim3求出来的s,R,t通过SearchBySim3得到更多匹配。
 
3.第三次匹配
matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);
这里得到的当前关键帧中匹配上闭环关键帧共视地图点(mvpCurrentMatchedPoints)将用于后面CorrectLoop时当时关键帧地图点的冲突融合，
到这里不仅确保了当前关键帧与闭环帧之间匹配度高， 而且保证了闭环帧的共视图中的地图点和当前帧的特征点匹配度更高,说明该闭环帧是正确的。

---

三、闭环校正CorrectLoop

1.暂停LocalMapping线程，防止其在闭环校正阶段向LoopClosing线程中插入新的关键帧；
2.停止全局BA优化线程运行；
3.更新当前关键帧与其共视的其他关键帧之间的连接关系UpdateConnections;
4.得到当前关键帧和与其连接的共视关键帧组，并通过当前关键帧的sim3位姿计算出与其相连接的共视关键帧的闭环优化后的sim3位姿；
5.利用4中所得的调整过的和当前关键帧相连的所有关键帧的sim3位姿来更新每个关键帧相对应的地图点的3D坐标和深度等信息；
6.根据关键帧的SIM3位姿构造出欧式变换SE3，也就是关键帧的位姿，并更新关键帧的位姿和连接关系；
7.更新当前关键帧当中的地图点，或者将经过sim3位姿纠正过的地图点加入到当前关键帧当中。
   前置理解事项：
        第一次匹配：通过BoW比较进行匹配；
        第二次匹配：在第一次匹配的基础上，根据匹配点进行sim3优化求解后，再根据sim3所求的旋转R、平移t和尺度s再次进行当前关键帧和闭环候选帧之间的匹配SearchBySim3，并再次进行优化；
        第三次匹配：使用第二次匹配完优化后的当前关键帧和闭环候选帧之间的SIM3变换再次进行匹配matcher.SearchByProjection；
   通过以上三次匹配后，所得的匹配点是质量最高的，毕竟每次匹配和优化后都会剔除一些outlier。在闭环校正当中使用这些匹配点进行地图点融合，也就是用这些地图点替换掉当前关键帧。
       
   用投影匹配上的经过SIM3计算纠正过位姿的地图点更新当前关键帧（也就是存在闭环的关键帧）中对应的地图点；
   如果当前关键帧中没有与之对应的地图点，则将这些经过SIM3计算纠正过位姿的地图点加入当前关键帧当中，并更新该地图点的观测帧和描述子；

8.地图点进行融合SearchAndFuse：
  这里是使用闭环匹配关键帧和其共视关键帧当中的地图点来更新当前关键帧的共视关键帧中相匹配的地图点。
  当前关键帧的共视关键帧中没有的地图点，则直接进行添加；有的则直接用闭环匹配关键帧和其共视关键帧当中对应的地图点进行更新。
  这样就完成了地图点的融合。

9.优化EseentialGraph：Optimizer::OptimizeEssentialGraph
  EseentialGraph其实就是当前地图中所有的关键帧、所有的地图点组成的图，那么这里其实就是一个大的图优化。
  只不过在这里边优化的时候要区分当前关键帧、和当前关键帧形成的闭环关键帧、当前关键帧的共视关键帧、和闭环关键帧有共视关系的关键帧。
  如果关键帧的位姿在sim3求解的时候已经求出来了，则使用sim3求解所得的位姿。图的顶点都是关键帧，边分为正常边(normal edge)和闭环边(loop edge),
  用优化所得的关键帧的sim3变换恢复出SE3，并对关键帧中相应的地图点更新其3D坐标和深度信息。
 

10.将当前关键帧添加到匹配的闭环关键帧的闭环边中。
   将与当前关键帧形成闭环的关键帧添加到当前关键帧的闭环边中；

11.启动一个线程进行全局优化RunGlobalBundleAdjustment。