ORB SLAM2源码解读(五):KeyFrame DataBase类

该类的主要作用就是利用词袋数据,在已有的关键帧中查找和当前帧最接近的帧。

这个功能有两个作用,一是重定位时候,通过检测当前帧和哪个关键帧最接近,来确定相机当前的位置和姿态,对应的检测函数是DetectRelocalizationCandidates。二是在闭环检测时,通过检测来确定当前关键帧需要和哪些关键帧建立闭环修正的边,对应的检测函数是DetectLoopCandidates。 二者的区别不大,唯一的区别是闭环检测时不需要遍历和自己在闭环检测之前就已经有共视关系的关键帧。

1. DetectRelocalizationCandidates

检测的主要步骤如下:

1)找出与当前帧pKF有公共单词的所有关键帧pKFi,不包括与当前帧相连的关键帧。

2)统计所有闭环候选帧中与pKF具有共同单词最多的单词数,只考虑共有单词数大于0.8*maxCommonWords以及匹配得分大于给定的minScore的关键帧,存入lScoreAndMatch。

3)对于第二步中筛选出来的pKFi,每一个都要抽取出自身的共视(共享地图点最多的前10帧)关键帧分为一组,计算该组整体得分(与pKF比较的),记为bestAccScore。所有组得分大于0.75*bestAccScore的,均当作闭环候选帧。

vector<KeyFrame*> KeyFrameDatabase::DetectRelocalizationCandidates(Frame *F)
{
    list<KeyFrame*> lKFsSharingWords;

    // Search all keyframes that share a word with current frame
    //搜索所有和和F有着相同单词的keyframe存储在lKFsSharingWords
    //并且更新keyframe中mnRelocWords,表示和此F有多少共同的单词
    {
        unique_lock<mutex> lock(mMutex);

        for(DBoW2::BowVector::const_iterator vit=F->mBowVec.begin(), vend=F->mBowVec.end(); vit != vend; vit++)
        {
            list<KeyFrame*> &lKFs =   mvInvertedFile[vit->first];

            for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
            {
                KeyFrame* pKFi=*lit;
                if(pKFi->mnRelocQuery!=F->mnId)
                {
                    pKFi->mnRelocWords=0;
                    pKFi->mnRelocQuery=F->mnId;
                    lKFsSharingWords.push_back(pKFi);
                }
                pKFi->mnRelocWords++;
            }
        }
    }
    if(lKFsSharingWords.empty())
        return vector<KeyFrame*>();

    // Only compare against those keyframes that share enough words
    //在lKFsSharingWords中,寻找mnRelocWords的最大值存入maxCommonWords
    int maxCommonWords=0;
    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
    {
        if((*lit)->mnRelocWords>maxCommonWords)
            maxCommonWords=(*lit)->mnRelocWords;
    }

    int minCommonWords = maxCommonWords*0.8f;

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

    int nscores=0;

    // Compute similarity score.
    //遍历lKFsSharingWords中的keyframe,当其中的keyframe的mRelocScore大于阈值minCommonWords则计算相似度后放入lScoreAndMatch中
    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
    {
        KeyFrame* pKFi = *lit;

        if(pKFi->mnRelocWords>minCommonWords)
        {
            nscores++;
            float si = mpVoc->score(F->mBowVec,pKFi->mBowVec);
            pKFi->mRelocScore=si;
            lScoreAndMatch.push_back(make_pair(si,pKFi));
        }
    }

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

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

    // Lets now accumulate score by covisibility
    //遍历lScoreAndMatch中的keyframe,找出其共视图中与此keyframe连接的权值前N的节点,加上原keyframe总共11个keyframe
    //累加这11个keyframe的相似度得分,然后在11个keyframe中选择相似度得分最高的那个放入lAccScoreAndMatch中
    //在遍历过程中计算bestAccScore,也就是AccScore的最大值,后面的再次筛选有用
    for(list<pair<float,KeyFrame*> >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++)
    {
        KeyFrame* pKFi = it->second;
        //返回共视图中与此keyframe连接的权值前10的节点keyframe
        vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);

        float bestScore = it->first;
        float accScore = bestScore;
        KeyFrame* pBestKF = pKFi;
        for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)
        {
            KeyFrame* pKF2 = *vit;
            //说明pKF2与F没有共同的单词,就放弃此循环的关键帧
            if(pKF2->mnRelocQuery!=F->mnId)
                continue;

            accScore+=pKF2->mRelocScore;
            if(pKF2->mRelocScore>bestScore)
            {
                pBestKF=pKF2;
                bestScore = pKF2->mRelocScore;
            }

        }
        lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));
        if(accScore>bestAccScore)
            bestAccScore=accScore;
    }

    // Return all those keyframes with a score higher than 0.75*bestScore
    //返回lAccScoreAndMatch中所有得分超过0.75*bestAccScore的keyframe集合
    float minScoreToRetain = 0.75f*bestAccScore;
    set<KeyFrame*> spAlreadyAddedKF;
    vector<KeyFrame*> vpRelocCandidates;
    vpRelocCandidates.reserve(lAccScoreAndMatch.size());
    for(list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)
    {
        const float &si = it->first;
        if(si>minScoreToRetain)
        {
            KeyFrame* pKFi = it->second;
            if(!spAlreadyAddedKF.count(pKFi))
            {
                vpRelocCandidates.push_back(pKFi);
                spAlreadyAddedKF.insert(pKFi);
            }
        }
    }

    return vpRelocCandidates;
}

2. DetectLoopCandidates

上面说道,这个函数的流程和DetectRelocalizationCandidates唯一的区别是忽略和自己已有共视关系的关键帧,所以我们此处不复制全部的代码了,只重点强调不同的地方

for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit != vend; vit++)
{
    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)
         {
               pKFi->mnLoopWords=0;
               // 此处如果if条件成立,代表没有共视关系,此时才会进入执行语句
               // 换言之,如果有共视关系,就直接忽略了,这是它和DetectRelocalizationCandidates唯一的区别
               if(!spConnectedKeyFrames.count(pKFi))
               {
                    pKFi->mnLoopQuery=pKF->mnId;
                    lKFsSharingWords.push_back(pKFi);
               }
          }
          pKFi->mnLoopWords++;
     }
}