(01)ORB-SLAM2源码无死角解析-(54) 闭环线程→闭环检测:寻找闭环候选关键帧 LoopClosing::DetectLoop()

本人讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析-接如下:
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196

${\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}$

一、前言

通过前面的博客，了解了闭环线程中 LoopClosing::Run 的总体结果，这篇博客主要就是对其调用的函数 bool LoopClosing::DetectLoop() 进行讲解，其同样位于 src/LoopClosing.cc 文件中。主要功能为为检测闭环。在进行代码讲解之前，先来讨论下其是怎么实现的，如下图所示：

图一

在这里插入图片描述

假设机器人(相机)溜达之后回到了起始点附近，形成了一个如上图的环。这个时候呢，机器人就会思考，我是不是回到起始位置了呢？那么当前关键帧就会和其他的关键帧进行比较，看下是否比较相似，如果比较相似，说明有可能回到起始位置了。

这个时候出现了一个问题:当前关键帧帧(上图红色点)应该和那些关键帧进行比较呢？首先要排除与当前关键帧连接的关键帧(上图黄色点)，因为相互连接的关键帧，肯定具备共视关系，因为只有共视点达到一定数目才会建立连接关系。为什么要排除它呢？因为机器人在运行过程中，当前关键帧肯定有一些共视关键帧与其连接，他们之间的相似度是比较高的，如果不把他们排除，那么回导致时时刻刻都会进行闭环。

所以，我们需要与当前关键帧比较的关键帧，是与当前关键帧不具备连接关系的关键帧，也就是上图绿色点，一般来说，这些关键帧都是在起始位置附近创建的。

$\color{red}注意$ 大家可能会有疑问，那就是上述绿色的点和黄色的点就没有连接关系了吗？回到之前讲解的 LocalMapping::SearchInNeighbors() 函数，该函数中调用了 mpCurrentKeyFrame->UpdateConnections(); 其会更新关键帧的连接关系，但是这里其更新的是关键帧的一级邻接和二级邻接关键帧。所以一般来说，上图红色点和绿色点是不具备连接关系的。

二、初始候选关键帧 vpCandidateKFs

LoopClosing::DetectLoop() 函数主要的目的是挑选出闭环候选关键帧，那么具体来说，什么是闭环候选关键帧呢？首先我们来看下图：
在这里插入图片描述
$\color{blue}(1)：$ 从队列中取出一个关键帧，作为当前检测闭环关键帧，也对应于源码中的 mpCurrentKF 与上图的当前帧。

$\color{blue}(2)：$ 如果距离上次闭环没多久（小于10帧），或者map中关键帧总共还没有10帧，则不进行闭环检测

$\color{blue}(3)：$ 获得所有与当前关键帧连接的关键帧(15个以上共视地图点)，计算当前关键帧与每个共视关键的BoW相似度得分，并得到最低得分minScore。这里是一个参考值，后续的挑选出来的闭环候选关键帧与当前关键帧的相似度需要大于该值。简单的说→认为和当前关键帧具有回环关系的关键帧，不应该低于当前关键帧的相邻关键帧的最低的相似度minScore

$\color{blue}(4)：$ 在所有关键帧中找出闭环候选帧(注意不和当前帧连接)，候选关键帧与当前关键帧具有较多的公共单词，相似度都高于minScore。挑选出来的初始闭环候选帧存储于 vpCandidateKFs 之中，如果 vpCandidateKFs 为空则返回 false，表示没有检测到闭环。

$\color{red}注意$ 这里挑选出来的 vpCandidateKFs 是初始闭环候选关键帧，后续还要经过一系列复杂的操作才能成为真正的候选候选关键帧。其代码位于 src/LoopClosing.cc 文件 LoopClosing::DetectLoop() 中，代码如下：

bool LoopClosing::DetectLoop()
{
    
    
    {
    
    
        // Step 1 从队列中取出一个关键帧,作为当前检测闭环关键帧
        unique_lock<mutex> lock(mMutexLoopQueue);
        // 从队列头开始取，也就是先取早进来的关键帧
        mpCurrentKF = mlpLoopKeyFrameQueue.front();
        // 取出关键帧后从队列里弹出该关键帧
        mlpLoopKeyFrameQueue.pop_front();
        // Avoid that a keyframe can be erased while it is being process by this thread
        // 设置当前关键帧不要在优化的过程中被删除
        mpCurrentKF->SetNotErase();
    }

    //If the map contains less than 10 KF or less than 10 KF have passed from last loop detection
    // Step 2：如果距离上次闭环没多久（小于10帧），或者map中关键帧总共还没有10帧，则不进行闭环检测
    // 后者的体现是当mLastLoopKFid为0的时候
    if(mpCurrentKF->mnId<mLastLoopKFid+10)
    {
    
    
        mpKeyFrameDB->add(mpCurrentKF);
        mpCurrentKF->SetErase();
        return false;
    }

    // Compute reference BoW similarity score
    // This is the lowest score to a connected keyframe in the covisibility graph
    // We will impose loop candidates to have a higher similarity than this
    // Step 3：遍历当前回环关键帧所有连接（>15个共视地图点）关键帧，计算当前关键帧与每个共视关键的bow相似度得分，并得到最低得分minScore
    const vector<KeyFrame*> vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames();
    const DBoW2::BowVector &CurrentBowVec = mpCurrentKF->mBowVec;
    float minScore = 1;
    for(size_t i=0; i<vpConnectedKeyFrames.size(); i++)
    {
    
    
        KeyFrame* pKF = vpConnectedKeyFrames[i];
        if(pKF->isBad())
            continue;
        const DBoW2::BowVector &BowVec = pKF->mBowVec;
        // 计算两个关键帧的相似度得分；得分越低,相似度越低
        float score = mpORBVocabulary->score(CurrentBowVec, BowVec);
        // 更新最低得分
        if(score<minScore)
            minScore = score;
    }

    // Query the database imposing the minimum score
    // Step 4：在所有关键帧中找出闭环候选帧（注意不和当前帧连接）
    // minScore的作用：认为和当前关键帧具有回环关系的关键帧,不应该低于当前关键帧的相邻关键帧的最低的相似度minScore
    // 得到的这些关键帧,和当前关键帧具有较多的公共单词,并且相似度评分都挺高
    vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);

    // If there are no loop candidates, just add new keyframe and return false
    // 如果没有闭环候选帧，返回false
    if(vpCandidateKFs.empty())
    {
    
    
        mpKeyFrameDB->add(mpCurrentKF);
        mvConsistentGroups.clear();
        mpCurrentKF->SetErase();
        return false;           
    }
    ......
    ......
    后面细分讲解
}

三、最终候选关键帧 mvpEnoughConsistentCandidates

在上一步操作中，已经获得了初始候选关键帧 vpCandidateKFs，闭环操作是一个比较重要的操作，不能说几个初始候选关键帧就认为目前已经进入闭环状态了。这里我们可以想象一下，如图一所示，如果机器人回到了其实位置，检测到了闭环帧，那么一般来说，其继续向前走，那么应该会继续检测到回环。

基于上述的思想，ORB-SLAM2 在回环的时候用到了连续的概念。为了具体的进行讲解，先来看下图：
在这里插入图片描述
先来对上图进行一个介绍：

实心红点: 当前关键帧；
绿色实心外红圈: 初始候选关键帧,该些关键帧与当前关键帧是不具备连接关系的
绿色实心外蓝圈(细): 与初始候选关键帧具备连接关系的关键帧
绿色实心外蓝圈(粗)与蓝色实心外绿圈(粗)是同一个关键帧

下面呢，我们就结合上图做一个比较细致的讲解，接着前面的代码段进行分析。

$\color{blue}(1)：$ 对所有初始候选帧 vpCandidateKFs 进行遍历，获得每一个初始候选帧pCandidateKF，然后得到与该候选关键帧连接的所有关键帧，共同形成一个子候选组spCandidateGroup。对应于上图的如下部分：
在这里插入图片描述
上图表示的是2个子候选组spCandidateGroup，这两个候选组的中心就是遍历的候选关键帧。

$\color{blue}(2)：$ 首先这里涉及到一个连续的概念，暂且先放一下。获得子候选组 spCandidateGroup 之后，其会在连续组 vCurrentConsistentGroups 中进行搜索，也就是上图的：在这里插入图片描述该图对应于 vCurrentConsistentGroups，这里我们称为所有连续组(其存储的是连续组)，上图中表示存储了2个子连续组。子连续组实际来源于子候选组 spCandidateGroup。遍历时获得一个候选关键帧的子候选组之后，该子候选组在所有连续组中进行搜索匹配，如果没有匹配上，或者所有连续组目前为空，则会把子候选组 spCandidateGroup 加入到所有连续组所有vCurrentConsistentGroups之中。他的连续长度0，也就是说子候选组变成了一个子连续组。

$\color{blue}(3)：$ 该子候选组 spCandidateGroup 在所有连续组 vCurrentConsistentGroups 中进行搜索匹配，如果匹配上，也会加入到所有连续组 vCurrentConsistentGroups 中，现在来回答前面的问题？什么是连续呢？如果所有连续组vCurrentConsistentGroups 中，存在一个子连续组的关键帧与子候选组的关键帧相同，如下图所示：
在这里插入图片描述
那么则认为这个子候选组是连续的，然后加入到所有连续组vCurrentConsistentGroups 中，且该候选组连续长度在原子连续组的基础上 +1。

$\color{blue}(4)：$ 如果连续长度满足要求，那么当前的这个候选关键帧是足够靠谱的，连续性阈值 mnCovisibilityConsistencyTh=3。该候选帧会被添加到 mvpEnoughConsistentCandidates 之中，同时也表示闭环检测成功了。

$\color{blue}(5)：$ 对成员变量连续组更新→mvConsistentGroups = vCurrentConsistentGroups；当前闭环检测的关键帧添加到关键帧数据库中→mpKeyFrameDB->add(mpCurrentKF);

代码注释如下，这里的代码注释是接着前面的代码，同样为 bool LoopClosing::DetectLoop() 函数的注释。

	......
	......

    // For each loop candidate check consistency with previous loop candidates
    // Each candidate expands a covisibility group (keyframes connected to the loop candidate in the covisibility graph)
    // A group is consistent with a previous group if they share at least a keyframe
    // We must detect a consistent loop in several consecutive keyframes to accept it
    // Step 5：在候选帧中检测具有连续性的候选帧
    // 1、每个候选帧将与自己相连的关键帧构成一个“子候选组spCandidateGroup”， vpCandidateKFs-->spCandidateGroup
    // 2、检测“子候选组”中每一个关键帧是否存在于“连续组”，如果存在 nCurrentConsistency++，则将该“子候选组”放入“当前连续组vCurrentConsistentGroups”
    // 3、如果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中只保存连续组链中末尾的组

    // 最终筛选后得到的闭环帧
    mvpEnoughConsistentCandidates.clear();

    // ConsistentGroup数据类型为pair<set<KeyFrame*>,int>
    // ConsistentGroup.first对应每个“连续组”中的关键帧，ConsistentGroup.second为每个“连续组”的已连续几个的序号

    vector<ConsistentGroup> vCurrentConsistentGroups;

    // 这个下标是每个"子连续组"的下标,bool表示当前的候选组中是否有和该组相同的一个关键帧
    vector<bool> vbConsistentGroup(mvConsistentGroups.size(),false);

    // Step 5.1：遍历刚才得到的每一个候选关键帧
    for(size_t i=0, iend=vpCandidateKFs.size(); i<iend; i++)
    {
    
    
        KeyFrame* pCandidateKF = vpCandidateKFs[i];

        // Step 5.2：将自己以及与自己相连的关键帧构成一个“子候选组”
        set<KeyFrame*> spCandidateGroup = pCandidateKF->GetConnectedKeyFrames();
        // 把自己也加进去
        spCandidateGroup.insert(pCandidateKF);

        // 连续性达标的标志
        bool bEnoughConsistent = false;
        bool bConsistentForSomeGroup = false;
        // Step 5.3：遍历前一次闭环检测到的连续组链
        // 上一次闭环的连续组链 std::vector<ConsistentGroup> mvConsistentGroups
        // 其中ConsistentGroup的定义：typedef pair<set<KeyFrame*>,int> ConsistentGroup
        // 其中 ConsistentGroup.first对应每个“连续组”中的关键帧集合，ConsistentGroup.second为每个“连续组”的连续长度
        for(size_t iG=0, iendG=mvConsistentGroups.size(); iG<iendG; iG++)
        {
    
    
            // 取出之前的一个子连续组中的关键帧集合
            set<KeyFrame*> sPreviousGroup = mvConsistentGroups[iG].first;

            // Step 5.4：遍历每个“子候选组”，检测子候选组中每一个关键帧在“子连续组”中是否存在
            // 如果有一帧共同存在于“子候选组”与之前的“子连续组”，那么“子候选组”与该“子连续组”连续
            bool bConsistent = false;
            for(set<KeyFrame*>::iterator sit=spCandidateGroup.begin(), send=spCandidateGroup.end(); sit!=send;sit++)
            {
    
    
                if(sPreviousGroup.count(*sit))
                {
    
    
                    // 如果存在，该“子候选组”与该“子连续组”相连
                    bConsistent=true;
                    // 该“子候选组”至少与一个”子连续组“相连，跳出循环
                    bConsistentForSomeGroup=true;
                    break;
                }
            }

            if(bConsistent)
            {
    
    
                // Step 5.5：如果判定为连续，接下来判断是否达到连续的条件
                // 取出和当前的候选组发生"连续"关系的子连续组的"已连续次数"
                int nPreviousConsistency = mvConsistentGroups[iG].second;
                // 将当前候选组连续长度在原子连续组的基础上 +1，
                int nCurrentConsistency = nPreviousConsistency + 1;
                // 如果上述连续关系还未记录到 vCurrentConsistentGroups，那么记录一下
                // 注意这里spCandidateGroup 可能放置在vbConsistentGroup中其他索引(iG)下
                if(!vbConsistentGroup[iG])
                {
    
    
                    // 将该“子候选组”的该关键帧打上连续编号加入到“当前连续组”
                    ConsistentGroup cg = make_pair(spCandidateGroup,nCurrentConsistency);
                    // 放入本次闭环检测的连续组vCurrentConsistentGroups里
                    vCurrentConsistentGroups.push_back(cg);
                    //this avoid to include the same group more than once
                    // 标记一下，防止重复添加到同一个索引iG
                    // 但是spCandidateGroup可能重复添加到不同的索引iG对应的vbConsistentGroup 中
                    vbConsistentGroup[iG]=true; 
                }
                // 如果连续长度满足要求，那么当前的这个候选关键帧是足够靠谱的
                // 连续性阈值 mnCovisibilityConsistencyTh=3
                // 足够连续的标记 bEnoughConsistent
                if(nCurrentConsistency>=mnCovisibilityConsistencyTh && !bEnoughConsistent)
                {
    
    
                    // 记录为达到连续条件了
                    mvpEnoughConsistentCandidates.push_back(pCandidateKF);
                    //this avoid to insert the same candidate more than once
                    // 标记一下，防止重复添加
                    bEnoughConsistent=true; 

                    // ? 这里可以break掉结束当前for循环吗？
                    // 回答：不行。因为虽然pCandidateKF达到了连续性要求
                    // 但spCandidateGroup 还可以和mvConsistentGroups 中其他的子连续组进行连接
                }
            }
        }

        // If the group is not consistent with any previous group insert with consistency counter set to zero
        // Step 5.6：如果该“子候选组”的所有关键帧都和上次闭环无关（不连续），vCurrentConsistentGroups 没有新添加连续关系
        // 于是就把“子候选组”全部拷贝到 vCurrentConsistentGroups， 用于更新mvConsistentGroups，连续性计数器设为0
        if(!bConsistentForSomeGroup)
        {
    
    
            ConsistentGroup cg = make_pair(spCandidateGroup,0);
            vCurrentConsistentGroups.push_back(cg);
        }
    }// 遍历得到的初级的候选关键帧

    // Update Covisibility Consistent Groups
    // 更新连续组
    mvConsistentGroups = vCurrentConsistentGroups;


    // Add Current Keyframe to database
    // 当前闭环检测的关键帧添加到关键帧数据库中
    mpKeyFrameDB->add(mpCurrentKF);

    if(mvpEnoughConsistentCandidates.empty())
    {
    
    
        // 未检测到闭环，返回false
        mpCurrentKF->SetErase();
        return false;
    }
    else 
    {
    
    
        // 成功检测到闭环，返回true
        return true;
    }

    // 多余的代码,执行不到
    mpCurrentKF->SetErase();
    return false;
}

四、结语

通过该篇博客，我们对闭环检测做了一个比较详细的讲解。整体逻辑获取不是很复杂，但是代码实现，确实需要花点心思琢磨一下。