ORBSLAM2系列-LoopClosing闭环检测线程

这是三大线程的最后一个线程，在每个关键帧从LoaclMapping线程处理后，就会经过闭环检测的线程进行回环处理。这里就是一直在进行闭环检测，或者说一直在寻找当前关键帧与之前关键帧是否有那些相似的地方，是否是回到的原来关键帧的地方形成回环。

这个线程主要由三个函数组成：DetectLoop初步检测以前关键帧是否与当前关键帧有联系形成闭环候选关键帧、ComputeSim3计算候选匹配关键帧和当前关键帧之间的相似变换sim3再次进行筛选、CorrectLoop对通过前两部筛选后的可回环关键帧进行全局处理

DetectLoop

这一步就如名字一样是在检测回环，主要就是在关键帧数据库中寻找与当前关键帧相似的关键帧作为闭环候选关键帧，当然这之中还有很多的筛选步骤。

在了解流程之前需要知道的概念：

• 由共视程度相连的关键帧（只是在局部地图中，或者说在前两个线程中）与闭环候选关键帧的关系：当前关键帧（闭环检测关键帧）相连的关键帧一定是与当前关键帧相似度比较高的，虽然要寻找的闭环候选关键帧也要满足比较高的相似度，但是共视关键帧是与当前关键帧比较近的帧，而闭环候选关键帧一定是很久以前的帧，这就可以形成两个寻找条件：闭环候选关键帧必须满足跟共视关键帧差不多的相似度，而且共视关键帧一定不是闭环候选关键帧。如下图所示：

• 闭环如果出现了误匹配，将会带来很严重的错误，因此不能仅仅依靠某一个关键帧检测到闭环就进行矫正，而是需要检测到连续的几个关键帧都检测到闭环才能称之为真正的出现了回环。因此，ORBSLAM2中对回环的检测有一步是连续性的检测：就是如果连续几个关键帧的闭环候选关键帧有一定的联系（他们的共视关键帧有相同的关键帧），就称为这几个关键帧都检测到闭环，才是真正的闭环，如下图（在程序中有对一些概念进行解读，这里不赘述）：

简单流程：

• 1.从mlpLoopKeyFrameQueue链表中取出需要检测的关键帧，这是从LoaclMapping线程处理后传递过来的，并且在回环检测的过程中，这个关键帧不能被删除
• 2.距上次闭环没多久，不进行闭环检测
• 3.使用BoW的score函数，计算当前关键帧与所有共视关键帧（大于15个地图点）的相似度得分，找到得分最低的得分（认为可闭环的关键帧与当前关键帧，不会低于这个最低得分）
• 4.使用DetectLoopCandidates函数，在所有的关键帧中寻找闭环候选关键帧，后面讲解
• 5.对闭环候选关键帧进行连续性检测：
• • 5-1.遍历每个闭环候选关键帧，并将其与其相连的关键帧（共视关键帧）构成一个组，称为子候选组spCandidateGroup
  • 5-2.遍历前一次闭环检测到的连续组（有前一次的闭环候选关键帧组成的子候选组和对应子候选组的连续长度构成），称为子连续组mvConsistentGroups
  • 5-3.在子连续组中寻找与当前遍历的子候选组中相同的关键帧，如果存在一个相同的关键帧，就认为这个子候选组对应的闭环候选关键帧是连续的（就是连续检测到闭环），将该子候选组的连续长度+1int nCurrentConsistency = nPreviousConsistency + 1;
  • 5-4.如果某一个子候选组连续三次认为是连续检测到闭环，那么就将该子候选组的闭环候选关键帧放入到mvpEnoughConsistentCandidates，认为该闭环候选关键帧可以作为闭环
  • 5-5.最后将本次的子候选组和对应连续长度更新到mvConsistentGroups
• 6.如果mvpEnoughConsistentCandidates中为空，则认为没有检测到闭环，退出

DetectLoopCandidates

该函数用于在关键帧数据库中，寻找与当前关键帧相似的关键帧做为闭环候选关键帧：

• 1.使用BowVector找到与当前关键帧具有相同Word单词的关键帧，并记录其公共单词数量。注意：与当前关键帧共视不能作为闭环候选关键帧，因此需要剔除（这是第一步筛选）
• 2.设定最小公共单词数量的阈值minCommonWords为最大公共单词数量的0.8倍
• 3.挑选公共单词数量大于minCommonWords，并且相似度大于最小阈值minScore（这是此函数之前，当前关键帧的共视关键帧的最低相似度得分）（这是第二部筛选）
• 4.仅仅只是某一帧满足相似是不够的，因此还需要添加共视关键帧的相似性：
• • 4-1.对经过两步筛选之后的关键帧提取10个共视关键帧，作为一组。计算每组的总相似度得分，得到最高的分数（注意：共视关键帧中只有也是经过两步筛选之后的关键帧才能贡献分数）
  • 4-2.设定最小的组相似度总分的阈值minScoreToRetain为最大组相似度总分的0.75倍
  • 4-3.没上面的每组得分中，对组相似度总分大于minScoreToRetain阈值的，将其组里相似度得分最高的关键帧作为闭环候选关键帧

ComputeSim3

这个函数就是在计算sim3（注意这里求得变换是闭环候选关键帧到当前关键帧之间的变换），然后优化，根据这个过程中主要是根据内点数量，判断这个闭环候选关键帧是否真的形成了闭环（闭环是否稳定）

在了解流程之前需要知道的概念：

• 在闭环的过程中，由于两个关键帧之间已经经过了很多时间，那么可能会出现两帧匹配特征点时都有其自己的地图点，而且地图点并不是同一个（ORBSLAM2虽然有重复地图点融合的情况，但是那是在LoaclMapping线程，融合的地图点都是局部地图点），因此会在SearchByBoW函数中看到，即使某个特征点有地图点，依然会进行匹配（但是匹配的依据仍然是描述子）
• sim3指的是相似变换，具体的变换公式放在了另一篇文章，那么在这里需要知道的是，相似变换是两个坐标系之间的变换，是某一点在相机1坐标系和相机2坐标系之间的变换，因此需要将地图点的3D坐标，变换到对应的两个相机坐标系中，然后才能进行sim3变换。

简单的流程如下：

• 1.遍历每个闭环候选关键帧，使用BoW加速匹配，得到与当前关键帧的匹配特征点，并且为每个闭环候选关键帧设置一个Sim3Solver，用于求解sim3变换
• • 1-1.使用SearchByBoW函数（与参考帧跟踪不同）进行匹配，整体流程跟之前一样，只不过对于有地图点的特征点依然会进行匹配
  • 1-2.在建立Sim3Solver时，会将匹配的地图点解算成对应相机坐标系的坐标，用于后续的sim3求解
• 2.遍历每个闭环候选关键帧，分别求解与当前关键帧的sim3变换，具体的变换变换流程在Sim3变换原理
• • 2-1.从使用BoW加速匹配的点中随机选择3个匹配点对，使用ComputeSim3函数计算sim3变换，就是尺度s、旋转R和位移t
  • 2-2.使用sim3变换得到的解，根据重投影误差进行内点检测，得到当前计算的sim3的内点数量
  • 2-3.上面两个过程经过多次迭代，选取内点数量最多的一次解作为最优解
• 3.使用SearchBySim3函数再次寻找匹配点，后面讲解
• 4.使用OptimizeSim3函数优化sim3优化：
• • 4-1.优化过程依然使用的是最小化重投影误差去做，整个优化过程仅优化sim3变换而不优化地图点
  • 4-2.本次优化的是sim3变换（尺度s、旋转R和位移t），而不像是之前的图像帧的位姿，因此g2o的优化顶点分别是sim3变换和地图点
• 5.只要有一个闭环候选关键帧通过了sim3的求解和优化，就停止闭环候选关键帧的遍历，否则就退出（这时就已经选中了一个闭环候选关键帧坐作为可闭环关键帧）
• • 5-1.注意：mScw变量是从世界坐标系到当前关键帧之间的sim3变换（这是根据求解的sim3变换计算而得）
• 6.为了得到更多的匹配点，再次进行SearchByProjection特征匹配，只不过这里匹配使用的是可闭环关键帧的局部地图点（其共视关键帧的所有地图点）
• • 6-1.SearchByProjection函数整体流程与参考帧跟踪时一样，使用的是投影匹配，在一开始是分解的sim变换矩阵，得到了当前关键帧的旋转和位移（相对于世界坐标系）
• 7.如果最后得到的匹配点超过40个，就认为这个关键帧可以作为可闭环关键帧（当前回环可靠），否则删除该关键帧

SearchBySim3

此函数是通过sim3变换，在两个关键帧之间寻找更多的匹配点对，为了能够得到正确的匹配，分别使用了正向和反向投影匹配，只有正反投影都匹配上了，才认为该匹配是正确的，其中涉及到的sim3的逆在Sim3变换原理说明

• 记录已经匹配的点对，这些点不会再次进行匹配过程（这部分主要是之前的BoW加速匹配得到的）
• 正向投影：从pKF1中的特征点中寻找pKF2特征点的匹配：
• • 根据sim3变换，将pKF1中特征点对应的地图点变换到pKF2图像上，计算其2D坐标
  • 经过一系列筛选后，使用GetFeaturesInArea函数进行网格搜索
  • 从网格搜索的候选匹配点中，选取描述子距离最小的，作为匹配点对
• 反向投影：从pKF2中的特征点中寻找pKF1特征点的匹配，流程如上一样
• 只有两个互相的匹配点对相同，才认为这两个特征点匹配

CorrectLoop

程序运行到这里时，已经确定当前关键帧可以与闭环关键帧（已经确定）形成闭环，因此这个函数就是用来将当前关键帧和闭环关键帧接起来，但是并不能直接生硬的将两帧连接（因为累计误差的存在这两帧可能相差较远），那么就需要将这两帧的共视帧都进行相应的移动，然后进行全局BA，将误差平均均摊到各个位姿中

在了解流程之前需要知道的概念：

• 位姿传播矫正：就是使用计算的sim3变换，分别对位姿和地图点进行相应的矫正（是对当前关键帧的局部地图进行矫正：一级共视帧组及其地图点）
• • 对于位姿矫正：就是要根据之前计算得到mg2oScw（当前关键帧的sim3变换：世界坐标系到当前相机坐标系）一步步传播到其他的共视帧中。如下图中计算，其中g2oSic就是由Tic得到，就能得到每个共视关键帧的g2oCorrectedSiw（就是通过位姿传播得到，该共视关键帧的世界坐标系到该相机坐标系的sim3变换）；将这个sim3变换转成位姿SE3：对于g2oCorrectedSiw中的R保留，而对于t需要进行尺度归一化，即 $t=t/s$

• • 对于地图点矫正：对局部地图点（一级共视关键帧的全部地图点）来说，就是做了一次正反变换：将3D坐标（某共视关键帧的地图点）根据位姿传播之前的R,t，由世界坐标转成该相机坐标；再根据位姿传播之后的R,t，由该相机坐标转成世界坐标。

• LoopConnections变量的解读：这个变量一开始看的时候有点懵，因此在这里用图的方式表示一下：如下图所示，LoopConnections变量最终要得到的就是红色的连接线，也就是由于闭环产生的新的连接关系

简单的流程如下：

• 1.将LocalMapping线程停止（防止添加了新的关键帧），并将正在运行的全局BA停止（如果有的话），接下来的过程一直等到LocalMapping线程停止才会进行
• 2.更新当前关键帧的连接关系（因为之前闭环检测时改变了该关键帧的地图点，因此需要更新）
• 3.建立当前关键帧组（一级共视帧和当前关键帧组成），对这个“局部地图”进行sim3变换矫正：
• • 3-1.对于每个一级共视帧进行位姿矫正，保存矫正前和矫正后的位姿（用于后面地图点矫正和优化时使用），并更新该共视帧的连接关系
  • 3-2.对于每个一级共视帧的地图点进行坐标矫正，并根据矫正后的地图点更新平均观测方向和距离 g2oCorrectedSwi.map(g2oSiw.map(eigP3Dw))
  • 3-3.对于在计算sim3变换时的匹配点，需要对其进行替换（融合）（因为匹配的两个特征点可能都有其各自的地图点，那么就需要将其融合，融合选择的是用闭环关键帧的地图点替代，因为闭环关键帧的地图点是比较靠前的，就是比较精确，而当前关键帧的地图点会比闭环的多了一些误差）
• 4.经过矫正之后，就会得到当前关键帧及其共视关键帧的新的“位姿”（经过位姿传播得到），那么就将闭环关键帧的局部地图点（闭环关键帧及其共视关键帧的地图点）投影到当前关键帧组，得到更多的匹配地图点，进行替换或者新增地图点
• • 4-1.到此，整个矫正的过程基本完成，位姿和地图点以及地图点在当前关键帧组和闭环关键帧组的对应关系，都完成了相应的矫正，接下来就是进行本质图和全局的优化
• 5.本质图优化：在此就是应用了父子关键帧，在使用g2o优化过程中，本质图的顶点是所有关键帧，而边是联系紧密的关键帧。（注意，整个优化第0帧也进行了优化，而闭环关键帧不进行优化，就是以这个为基准）
• • 5-1.第1种边：因为闭环调整后而出现的新的链接关系（边），就是LoopConnections变量中的连接关系
  • 5-2.第2种边：每个关键帧与父关键帧相连的边（注意，父关键帧只有一个，子关键帧可以有很多）
  • 5-3.第3种边：形成闭环的两个关键帧之间的边（当前关键帧和闭环关键帧），其中不仅仅是当前闭环的边，还包括以前闭环的两个关键帧
  • 5-4.第4种边：将共视程度超过100的关键帧作为边
• 6.新建一个线程，进行全局BA：
• • 6-1.全局BA与初始化时的全局优化一样（使用同一个函数），注意：这里不优化第0帧（第0帧在本质图优化的时候已经优化过了）
  • 6-2.全局优化之后，会进行地图更新，对全局所有的位姿和地图点重新设置