ORB-SLAM2中的Loop Closinng中DetectLoopCandidates函数解析

/函数的三要素是：函数返回值类型，函数名称，函数参数
函数的返回值是装有关键帧指针的vector
该函数是类KeyFrameDatabase的成员函数,函数名是DetectLoopCandidate
该函数的参数分别是KeyFrame类型的指针变量 pKF和最小得分

vector<KeyFrame*> KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF, float minScore)
{
    
    spConnectedKeyFrames是一个set定义的变量，set中装的是在covisibility graph中与
    关键帧pKF相关联的关键帧。
    GetConnectedKeyFrame()是类KeyFrame的成员函数，通过遍历变量map变量                 
    mConnectedKeyFrameWeights来得到关联帧。
    map<KeyFrame*, int> mConnectedKeyFrameWeights;

    set 有自动排序功能，不能直接存取元素
    list不可以随机存取元素
    set<KeyFrame*> spConnectedKeyFrames = pKF->GetConnectedKeyFrames();
    list<KeyFrame*> lKFsSharingWords;

    // Search all keyframes that share a word with current keyframes
    // Discard keyframes connected to the query keyframe
    {
        unique_lock<mutex> lock(mMutex);
        //遍历pKF中所有生成的Bow向量
        for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit != vend; vit++)
        {
            //在inverse indexes中查找该word都在哪些图像中出现过
            list<KeyFrame*> &lKFs =   mvInvertedFile[vit->first];
            //遍历这些查找出来的与pKF有共视的word的关键帧
            for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
            {
                //紧接着这一小段程序可以统计pKF与pKFi共视words的个数。

                //到了这里就是说pKF与pKFi有共视关系，但是并不是pKFi都可以被认为是闭环候选帧
                //只有当pKFi不是在covisibility graph中与pKF直接相连的关键帧才有机会入围。
                KeyFrame* pKFi=*lit;
                //如果pKFi是第一次被pKF查询，那么先初始化成员变量mnLoopwords为0
                //经判断pKFi确实不与pKF直接相连则将成员变量mnLoopQuery设置为pKF的Id号码
                //表示pKFi已经被pKF查询过，下次再次查询pKFi的时候
                //pKFi->mnLoopQuery = pKF->mnId 则直接让 pKFi->mnLoopWords++;
                //并且将pKFi插入到lKFsSharingwords中去。
                if(pKFi->mnLoopQuery!=pKF->mnId)
                {
                    pKFi->mnLoopWords=0;
                    if(!spConnectedKeyFrames.count(pKFi))
                    {
                        pKFi->mnLoopQuery=pKF->mnId;
                        lKFsSharingWords.push_back(pKFi);
                    }
                }
                pKFi->mnLoopWords++;
            }
        }
    }

    if(lKFsSharingWords.empty())
        return vector<KeyFrame*>();

    //IScoreAndMatch变量中与pKF有共视关系的关键帧以及两者之间的得分
    //pair<float, KeyFrame>pair的使用是将两者捆绑到一起存储

    list<pair<float,KeyFrame*> > lScoreAndMatch;

    
    int maxCommonWords=0;
    //遍历那些所有与pKF有共视关系又满足条件的存储在lKFsSharingWords中的关键帧
    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
    {
         //从上面的程序我们知道mnLoopWords变量中存储了该关键帧与pKF的共视word数目
         //找到这些所有与pKF有共视关系的关键帧中与pKF共视word最大值 maxCommonWords

        if((*lit)->mnLoopWords>maxCommonWords)
            maxCommonWords=(*lit)->mnLoopWords;
    }
    //但是我们如果要求这么严格，那满足我们条件的关键帧真的是寥寥无几啦，所以为了得到多一些的
    //闭环候选帧我们不得不降低要求，让前20%的关键帧都能进入下一轮的比赛。
    //如果这个要求您都不能满足的话，那对不起，您只能等下一次了。

    int minCommonWords = maxCommonWords*0.8f;

    int nscores=0;

    //再次遍历lKFsSharingWords中存储的所有与pKF有共视关系的关键帧
    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
    {
        //将每一个关键帧单独拿出来，看一看他的mnLoopWords与minCommonWords的大小
        
        KeyFrame* pKFi = *lit;
        if(pKFi->mnLoopWords>minCommonWords)
        {
            nscores++;
            //对于那些共视单词数满足条件的pKFi计算与pKF之间的BoW得分
            float si = mpVoc->score(pKF->mBowVec,pKFi->mBowVec);

            //mLoopScore目前的值是pKFi与pKF之间的Bow得分。

            pKFi->mLoopScore = si;
            //只有那些得分大于minScore的pKFi才可以留下
            第三关：Bow得分限制
            if(si>=minScore)
                lScoreAndMatch.push_back(make_pair(si,pKFi));
        }
    }

    if(lScoreAndMatch.empty())
        return vector<KeyFrame*>();

    list<pair<float,KeyFrame*> > lAccScoreAndMatch;
    float bestAccScore = minScore;

    // Lets now accumulate score by covisibility

    //遍历所有上一贯留下的关键帧pKFi
    for(list<pair<float,KeyFrame*> >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++)
    {
        //单独拿出每一帧
        KeyFrame* pKFi = it->second;
        //查找在covisibility graph上与pKFi连接最密切的10个关键帧
        vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);

        //bestScore初始化为pKFi与pKF之间的得分
        float bestScore = it->first;
        //这个累计得分accScore也被初始化为pKFi与pKF之间的得分
        float accScore = it->first;
        //而最佳帧选也初始化为pKFi
        KeyFrame* pBestKF = pKFi;
        
        //遍历每一个pKFi的10个联系最为密切的关键帧
        for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)
        {
            KeyFrame* pKF2 = *vit;
            //如果在这10个关键帧中有之前被pKF查询过（即有共视单词），并且共视的单词数还满足
            //大于minCommonwords的要求
            if(pKF2->mnLoopQuery==pKF->mnId && pKF2->mnLoopWords>minCommonWords)
            {
                //那么就要在原来pKFi与pKF得分的基础上再加上pKFi的“好友”pKF2与pKF之间的得分
                accScore+=pKF2->mLoopScore;
                //残忍的时刻到了，倘若pKF2与pKF之间的得分要比pKFi大，
                //pKF2就会超过pKFi pBestKF就成了pKF2了
                if(pKF2->mLoopScore>bestScore)
                {
                    pBestKF=pKF2;
                    bestScore = pKF2->mLoopScore;
                }
            }
        }
        //现在存储是从多个成员是11的小组内选择出的与pKF得分最高的pBestKF以及累计得分accScore
        lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));
        //并且在所有的accScore中找到那个最大的得分bestAccScore.
        if(accScore>bestAccScore)
            bestAccScore=accScore;
    }

   //但是为了能够得到多一些的闭环候选帧，需要降低要求，将那些上一关留下来分数靠前25%的关键帧
    //留下来
    float minScoreToRetain = 0.75f*bestAccScore;

    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++)
    {
        //将那些上一关留下来分数靠前25%的关键帧留下来
        if(it->first>minScoreToRetain)
        {
            KeyFrame* pKFi = it->second;
            //如果检测到set中已经有pKFi了就不要重复插入了，其实这个判断是多余的
            //set本来就具有值唯一性，不可以存入重复的值。
            if(!spAlreadyAddedKF.count(pKFi))
            {
                vpLoopCandidates.push_back(pKFi);
                spAlreadyAddedKF.insert(pKFi);
            }
        }
    }

    //最后终于得到了想要的关键帧，就想皇帝选妃子，需要层层选拔
    //可以走到这里的关键帧真的是过五关斩六将了
    //第一关：与pKF有共视单词的pKF1，且不能是与pKF在covisibility graph中与pKF直接相连的
    //关键帧
    //第二关：共视单词数必须大于minCommonWords的才可以留下 pKF2
    //第三关：pKF2中与pKF的BOw得分必须大于minScore的才可以留下 pKF3
    //第四关：pKF3要与自己最亲密的“朋友”PK与pKF之间的共视单词数，留下的记为pKF4
    //第五关：pKF4中那些与pKF得分大于minScoreToRetain的才可以最终留下来 记为pKF5;
    return vpLoopCandidates;
}