orbslam2学习笔记 -- loopclosing 流程梳理

读之前提醒：首先，该文章是按照时间顺序讲述，并且不提前讲各种类关系和数据结构关系，而是在讲述系统时间执行流成的时候及时的补充相关的东西。意思就是先知道这个东西在流程中是干嘛的，然后告诉你这个东西是啥。如果你还没有读过tracking的部分，请先读tracking，因为很多概念会在tracking里先讲出来，后面再遇到，就不会细讲了。

tracking : https://blog.csdn.net/michaelhan3/article/details/89742663

 

loopclosing是一个单独的线程，通过函数LoopClosing::Run()里的一个死循环执行。首先通过函数CheckNewKeyFrames()检测mlpLoopKeyFrameQueue里的关键帧是否为空，如果不空则进行闭环检测的处理。Loopclosing中的关键帧是LocalMapping发送过来的，LocalMapping是Tracking中发过来的，在LocalMapping中通过InsertKeyFrame将关键帧插入闭环检测队列mlpLoopKeyFrameQueue。其实在整个loopclosing线程里，只是做了以下几个函数的事情。就是三个函数，分别是检测闭环DetectLoop()，计算闭环帧的Sim3(Sim3就是相似变换)，然后根据计算的闭环帧的变换矫正位姿图。下面将一一说明。

    while(1) {
        if(CheckNewKeyFrames()) {
            if(DetectLoop()) {
               // Compute similarity transformation [sR|t]
               // In the stereo/RGBD case s=1
               if(ComputeSim3()) {
                   // Perform loop fusion and pose graph optimization
                   CorrectLoop();
               }
            }
        }
    }

检测闭环DetectLoop()

       进入函数，首先从队列里取出一个关键帧，然后如果当前关键帧距离上次闭环关键帧在10帧以内，则不进行闭环的检测。因为如果发生闭环，很可能在闭环帧附近的一些关键帧也都会形成闭环。然后取出当前关键帧，也就是刚才从队列取出来的关键帧，将该关键帧的所有共视关键帧取出来，然后计算当前关键帧与其所有共视关键帧的最低相似分数，就是bow向量距离分数。

        然后通过函数vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore)，在所有关键帧中找出闭环备选帧。这里需要简单说一下这个函数。进入该函数。首先一个 关键帧的闭环帧应该避开自己相连接的帧，因为相连接的帧肯定与当前关键帧有很多相似的场景点，而闭环想要找的就是曾经的某帧，但不能是当前帧有连接的帧，有连接意思就是有共视的地图点。现在我们来想，怎么才能找到当前帧可能存在闭环的候选帧呢？一个很简单的办法，就是用当前帧和地图里的所有帧都进行相似性检测，给出相似性比较高的前面的一些帧作为候选帧。实际上也是这样做的。只不过利用了一些技巧。还记得在将tracking部分的时候我们简单的提到过一个图片的Bow向量，这个向量的每一个元素都可以看成一个单词和一个响应值。而在KeyFrameDatabase里，我们记录了包含每个单词的所有关键帧，这个数据结构是std::vector<list<KeyFrame*> > mvInvertedFile; ///< 倒排索引，mvInvertedFile[i]表示包含了第i个word id的所有关键帧。那么我们就可以遍历当前关键帧包含的所有单词，然后通过每个关键帧的变量mnLoopWords(int类型)记录与当前关键帧的共同单词数量。删除与当前帧共同单词数量小于所有候选帧与当前帧共同单词数量最大值的0.8倍的那些关键帧。然后进入下一步筛选。然后将剩下的候选帧，找出每一帧共视最佳的前10帧，然后加上该候选帧组成一组，计算该组与当前帧的相似性分数之和，这里共视的10帧只有在当前的候选帧中时，才能累加它与当前关键帧的相似性分数，并且记录下本组与当前帧相似性匹配分数最高的那个帧，后面会用到。找到所有组中的累计分的最高分数，用这个最高分数的0.75倍作为阈值，将所有组中累计分数大于这个阈值的该组里与当前帧匹配最高的那个关键帧加入到最终的候选帧里。至此一个关键帧的所有候选闭环帧寻找完毕。

       下面来讲述怎么从剩下的闭环候选帧里进一步筛选。

如图所示，从k0 - k3为从队列里依次取出来的4个关键帧。同一列的绿点属于该关键帧的候选关键帧，与绿点相连接的蓝点为该绿点代表关键帧的共视关键帧，所以按照图片所示四个蓝点和一个绿点代表了一个子候选关键帧组，所以没一列的三个星状图标表示该列最下方关键帧的候选关键帧组。列与列之间的虚线表示连线的两个帧是同一个帧，这也表示了这两个子候选关键帧组有联系。红色椭圆是将会传递到下一次检测的连续组。

接下来开始讲下筛选过程。整体利用的思路就是，如果真的在某个地方发生了闭环，那么在该闭环帧的后面几帧里，每前后两帧的候选关键帧组都会有或多或少的联系。假设k0为取出的当前关键帧，该列上的三个绿点为上一步筛选过后剩下的候选闭环关键帧，然后找到每个关键帧的前几个共视关键帧（代码中取了10个，图里中只画了四个）。如果当前的变量mvConsistentGroups，也就是上一次的连续组为空，那么就把现在的包含四个关键帧组的连续组赋值给mvConsistentGroups，并且标记该组的三个成员连续的计数为0。进入下一次循环，这次取出关键帧k1作为当前关键帧，同样构建k1的候选关键帧的候选关键帧组，然后在上一次的候选关键帧连续组中找是否和当前的候选关键帧连续组有联系，这里的联系在代码里指的就是至少包含一个相同的关键帧，如果有，将当前某个组元素和之前的某个组元素相连接，并且连续计数加1。然后继续从队列里取下一个关键帧，按照上面的办法寻找连续，并更新计数。直到当前取出当前关键帧为k3，此时k3的红点所在的候选组的连续计数已经为3，那么该组所代表的候选关键帧，也就是红点所代表的候选关键帧将入选，成为真正的闭环关键帧。

       

接下来是计算当前帧与闭环帧的Sim3变换ComputeSim3（）。

        进入函数，ComputeSim3（），遍历上一步留下的闭环候选帧，依次与当前关键帧进行BOW方式的匹配(Bow方式的匹配在tracking里讲过了)，存储匹配的特征点信息。如果匹配数小于20个，则删除该闭环帧，否则为其构造Sim3求解器。对于剩下的每个候选关键帧，使用RANSAC求解候选关键帧到当前关键帧的sim3变换。RANSAC最多迭代5次，只有内点的数目大于一定的阈值才会返回变换矩阵，如果5次迭代还没有求出sim3，则删除该候选帧。对于RANSAC求出sim3的那个帧，保留求出来的内点的匹配关系，然后利用sim3投影匹配，补充当前的匹配。然后利用重投影误差优化sim3，不优化地图点坐标（这里的优化需要在这里先提前简单提一下，一般的优化位姿和地图的不同。这里的优化顶点只有一个，这个顶点就是候选关键帧，也就是候选的闭环帧到当前关键帧的sim3变换。误差边是两方面的，一方面是闭环帧的地图点变换到相机坐标系后，经过sim3变换，投影到当前关键帧下的坐标，和匹配点的坐标的差。另一方面是，当前关键帧的地图点变换到相机坐标系后，投影sim3逆变换，投影到闭环关键帧下的坐标和匹配点的坐标差。）。优化后，剔除误差大的边，返回内点的数量。如果内点的数量大于20个，则说明正确找到了闭环帧，直接跳过其余的候选帧的计算。然后以闭环帧的位姿为基准，用优化计算出来的闭环帧到当前帧的sim3变换，乘以闭环帧的位姿，把这个结果记下来，将来会作为用来更新当前帧的位姿（因为我们假设，闭环帧是发生在时间上的过去，而从过去到现在出现的当前帧，虽然相机又走到了相同的场景，但是位姿并没有很理想的重合，因为误差累计的原因。所以我们假设相信闭环帧的位姿，相信当前帧的位姿误差会多点，所以我们会固定闭环帧的位姿，而用闭环帧和当前帧的相对位姿变换乘以闭环帧的位姿作为当前帧的位姿）。这里插入两个变量的概念，一个是闭环相连接关键帧vpLoopConnectedKFs，这个vector向量有当前计算出来的闭环帧，和该闭环帧的所有共视关键帧组成。另一个概念是闭环关键点集mvpLoopMapPoints，这个vector向量内的点是所有闭环连接关键帧所能够观测到的所有地图点。然后将闭环匹配上关键帧以及相连关键帧的MapPoints，利用刚才求出来的基于固定闭环帧的位姿而得到的当前帧的位姿，投影到当前关键帧进行投影匹配根据投影查找更多的匹配，这里对于原来已经存在的匹配关系则不去改变，只是增加原来不存在的匹配。如果所有匹配点的数目超过了40个，则说明计算闭环帧到当前帧的sim3成功了。

 

接下来讲矫正闭环CorrectLoop()

       进入函数，首先更新当前帧与其他关键帧的连接关系，也就是根据当前帧的地图点，利用能够观测到这些地图点的关键帧，求出与当前帧共同看到的地图点的个数作为当前帧与另一个关键帧的连接权重。接下来需要根据利用闭环优化过的当前帧的位姿，矫正与当前帧连接的帧的位姿，这也就是所谓的位姿传播矫正。意思就是，利用当前帧原来未经过和闭环帧优化过的位姿，和与其相连接的关键帧的位姿，求出他们的相对位姿，然后利用相对位姿乘以当前帧优化后的位姿得到连接帧的矫正过的位姿，当然同时也会记录下来这些连接帧未经过矫正的原来的位姿。接下来开始根据矫正的位姿，来更新相应的地图点。具体的做法也还是很简单，首先利用获得地图点的世界坐标，利用原来矫正之前的位姿求出地图点在相机坐标系下的位姿，然后利用矫正后的位姿，求出矫正后相机坐标系到世界坐标系的变换，然后利用这个矫正后的变换乘以地图点的相机坐标系下的位姿得到地图点的矫正后的世界坐标系的位姿。并且这次，更新每个帧的矫正后的位姿。检查当前帧的地图点，与闭环匹配的地图点是否存在冲突，如果存在冲突就用匹配的替换，如果当前还没有匹配地图点，就添加上闭环匹配的地图点。

         SearchAndFuse(const KeyFrameAndPose& CorrectedPosesMap)该函数的作用是对于每个经过位姿传播矫正过的当前帧的相连关键帧，将闭环地图点按照矫正后的位姿投影到这些帧中，若该地图点已经可以被该帧观测到则跳过，否则利用区域块内的描述子匹配找到该地图点所匹配的最佳特征点，如果该特征点还没有被匹配，则添加该特征点和地图点的匹配关系，如果该特征点已经有匹配的地图点，则用闭环特征点替换掉原来的地图点作为新的匹配。这个新的匹配的地图点暂时放在MapPoint的mpReplaced变量里。而同时也需要更新所有能观测到该地图点（需要更新的原来地图点）为新的闭环地图点。

        接下来更新当前矫正过位姿和更新过地图点的关键帧之间的共视相连关系，得到因闭环时MapPoints融合而新得到的连接关系，然后在这些连接关系中除去原来的连接关系，只剩下由于闭环而新生成的连接关系。

       进行EssentialGraph优化，也就是pose graph优化，LoopConnections是形成闭环后新生成的连接关系，这些连接关系中大部分是当前帧连接组与闭环匹配帧连接组之间的连接关系，当然也可能会包括一些当前帧连接组内之间的新增加的联系。这个函数也就是所谓的根据闭环的关系，将累积的误差分配到各处。

Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF,NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);

本来这个函数需要在优化部分将，但是闭环的优化，这里又是关键，所以在这里需要比较详细的讲一下这个函数。

{

1. 设置g2o优化器

2. 将目前为止地图中的所有关键帧的位姿，添加为图优化的顶点。如果当前帧是闭环帧，则该帧的位姿固定，不优化。并且，如果该帧的位姿经过闭环传播调整过，那么使用经过调整后的位姿。

3. 添加由于闭环时地图点的更新而新出现的关键帧之间的联系作为图优化的边。当然这部分边主要是当前帧连接组和闭环帧连接组之间新建立的边。

4. 对所有关键帧添加跟踪时得到的边，也就是添加该关键帧和其父关键的联系作为边，并且使用未经过闭环位姿传播调整的原始位姿求相对位姿作为边的观测值；

对所有关键帧添加该关键帧的所有闭环帧连接得到的边，并且使用未经过位姿传播调整的原始位姿求相对位姿作为边的观测值；对所有关键帧中的每个关键帧，添加与该关键帧共视地图点个数大于100的关键帧组成的边，且使用未经过位姿传播调整的原始位姿求解相对位姿作为边的观测值。

5. 开始执行优化，迭代20次

6. 根据优化，更新所有关键帧的位姿。

7. 根据优化，更新所有地图点的三维位置。这里有个细节就是需要找到该地图点的参考关键帧，然后利用该参考关键帧的位姿，求解地图点的在相机坐标系下的坐标。然后利用优化后的位姿，再将相机位姿再映射到世界坐标系下。一般情况下，地图点的参考关键帧就是创建该地图点的关键帧。但是有一种情况比较特殊，还记得当我们找到了当前关键帧的闭环关键帧的时候，我们会固定闭环帧位姿，然后利用优化的相对位姿求解矫正的当前关键帧的位姿，然后利用当前关键帧的优化后的位姿传播矫正所有与其相连接的关键帧的位姿和地图点，这里被矫正更新的地图点的参考关键帧就是当前关键帧了，代码里使用了矫正后的参考关键帧变量mnCorrectedReference。但是如果该地图点经过闭环位姿传播矫正更新，那么该地图点的参考关键帧就是当前帧。

}

       接下来，创建一个新的线程，用于对地图里的所有关键帧和所有地图点做全局优化。

 

       好了，至此，闭环检测部分讲完了。