SLAM_ORB-SLAM3回环检测是怎么做的？

青旅半醒 2023-01-20 09:35 304阅读 0赞

**目录**

基本步骤

主流程代码实现

参考

--------------------

# 基本步骤 #

在系统初始化是先建立好LoopClosing线程，定义关键帧队列，LocalMapping会在运行过程中将关键帧插入到该队列中，然后主流程如下所示：

1. 查看闭环检测队列mlpLoopKeyFrameQueue中有没有关键帧进来

2. **检测是否有回环**

*  Step 1：从队列中取出一个关键帧,作为当前检测闭环关键帧，设置当前关键帧不要在优化的过程中被删除；
 *  Step 2：如果距离上次闭环没多久（小于10帧），或者map中关键帧总共还没有10帧，则不进行闭环检测；
 *  Step 3：遍历当前回环关键帧所有**连接（>15个共视地图点）的相邻关键帧**，计算**当前关键帧**与每个共视关键的BOW相似度得分，并得到最低得分minScore(认为和当前关键帧具有回环关系的关键帧,不应该低于当前关键帧的**相邻关键帧**的最低的相似度minScore);
 *  ![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70][]
 *  Step 4：在所有关键帧中找出**闭环候选帧**（注意不和当前帧连接）；
 *  ![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70 1][]
 *  Step 5：在候选帧中检测**具有连续性的候选帧**
    
     *  a、每个候选帧将与自己相连的关键帧构成一个“子候选组spCandidateGroup”， vpCandidateKFs-->spCandidateGroup
     *  b、检测“子候选组”中每一个关键帧是否存在于“连续组”，如果存在 nCurrentConsistency++，则将该“子候选组”放入“当前连续组vCurrentConsistentGroups”
     *  c、如果nCurrentConsistency大于等于3，那么该”子候选组“代表的候选帧过关，进入mvpEnoughConsistentCandidates
     *  相关的概念说明:（为方便理解，见视频里的图示）
        
         *  组(group): 对于某个关键帧, 其和其具有共视关系的关键帧组成了一个"组";
         *  子候选组(CandidateGroup): 对于某个候选的回环关键帧, 其和其具有共视关系的关键帧组成的一个"组";
         *  连续(Consistent): 不同的组之间如果共同拥有一个及以上的关键帧,那么称这两个组之间具有连续关系
         *  连续性(Consistency):称之为连续长度可能更合适,表示累计的连续的链的长度:A--B 为1, A--B--C--D 为3等;具体反映在数据类型 ConsistentGroup.second上
         *  连续组(Consistent group): mvConsistentGroups存储了上次执行回环检测时, 新的被检测出来的具有连续性的多个组的集合.由于组之间的连续关系是个网状结构,因此可能存在一个组因为和不同的连续组链都具有连续关系,而被添加两次的情况(当然连续性度量是不相同的)
         *  连续组链:自造的称呼,类似于菊花链A--B--C--D这样形成了一条连续组链.对于这个例子中,由于可能E,F都和D有连续关系,因此连续组链会产生分叉;为了简化计算,连续组中将只会保存最后形成连续关系的连续组们(见下面的连续组的更新)
         *  子连续组: 上面的连续组中的一个组
         *  连续组的初始值: 在遍历某个候选帧的过程中,如果该子候选组没有能够和任何一个上次的子连续组产生连续关系,那么就将添加自己组为连续组,并且连续性为0(相当于新开了一个连续链)
         *  连续组的更新: 当前次回环检测过程中,所有被检测到和之前的连续组链有连续的关系的组,都将在对应的连续组链后面+1,这些子候选组(可能有重复,见上)都将会成为新的连续组;换而言之连续组mvConsistentGroups中只保存连续组链中末尾的组
     *  最终筛选后得到的闭环帧

3.计算Sim3

4.进行回环校正

# 主流程代码实现 #

参考：LoopClosing.cc

// 回环线程主函数
    void LoopClosing::Run()
    {
        mbFinished =false;
    
        // 线程主循环
        while(1)
        {
            // Check if there are keyframes in the queue
            // Loopclosing中的关键帧是LocalMapping发送过来的，LocalMapping是Tracking中发过来的
            // 在LocalMapping中通过 InsertKeyFrame 将关键帧插入闭环检测队列mlpLoopKeyFrameQueue
            // Step 1 查看闭环检测队列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();
    }

# 参考 #

*  计算机视觉life 第32讲-闭环线程中寻找初始闭环候选关键帧

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70]: /images/20221021/46f012b8c2014988b8a3d401a8e13500.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70 1]: /images/20221021/55fd4aef17a94aef8ee113b325edb732.png