ORB-SLAM2代码笔记（七）：KeyFrameDatabas

2019-07-26 本文已影响0人 liampayne_66d0

KeyFrameDatabase

vector<list<KeyFrame*> > mvInvertedFile;
倒排索引，mvInvertedFile[i]表示包含了第i个word id的所有关键帧，数据库的主要内容
void KeyFrameDatabase::add(KeyFrame *pKF) 输入关键帧，根据关键帧的词包，更新数据库的倒排索引

   // 为每一个word添加该KeyFrame
   for(DBoW2::BowVector::const_iterator vit= pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit!=vend; vit++)
       mvInvertedFile[vit->first].push_back(pKF);

void KeyFrameDatabase::erase(KeyFrame* pKF)
关键帧删除后，更新数据库的倒排索引
一个关键帧包含多个word，遍历mvInvertedFile中的这些words，然后在word中删除该KeyFrame

list<KeyFrame*> &lKFs = mvInvertedFile[vit->first];
        // 这个效率有点低啊
        for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
        {
            if(pKF==*lit)
            {
                lKFs.erase(lit);
                break;
            }
        }

void KeyFrameDatabase::clear() 清空关键帧数据库

vector<KeyFrame> KeyFrameDatabase::DetectLoopCandidates(KeyFrame pKF, float minScore)在闭环检测中找到与该关键帧可能闭环的关键帧
minScore是相似性分数最低要求

找出和当前帧具有公共单词的所有关键帧（不包括与当前帧链接的关键帧）
遍历当前帧中每一个word，然后每一个word提取包含该word的关键帧，遍历关键帧，每个关键帧的和当前帧相同word个数加1。

    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(!spConnectedKeyFrames.count(pKFi))// 与pKF局部链接的关键帧不进入闭环候选帧
                {
                    pKFi->mnLoopQuery=pKF->mnId;// pKFi标记为pKF的候选帧，之后直接跳过判断
                    lKFsSharingWords.push_back(pKFi);//用于保存可能与pKF形成回环的候选帧
                }
            }
            pKFi->mnLoopWords++;// 记录pKFi与pKF具有相同word的个数
        }

统计所有闭环候选帧中与pKF具有共同单词最多的单词数
遍历闭环候选帧,然后根据最多单词数计算出minCommonWords

int maxCommonWords=0;
for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
{
    if((*lit)->mnLoopWords>maxCommonWords)
        maxCommonWords=(*lit)->mnLoopWords;
}
int minCommonWords = maxCommonWords*0.8f;

遍历所有闭环候选帧，挑选出共有单词数大于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++;// 这个变量后面没有用到
        // 相似度评分就是在这里计算的
        float si = mpVoc->score(pKF->mBowVec,pKFi->mBowVec);
        pKFi->mLoopScore = si;
        if(si>=minScore)
            lScoreAndMatch.push_back(make_pair(si,pKFi));
    }
}

把lScoreAndMatch中每一个KeyFrame都把与自己共视程度较高的帧归为一组，每一组会计算组得分并记录该组分数最高的KeyFrame，记录于lAccScoreAndMatch

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);
    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;
        if(pKF2->mnLoopQuery==pKF->mnId && pKF2->mnLoopWords>minCommonWords)
        {
            accScore+=pKF2->mLoopScore;// 因为pKF2->mnLoopQuery==pKF->mnId，所以只有pKF2也在闭环候选帧中，才能贡献分数
            if(pKF2->mLoopScore>bestScore)// 统计得到组里分数最高的KeyFrame
            {
                pBestKF=pKF2;
                bestScore = pKF2->mLoopScore;
            }
        }
    }
    lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));
    if(accScore>bestAccScore)// 记录所有组中组得分最高的组
        bestAccScore=accScore;
}

得到组得分大于minScoreToRetain的组，得到组中分数最高的关键帧

vector<KeyFrame*> KeyFrameDatabase::DetectRelocalizationCandidates(Frame *F) 进行回环检测的时候基本上行都是相同的

ORB-SLAM2代码笔记（七）：KeyFrameDatabas

KeyFrameDatabase

猜你喜欢

热点阅读