ORB-SLAM2代码整理--LoopClosing线程

找到线程的Run函数

void LoopClosing::Run()
{
    mbFinished =false;

    while(1)
    {
        // Check if there are keyframes in the queue
        // Loopclosing中的关键帧是LocalMapping发送过来的，LocalMapping是Tracking中发过来的
        // 在LocalMapping中通过InsertKeyFrame将关键帧插入闭环检测队列mlpLoopKeyFrameQueue
        // 闭环检测队列mlpLoopKeyFrameQueue中的关键帧不为空
        if(CheckNewKeyFrames())
        {
            // Detect loop candidates and check covisibility consistency
            if(DetectLoop())
            {
               // Compute similarity transformation [sR|t]
               // In the stereo/RGBD case s=1
               if(ComputeSim3())
               {
                   // Perform loop fusion and pose graph optimization
                   CorrectLoop();
               }
            }
        }

        ResetIfRequested();

        if(CheckFinish())
            break;

        //usleep(5000);
		std::this_thread::sleep_for(std::chrono::milliseconds(5));

	}

    SetFinish();
}

只要闭环检测关键帧队列不为空下面的检测会一直执行 

1 DetectLoop()  进行闭环检测

     1.1 从队列中取出一个关键帧

     1.2 判断:如果距离上次闭环没多久（小于10帧），或者map中关键帧总共还没有10帧，则不进行闭环检测

     1.3 遍历所有共视关键帧，计算当前关键帧与每个共视关键的bow相似度得分，并得到最低得分minScore

        

             1.3.1 找出所有与当前关键帧相连的KeyFrame，这些相连Keyframe都是局部相连，在闭环检测的时候将被剔除

             1.3.2 步骤1:找出和当前帧具有公共单词的所有关键帧（不包括与当前帧链接的关键帧）

             1.3.3 遍历当前关键帧的每一个word

             1.3.4 提取包含该word的关键帧pKFi,但不包括与当前关键帧相邻的关键帧

             1.3.5 步骤2:统计所有闭环候选帧中与pKF具有共同单词最多的单词数

         1.3.6 步骤3：遍历所有闭环候选帧，挑选出共有单词数大于minCommonWords且单词匹配度大于minScore存入lScoreAndMatch

              1.3.7 步骤4: 单单计算当前帧和某一关键帧的相似性是不够的，这里将与对于上文筛选出来的pKFi相连（权值最高，共视程度最高）的前十个关键帧归为一组，计算累计得分

         即:lScoreAndMatch中每一个KeyFrame都把与自己共视程度较高的帧归为一组，每一组会计算组得分并记录该组分数最高的KeyFrame，记录于lAccScoreAndMat

              1.3.8 步骤5：得到组得分大于minScoreToRetain的组，得到组中分数最高的关键帧

     1.4 在候选帧中检测具有连续性的候选帧

   // 1、每个候选帧将与自己相连的关键帧构成一个“子候选组spCandidateGroup”，vpCandidateKFs-->spCandidateGroup
   // 2、检测“子候选组”中每一个关键帧是否存在于“连续组”，如果存在nCurrentConsistency++，则将该“子候选组”放入“当前 连续组vCurrentConsistentGroups”
   // 3、如果nCurrentConsistency大于等于3，那么该”子候选组“代表的候选帧过关，进mvpEnoughConsistentCandidates

     1.5 将上一步中得到的候选闭环帧以及与自己相连的关键帧构成一个"子候选组"

     1.6 遍历之前的"子连续组".....(子连续组是Covisibility图的连续性地图)

     1.7 遍历每个“子候选组”，检测候选组中每一个关键帧在“子连续组”中是否存在

         // 如果有一帧共同存在于“子候选组”与之前的“子连续组”，那么“子候选组”与该“子连续组”连续

     1.8 将该“子候选组”的该关键帧打上编号加入到“当前连续组”

     1.9  如果该“子候选组”的所有关键帧都不存在于“子连续组”，那么vCurrentConsistentGroups将为空，

        // 于是就把“子候选组”全部拷贝到vCurrentConsistentGroups，并最终用于更新mvConsistentGroups，计数器设为0，重新开始

      1.10 更新Covisibility连续地图

2  ComputeSim3()

       2.1 步骤1: 从筛选的闭环候选帧中取出一帧关键帧pKF

       2.2 步骤2：将当前帧mpCurrentKF与闭环候选关键帧pKF匹配

                        SearchByBoW (见Tracking线程的4.3)

       2.3 构造Sim3求解器

       2.4 一直循环所有的候选帧，每个候选帧迭代5次，如果5次迭代后得不到结果，就换下一个候选帧

   // 直到有一个候选帧首次迭代成功bMatch为true，或者某个候选帧总的迭代次数超过限制，直接将它剔除

      RANSAC：利用上面匹配上的地图点（虽然匹配上了，但是空间位置相差了一个Sim3），用RANSAC方法求解Sim3位姿
        //这里有可能求解不出Sim3,也就是虽然匹配满足，但是空间几何姿态不满足vvpMapPointMatche         

       2.5 步骤3：对步骤2中有较好的匹配的关键帧求取Sim3变换

       2.6 最多迭代5次，返回的Scm是候选帧pKF到当前帧mpCurrentKF的Sim3变换（T12）

       2.7   经过n次循环，每次迭代5次，总共迭代 n*5 次

      // 总迭代次数达到最大限制还没有求出合格的Sim3变换，该候选帧剔除

      2.8 如果RANSAC成功,则保存优化后的mappoint    vpMapPointMatches

      2.9  通过步骤3求取的Sim3变换根据计算出的Sim3(s, R, t)，去找更多的匹配点(SearchBySim3),更新vpMapPointMatches,弥补步骤2中的漏匹配

           SearchBySim3

// 通过Sim3变换，确定pKF1的特征点在pKF2中的大致区域，同理，确定pKF2的特征点在pKF1中的大致区域
// 在该区域内通过描述子进行匹配捕获pKF1和pKF2之前漏匹配的特征点，更新vpMatches12（之前使用SearchByBoW进行特征点匹配时会有漏匹配）

              2.9.1 获得当前关键帧的内参 R t 和 候选闭环帧的R t 和尺度s

              2.9.2 vbAlreadyMatched1 用于记录该特征点是否被处理过    

                       vbAlreadyMatched2  用于记录该特征点是否在pKF1中有匹配

              2.9.3 用vpMatches12更新vbAlreadyMatched1和vbAlreadyMatched2

                       // vpMatch12是在SearchByBoW中的pKF2中与pKF1匹配的MapPoint

             2.9.4 步骤3.1：通过Sim变换，确定pKF1的特征点在pKF2中的大致区域，在该区域内通过描述子进行匹配捕获pKF1和pKF2之前漏匹配的特征点，更新vpMatches12 （之前使用SearchByBoW进行特征点匹配时会有漏匹配）

            2.9.5 把pKF1系下的MapPoint从world坐标系变换到camera1坐标系

                      再通过Sim3将该MapPoint从camera1变换到camera2坐标系

                       得到像素坐标

           2.9.6 预测该MapPoint对应的特征点在图像金字塔哪一层

                    根据金字塔层数确定特征点的搜索半径

                    取出该区域内的所有特征点

           2.9.7 遍历搜索区域内的所有特征点，与pMP进行描述子匹配 成功的放入 vnMatch1

           2.9.8 步骤3.2：通过Sim变换，确定pKF2的特征点在pKF1中的大致区域，

         //    在该区域内通过描述子进行匹配捕获pKF1和pKF2之前漏匹配的特征点，更新vpMatches12   
         //   （之前使用SearchByBoW进行特征点匹配时会有漏匹配）

                        与步骤3.1 相反

             2.9.9 返回找到多少匹配点

      2.10 步骤5：Sim3优化，只要有一个候选帧通过Sim3的求解与优化，就跳出停止对其它候选帧的判断

                           优化mpCurrentKF与pKF对应的MapPoints间的Sim3，得到优化后的量gScm

      2.11 优化成功后停止RANSAC

                mpMatchedKF就是最终闭环检测出来与当前帧形成闭环的关键帧

               得到从世界坐标系到该候选帧的Sim3变换，Scale=1

                得到g2o优化后从世界坐标系到当前帧的Sim3变换

      2.12 没有一个闭环匹配候选帧通过Sim3的求解与优化,清空mvpEnoughConsistentCandidates

      2.13 步骤6：取出闭环匹配上关键帧的相连关键帧，得到它们的MapPoints放入mvpLoopMapPoints

          注意是匹配上的那个关键帧：mpMatchedKF
          // 将mpMatchedKF相连的关键帧全部取出来放入vpLoopConnectedKFs
          // 将vpLoopConnectedKFs的MapPoints取出来放入mvpLoopMapPoints

      2.14 步骤7：将闭环匹配上关键帧以及相连关键帧的MapPoints投影到当前关键帧进行投影匹配

   // 根据投影查找更多的匹配（成功的闭环匹配需要满足足够多的匹配特征点数）
   // 根据Sim3变换，将每个mvpLoopMapPoints投影到mpCurrentKF上，并根据尺度确定一个搜索区域，
   // 根据该MapPoint的描述子与该区域内的特征点进行匹配，如果匹配误差小于TH_LOW即匹配成功，更新  mvpCurrentMatchedPoints
   // mvpCurrentMatchedPoints将用于SearchAndFuse中检测当前帧MapPoints与匹配的MapPoints是否存在冲突

                  2.14.1 获得当前关键帧的内参

                  2.14.2 计算得到尺度s

                  2.14.3 使用set类型，并去除没有匹配的点，用于快速检索某个MapPoint是否有匹配

                  2.14.4 遍历所有的MapPoints

                  2.14.5 判断距离在范围内,判断视角是否在范围内

                  2.14.6 根据尺度确定搜索半径

                  2.14.7 遍历搜索区域内所有特征点，与该MapPoint的描述子进行匹配

                  2.14.8 该MapPoint与bestIdx对应的特征点匹配成功

       2.15 步骤8：判断当前帧与检测出的所有闭环关键帧是否有足够多的MapPoints匹配

       2.16 清空mvpEnoughConsistentCandidates

3 CorrectLoop()

      3.1 局部地图线程停止

      3.2 根据共视关系更新当前帧与其它关键帧之间的连接

      3.3  步骤2：通过位姿传播，得到Sim3优化后，与当前帧相连的关键帧的位姿，以及它们的MapPoints

    // 当前帧与世界坐标系之间的Sim变换在ComputeSim3函数中已经确定并优化，
    // 通过相对位姿关系，可以确定这些相连的关键帧与世界坐标系之间的Sim3变换

      3.4 取出与当前帧相连的关键帧，包括当前关键帧

      3.5 将当前帧的sim3变换存入

      3.6 通过位姿传播，得到Sim3调整后其它与当前帧相连关键帧的位姿

   //当前帧的位姿固定不动，其它的关键帧根据相对关系得到Sim3调整的位姿 

   // 得到闭环g2o优化后各个关键帧的位姿

        //得到当前帧相连关键帧，没有进行闭环g2o优化的位姿

      3.7  步骤2.2：步骤2.1得到调整相连帧位姿后，修正这些关键帧的MapPoints

      3.8 将Sim3转换为SE3，根据更新的Sim3，更新关键帧的位姿

      3.9 根据共视关系更新当前帧与其它关键帧之间的连接

      3.10 检查当前帧的MapPoints与闭环匹配帧的MapPoints是否存在冲突，对冲突的MapPoints进行替换或填补

      3.11 通过将闭环时相连关键帧的mvpLoopMapPoints投影到这些关键帧中，进行MapPoints检查与替换

      3.12 更新当前关键帧之间的共视相连关系，得到因闭环时MapPoints融合而新得到的连接关系

      3.13 遍历当前帧相连关键帧（一级相连）

      3.14 得到与当前帧相连关键帧的相连关键帧

      3.15 更新一级相连关键帧的连接关系 UpdateConnections()

      3.16 取出该帧更新后的连接关系

      3.17 从连接关系中去除闭环之前的二级连接关系和一级链接关系，剩下的连接就是由闭环得到的连接关系(闭环链接关系哪里添加进去的.待查明)

      3.18 进行EssentialGraph优化，LoopConnections是形成闭环后新生成的连接关系，不包括下面的步骤中当前帧与闭环匹配帧之间的连接关系

     3.19 添加当前帧与闭环匹配帧之间的边（这个连接关系不优化）

     3.20 新建一个线程用于全局BA优化

           //OptimizeEssentialGraph只是优化了一些主要关键帧的位姿，这里进行全局BA可以全局优化所有位姿和MapPoints