1、ORBSLAM yyds，闭环检测第一步——闭环候选关键帧的层层选拔（谨慎的态度都是细节）

1、回环检测的重要性

读过《视觉SLAM十四讲》的很熟悉下面的图，这张图足以说明回环检测的重要性。因为只依靠前端根据传感器的采集数据进行轨迹推演，即使加上后端的各种优化策略，累积误差也是不可避免的，可见优化不是万灵药，抵不住传感器送来的数据不行啊，这时候回环检测就能利用传感器在同样地方采集的数据相似这种特性，来将漂移了的轨迹修复为一个闭环，有效消除累积误差，得到一个较好的全局一致的估计。

2、相关关键帧的名称

这一阶段知道上面几个概念就好了，其中蓝色的实线表示共视并且相连接，红色虚线表示不相连接但可能共视。（相连就代表是目前跟踪的局部连接位姿，而闭环找的是与当前的局部相似的很早以前的位姿啊。）

3、回环检测的闭环候选帧层层选拔

3.1 Round 1

检查一下闭环关键帧队列里是否为空，你说要跟闭环检测帧闭环，去哪找？可不得去闭环关键帧队列里去嘛。一般是不为空的。这个队列里的关键帧是从局部建图线程中插入的。具体代码如下：

/*
 * 查看列表中是否有等待被插入的关键帧
 * @return 如果存在，返回true
 */
bool LoopClosing: :CheckNewKeyFrames()
{
    unique_lock<mutex> lock(mMutexLoopQueue);
    return(!mlpLoopKeyFrameQueue.empty());//闭环关键帧队列里是否为空，由InsertKeyFrame(）函数，在Local Mappoing Step8里把关键帧插入进来的
}

// 将某个关键帧加入到回环检测的过程中,由局部建图线程调用
void LoopClosing::InsertKeyFrame(KeyFrame *pKF)
{
    unique_lock<mutex> lock(mMutexLoopQueue);
    // 注意：这里第0个关键帧不能够参与到回环检测的过程中,因为第0关键帧定义了整个地图的世界坐标系
    if(pKF->mnId!=0)//第0帧不能用于闭环检测，因为第0帧定义整个地图的世界坐标系
        mlpLoopKeyFrameQueue.push_back(pKF);//当前关键帧加入到闭环检测的关键帧队列
}

3.2 Round 2

统计与闭环检测帧有连接关系的关键帧（共视关键帧），并计算出闭环检测帧与每个共视关键的bow相似度得分，并得到最低得分minScore。
目的：闭环检测关键帧和闭环候选关键帧建立联系的条件是 共视关系要比闭环检测关键帧的共视关键帧得分最低的那个高才行
关于如何找与闭环检测帧的共视关键帧，见以下示意图

具体实现对应代码如下：

const vector<KeyFrame*> vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames();//取出当前关键帧连接的所有共视关键帧（这里面放的是至少大于15个共视地图点的关键帧，已经按权值从大到小排列）
    const DBoW2::BowVector &CurrentBowVec = mpCurrentKF->mBowVec;//取出当前关键帧的词袋BowVector
    float minScore = 1;//定义最低分数阈值，初始化为1
    for(size_t i=0; i<vpConnectedKeyFrames.size(); i++)//循环当前关键帧连接的所有共视关键帧
    {
        KeyFrame* pKF = vpConnectedKeyFrames[i];//取出当前循环的当前关键帧连接的共视关键帧
        if(pKF->isBad())//判断共视关键帧是否Bad
            continue;
        const DBoW2::BowVector &BowVec = pKF->mBowVec;//取出当前循环的当前关键帧连接的共视关键帧的词袋BowVector
        // 计算两个关键帧的相似度得分；得分越低,相似度越低
        float score = mpORBVocabulary->score(CurrentBowVec, BowVec);//计算当前关键帧和共视关键帧的相似度得分（调用的DBow库里函数）
        // 更新最低得分
        if(score<minScore)//记录最小分数minScore （闭环候选关键帧和当前关键帧的相似度得分至少要达到这个最低得分，才能考虑闭环候选关键帧和当前关键帧有闭环关系）
            minScore = score;
    }

minScore的作用：认为和闭环检测关键帧具有回环关系的关键帧,不应该低于当前关键帧的相邻关键帧的最低的相似度minScore

3.3 Round 3

计算与当前关键帧存在最低相似度得分的闭环候选关键帧，且不是当前关键帧的共视关键帧（原因如前：相连就代表是目前跟踪的局部连接位姿，而闭环找的是与当前的局部相似的很早以前的位姿啊。）

vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);//跳转

set<KeyFrame*> spConnectedKeyFrames = pKF->GetConnectedKeyFrames();//取出当前关键帧的连接的共视关键帧

    // 用于保存可能与闭环检测关键帧形成闭环的候选帧（只要有相同的word，且不属于共视帧）
    list<KeyFrame*> lKFsSharingWords;
    // 来了：找出和当前关键帧具有公共单词的所有关键帧，不是当前关键帧的共视关键帧
    {
        unique_lock<mutex> lock(mMutex);
        for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit != vend; vit++)//循环闭环检测关键帧所有的BowVector
        {
            // 提取所有包含该word的关键帧,然后对这些关键帧展开遍历
            list<KeyFrame*> &lKFs = mvInvertedFile[vit->first];//这里用到了倒排索引
            for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
            {
                KeyFrame* pKFi=*lit;
                if(pKFi->mnLoopQuery!=pKF->mnId)//为了防止后面的重复添加，做上标记
                {
                    // 还没有标记为pKF的闭环候选关键帧
                    pKFi->mnLoopWords=0;//其与闭环检测关键帧具有相同word的个数，初始化为0
                    // 和闭环检测关键帧共视的话就不作为闭环候选帧（只要不共视的）
                    if(!spConnectedKeyFrames.count(pKFi))//判断当前循环的关键帧 不是 闭环检测关键帧的共视关键帧，才能作为闭环候选关键帧  
                    {
                        // 没有共视就标记作为闭环候选关键帧，放到lKFsSharingWords里
                        pKFi->mnLoopQuery=pKF->mnId;//该关键帧的闭环候选关键帧标记为当前关键帧ID，标记了该关键帧是id为mnLoopQuery（pKF->mnId）的闭环候选关键帧
                        lKFsSharingWords.push_back(pKFi);//并且加入到闭环候选关键帧列表lKFsSharingWords
                    }
                }
                pKFi->mnLoopWords++;// 记录pKFi与pKF具有相同word的个数
            }
        }

3.4 Round 4

这时候已经选出了有资格能当闭环候选帧的最大范围的关键帧了，接下来会怎么办呢？总不能，挨个检测回环吧，继续选，选出哪个最像的来（何为最像就是靠前一回合计算的公共单词做阈值继续筛选。）
首先，计算上面所有闭环候选关键帧的公共单词数，以这个单词数的0.8倍，进一步滤除掉一些关键帧。然后将剩下的每个闭环候选关键帧和其共视帧，且必须是闭环候选帧的所有关键帧作为一个组，计算每个组的相似度得分，然后取最高相似组的得分*0.75作为阈值滤掉一些组。

list<pair<float,KeyFrame*> > lScoreAndMatch;//定义 1.和闭环检测关键帧有最小共有单词数 且 2.和闭环检测关键帧的相似度得分大于最低相似度得分 的闭环候选关键帧     
    // 统计上述所有闭环候选帧中与闭环检测关键帧具有共同单词最多的单词数，用来决定相对阈值 
    int maxCommonWords=0;//定义闭环候选关键帧中与闭环检测关键帧具有共同单词最多的单词数，初始化为0
    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)//循环闭环候选关键帧
    {
        if((*lit)->mnLoopWords>maxCommonWords)//判断当前循环的闭环候选关键帧与闭环检测关键帧的共有单词数 是否 大于最大共有单词数
            maxCommonWords=(*lit)->mnLoopWords;//把闭环候选关键帧与闭环检测关键帧最大共有的单词数目放到maxCommonWords
    }

    // 确定最小公共单词数为最大公共单词数目的0.8倍（这不阈值出来了嘛，也就是说下面选出来的关键帧更严格了，也就是与闭环检测关键帧更像了）
    int minCommonWords = maxCommonWords*0.8f;//定义最小共有单词数阈值

    int nscores=0;//定义得分，初始化为0
    
    // 遍历上述所有闭环候选帧，挑选出共有单词数大于minCommonWords且单词匹配度大于minScore存入lScoreAndMatch
    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)//根据上面得到的最小单词阈值，循环闭环候选关键帧，筛选出比最小单词数大的闭环候选关键帧，才会进行下一步考虑
    {
        KeyFrame* pKFi = *lit;//取出当前循环的闭环候选关键帧

        // pKF只和具有共同单词较多（大于minCommonWords）的关键帧进行比较
        if(pKFi->mnLoopWords>minCommonWords)//判断 当前循环的闭环候选关键帧与当前关键帧共有的单词数目 是否大于 最小共有单词数阈值
        {
            nscores++;// 这个变量后面没有用到

            // 用mBowVec来计算两者的相似度得分
            float si = mpVoc->score(pKF->mBowVec,pKFi->mBowVec);//计算当前关键帧和闭环候选关键帧（大于最小共有单词数）之间的词袋相似度得分

            pKFi->mLoopScore = si;//记录词袋相似度
            if(si>=minScore)
                lScoreAndMatch.push_back(make_pair(si,pKFi));
        }
    }

    // 如果没有超过指定相似度阈值的，那么也就直接跳过去
    if(lScoreAndMatch.empty())
        return vector<KeyFrame*>();

    list<pair<float,KeyFrame*> > lAccScoreAndMatch;//定义 满足上面2个条件的闭环候选关键帧的10个最佳共视关键帧的 累加词袋相似度得分 和 最高词袋相似度得分的共视关键帧
    float bestAccScore = minScore;//定义最高组得分，所有闭环候选关键帧的共视关键帧中的最高的累计相似度得分，初始化为minScore

    // 单单计算闭环检测关键帧和某一关键帧的相似性是不够的，这里将与关键帧相连（权值最高，共视程度最高）的前十个关键帧归为一组，计算累计得分
    // 计算上述候选帧对应的共视关键帧组的总得分，得到最高组得分bestAccScore，并以此决定阈值minScoreToRetain  
    for(list<pair<float,KeyFrame*> >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++)
    {
        KeyFrame* pKFi = it->second;//取出当前循环的闭环候选关键帧
        vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);//取出闭环候选关键帧的前10个最佳共视关键帧（权值从大到小排列）

        float bestScore = it->first; // 该组最高分数
        float accScore = it->first;  // 该组累计得分
        KeyFrame* pBestKF = pKFi;    // 该组最高分数对应的关键帧
        // 遍历共视关键帧，累计得分 
        for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)
        {
            KeyFrame* pKF2 = *vit;//取出当前循环的闭环候选关键帧的共视关键帧
            // 只有pKF2也在闭环候选帧中，且公共单词数超过最小要求，才能贡献分数
            if(pKF2->mnLoopQuery==pKF->mnId && pKF2->mnLoopWords>minCommonWords)//判断当前循环的闭环候选关键帧的共视关键帧 有没有标记为 当前关键帧的闭环候选关键帧（即共视关键帧也要是当前关键帧的闭环候选关键帧） 并且 要大于 和当前关键帧具有最小共有单词数 
            {
                accScore+=pKF2->mLoopScore;//记录组得分，即 累加 当前循环的闭环候选关键帧的共视关键帧和当前关键帧的词袋相似度得分
                // 统计得到组里分数最高的关键帧
                if(pKF2->mLoopScore>bestScore)//记录组内最高得分 //判断当前循环的闭环候选关键帧的共视关键帧和当前关键帧的词袋相似度得分 是否为 该组最高分数（当前闭环候选关键帧的10个共视关键帧中最高分数）
                {
                    pBestKF=pKF2;//记录组内最大相似度得分对应的当前循环的闭环候选关键帧中的共视关键帧
                    bestScore = pKF2->mLoopScore;//记录组内最大相似度得分对应的当前循环的闭环候选关键帧中的共视关键帧的得分
                }
            }
        }

        lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));//把当前循环的闭环候选关键帧的10个最佳共视关键帧的 累加词袋相似度得分 和 组内最高词袋相似度得分的共视关键帧 放到lAccScoreAndMatch
        // 记录所有组中组得分最高的组，用于确定相对阈值
        if(accScore>bestAccScore)//判断当前循环的闭环候选关键帧的组得分（10个最佳共视关键帧的累加词袋相似度得分）是否大于最高组得分（外层for循环结束得到所有闭环候选关键帧的共视关键帧组中的最高累计相似度得分）
            bestAccScore=accScore;//记录最高组得分
    }
    // 所有组中最高得分的0.75倍，作为最低阈值
    float minScoreToRetain = 0.75f*bestAccScore;//定义组阈值

3.5 Round 5

上面是围绕每一帧闭环候选帧进行分组计算其与闭环检测帧的相似度得分，并且通过阈值滤掉一下关键帧。这里只取剩下的闭环候选关键帧组里面相似度得分最高的那一个关键帧作为最终的闭环候选帧，可见这里得到的还不只是一个，而是剩下几个组就有几个闭环候选关键帧，但是已经非常相似了，这时候有很大的把握认为这些帧可以用来检测和计算闭环了。

	set<KeyFrame*> spAlreadyAddedKF;
    vector<KeyFrame*> vpLoopCandidates;
    vpLoopCandidates.reserve(lAccScoreAndMatch.size());

    // 只取组得分大于阈值的组，得到组中分数最高的关键帧作为闭环候选关键帧
    for(list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)//循环 闭环候选关键帧的组得分（10个最佳共视关键帧的累加词袋相似度得分）和 组内最高得分的共视关键帧（组内最高词袋相似度得分的共视关键帧）
    {
        if(it->first>minScoreToRetain)//判断当前循环的闭环候选关键帧的组得分 是否大于 组阈值
        {
            KeyFrame* pKFi = it->second;//如果大于，取出当前循环的闭环候选关键帧组的组内最高得分的共视关键帧（这个共视关键帧是也是当前关键帧的闭环候选关键帧）
            // spAlreadyAddedKF 是为了防止重复添加
            if(!spAlreadyAddedKF.count(pKFi))//判断是否已经添加到了真正的闭环候选关键帧，防止重复添加
            {
                vpLoopCandidates.push_back(pKFi);//放进真正的闭环候选关键帧中
                spAlreadyAddedKF.insert(pKFi);//标记这个关键帧已经放入了真正的闭环候选关键帧，防止重复添加
            }
        }
    }

    return vpLoopCandidates;//返回真正的闭环候选关键帧

4、回环检测的闭环候选帧为什么要层层选拔呢？

其实原因很简单：就是害怕误闭环啊，闭环一旦错误整个轨迹肯定乱了套了，所以要谨慎加谨慎。
其实，谨慎到这个份上还不够，还是不能认为这几帧就是闭环可以基于这几帧进行全局优化了。这个下次在写吧，累了。。。。。

5、非常感谢您的阅读，也希望您能提供宝贵意见，欢迎交流！

6 期待您加入

也非常期待您能关注我的微信公众号–“过千帆”，里面不仅有技术文章还有我的读书分享，希望您在那里也有收获。我们一起进步。