欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

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

程序员文章站 2024-03-25 23:47:04
...

找到线程的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

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

             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()

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

       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()

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

      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