2021SC@SDUSC

1.关键帧数据库简介

构建关键帧数据库，可以联系链表等常用数据结构的构建过程：创建、增加元素、删除元素、清理。

首先需要明确数据存储的数据类型：以关键帧作为数据库的元素。

这个地方需要理解两个概念：单词（词袋）和关键帧。
单词（词袋）：预先构建好的，离线词典（ORBvoc.txt）：它是DBoW2作者使用orb特征，使用大量图片训练的结果。

2.代码分析

在闭环检测中找到与该关键帧可能闭环的关键帧（注意不和当前帧连接）

• Step 1：找出和当前帧具有公共单词的所有关键帧，不包括与当前帧连接（也就是共视）的关键帧
• Step 2：只和具有共同单词较多的（最大数目的80%以上）关键帧进行相似度计算
• Step 3：计算上述候选帧对应的共视关键帧组的总得分，只取最高组得分75%以上的组
• Step 4：得到上述组中分数最高的关键帧作为闭环候选关键帧

vector<KeyFrame*> KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF, float minScore)
{
    // 取出与当前关键帧相连（>15个共视地图点）的所有关键帧，这些相连关键帧都是局部相连，在闭环检测的时候将被剔除
    // 相连关键帧定义见 KeyFrame::UpdateConnections()
    set<KeyFrame*> spConnectedKeyFrames = pKF->GetConnectedKeyFrames();
    // 用于保存可能与当前关键帧形成闭环的候选帧（只要有相同的word，且不属于局部相连（共视）帧）
    list<KeyFrame*> lKFsSharingWords;

    // Search all keyframes that share a word with current keyframes
    // Discard keyframes connected to the query keyframe
    // Step 1：找出和当前帧具有公共单词的所有关键帧，不包括与当前帧连接（也就是共视）的关键帧
    {
        unique_lock<mutex> lock(mMutex);

        // words是检测图像是否匹配的枢纽，遍历该pKF的每一个word
        // mBowVec 内部实际存储的是std::map<WordId, WordValue>
        // WordId 和 WordValue 表示Word在叶子中的id 和权重
        for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit != vend; vit++)
        {
            // 提取所有包含该word的KeyFrame
            list<KeyFrame*> &lKFs =   mvInvertedFile[vit->first];
            // 然后对这些关键帧展开遍历
            for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
            {
                KeyFrame* pKFi=*lit;
                if(pKFi->GetMap()==pKF->GetMap()) // For consider a loop candidate it a candidate it must be in the same map
                {
                    if(pKFi->mnLoopQuery!=pKF->mnId)
                    {
                    	// 还没有标记为pKF的闭环候选帧
                        pKFi->mnLoopWords=0;
                    	// 和当前关键帧共视的话不作为闭环候选帧
                        if(!spConnectedKeyFrames.count(pKFi))
                        {
                        	// 没有共视就标记作为闭环候选关键帧，放到lKFsSharingWords里
                            pKFi->mnLoopQuery=pKF->mnId;
                            lKFsSharingWords.push_back(pKFi);
                        }
                    }
                	pKFi->mnLoopWords++;// 记录pKFi与pKF具有相同word的个数
                }


            }
        }
    }
    // 如果没有关键帧和这个关键帧具有相同的单词,那么就返回空
    if(lKFsSharingWords.empty())
        return vector<KeyFrame*>();

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

    // Only compare against those keyframes that share enough words
    // Step 2：统计上述所有闭环候选帧中与当前帧具有共同单词最多的单词数，用来决定相对阈值 
    int maxCommonWords=0;
    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
    {
        if((*lit)->mnLoopWords>maxCommonWords)
            maxCommonWords=(*lit)->mnLoopWords;
    }

    // 确定最小公共单词数为最大公共单词数目的0.8倍
    int minCommonWords = maxCommonWords*0.8f;

    int nscores=0;

    // Compute similarity score. Retain the matches whose score is higher than minScore
    // Step 3：遍历上述所有闭环候选帧，挑选出共有单词数大于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;
    float bestAccScore = minScore;

    // Lets now accumulate score by covisibility
    // 单单计算当前帧和某一关键帧的相似性是不够的，这里将与关键帧相连（权值最高，共视程度最高）的前十个关键帧归为一组，计算累计得分
    // Step 4：计算上述候选帧对应的共视关键帧组的总得分，得到最高组得分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);

        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)
                {
                    pBestKF=pKF2;
                    bestScore = pKF2->mLoopScore;
                }
            }
        }

        lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));
        // 记录所有组中组得分最高的组，用于确定相对阈值
        if(accScore>bestAccScore)
            bestAccScore=accScore;
    }

    // Return all those keyframes with a score higher than 0.75*bestScore
    // 所有组中最高得分的0.75倍，作为最低阈值
    float minScoreToRetain = 0.75f*bestAccScore;

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

    // Step 5：只取组得分大于阈值的组，得到组中分数最高的关键帧作为闭环候选关键帧
    for(list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)
    {
        if(it->first>minScoreToRetain)
        {
            KeyFrame* pKFi = it->second;
            // spAlreadyAddedKF 是为了防止重复添加
            if(!spAlreadyAddedKF.count(pKFi))
            {
                vpLoopCandidates.push_back(pKFi);
                spAlreadyAddedKF.insert(pKFi);
            }
        }
    }


    return vpLoopCandidates;
}

找到N个融合候选N个回环候选

void KeyFrameDatabase::DetectNBestCandidates(KeyFrame *pKF, vector<KeyFrame*> &vpLoopCand, vector<KeyFrame*> &vpMergeCand, int nNumCandidates)
{
    // Step 1统计与当前关键帧有相同单词的关键帧
    list<KeyFrame*> lKFsSharingWords;
    //set<KeyFrame*> spInsertedKFsSharing;
    // 当前关键帧的共视关键帧(避免将当前关键帧的共视关键帧加入回环检测)
    set<KeyFrame*> spConnectedKF;

    // Search all keyframes that share a word with current frame
    {
        unique_lock<mutex> lock(mMutex);
        // 拿到当前关键帧的共视关键帧
        spConnectedKF = pKF->GetConnectedKeyFrames();

        // 遍历当前关键帧bow向量的每个单词
        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(spConnectedKF.find(pKFi) != spConnectedKF.end())
                {
                    continue;
                }*/
                // 如果此关键帧没有被当前关键帧访问过(防止重复添加)
                if(pKFi->mnPlaceRecognitionQuery!=pKF->mnId)
                {   
                    // 初始化公共单词数为0
                    pKFi->mnPlaceRecognitionWords=0;
                    // 如果该关键帧不是当前关键帧的共视关键帧
                    if(!spConnectedKF.count(pKFi))
                    {
                        // 标记该关键帧被当前关键帧访问到（也就是有公共单词）
                        pKFi->mnPlaceRecognitionQuery=pKF->mnId;
                        // 把当前关键帧添加到有公共单词的关键帧列表中
                        lKFsSharingWords.push_back(pKFi);
                    }
                }
                // 递增该关键帧与当前关键帧的公共单词数
                pKFi->mnPlaceRecognitionWords++;
                /*if(spInsertedKFsSharing.find(pKFi) == spInsertedKFsSharing.end())
                {
                    lKFsSharingWords.push_back(pKFi);
                    spInsertedKFsSharing.insert(pKFi);
                }*/

            }
        }
    }
    // 如果没有有公共单词的关键帧,直接返回
    if(lKFsSharingWords.empty())
        return;

    // Only compare against those keyframes that share enough words
    // Step 2 统计所有候选帧中与当前关键帧的公共单词数最多的单词数maxCommonWords,并筛选
    int maxCommonWords=0;
    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
    {
        if((*lit)->mnPlaceRecognitionWords>maxCommonWords)
            maxCommonWords=(*lit)->mnPlaceRecognitionWords;
    }
    // 取0.8倍为阀值
    int minCommonWords = maxCommonWords*0.8f;
    // 这里的pair是 <相似度,候选帧的指针> : 记录所有大于minCommonWords的候选帧与当前关键帧的相似度
    list<pair<float,KeyFrame*> > lScoreAndMatch;
    // 只是个统计变量,貌似没有用到
    int nscores=0;

    // Compute similarity score.
    // 对所有大于minCommonWords的候选帧计算相似度


    // 遍历所有有公共单词的候选帧
    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
    {
        KeyFrame* pKFi = *lit;
        // 如果当前帧的公共单词数大于minCommonWords
        if(pKFi->mnPlaceRecognitionWords>minCommonWords)
        {
            nscores++;  //未使用
            // 计算相似度
            float si = mpVoc->score(pKF->mBowVec,pKFi->mBowVec);
            // 记录该候选帧与当前帧的相似度
            pKFi->mPlaceRecognitionScore=si;
            // 记录到容器里, 每个元素是<相似度,候选帧的指针>
            lScoreAndMatch.push_back(make_pair(si,pKFi));
        }
    }
    // 如果为空,直接返回,表示没有符合上述条件的关键帧
    if(lScoreAndMatch.empty())
        return;
    // Step 3 : 用小组得分排序得到top3总分里最高分的关键帧,作为候选帧
    // 统计以组为单位的累计相似度和组内相似度最高的关键帧, 每个pair为<小组总相似度,组内相似度最高的关键帧指针>
    list<pair<float,KeyFrame*> > lAccScoreAndMatch;
    float bestAccScore = 0;

    // Lets now accumulate score by covisibility
    // 变量所有被lScoreAndMatch记录的pair <相似度,候选关键帧>
    for(list<pair<float,KeyFrame*> >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++)
    {
        // 候选关键帧
        KeyFrame* pKFi = it->second;
        // 与候选关键帧共视关系最好的10个关键帧
        vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);
        // 初始化最大相似度为该候选关键帧自己的相似度
        float bestScore = it->first;
        // 初始化小组累计得分为改候选关键帧自己的相似度
        float accScore = bestScore;
        // 初始化组内相似度最高的帧为该候选关键帧本身
        KeyFrame* pBestKF = pKFi;
        // 遍历与当前关键帧共视关系最好的10帧
        for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)
        {
            KeyFrame* pKF2 = *vit;
            // 如果该关键帧没有被当前关键帧访问过(也就是没有公共单词)则跳过
            if(pKF2->mnPlaceRecognitionQuery!=pKF->mnId)
                continue;
            // 累加小组总分
            accScore+=pKF2->mPlaceRecognitionScore;
            // 如果大于组内最高分,则更新当前最高分记录
            if(pKF2->mPlaceRecognitionScore>bestScore)
            {
                pBestKF=pKF2;
                bestScore = pKF2->mPlaceRecognitionScore;
            }

        }
        // 统计以组为单位的累计相似度和组内相似度最高的关键帧, 每个pair为<小组总相似度,组内相似度最高的关键帧指针>
        lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));
        // 统计最高得分, 这个bestAccSocre没有用到
        if(accScore>bestAccScore)
            bestAccScore=accScore;
    }

    //cout << "Amount of candidates: " << lAccScoreAndMatch.size() << endl;
    // 按相似度从大到小排序
    lAccScoreAndMatch.sort(compFirst);
    // 最后返回的变量, 记录回环的候选帧
    vpLoopCand.reserve(nNumCandidates);
    // 最后返回的变量, 记录融合候选帧
    vpMergeCand.reserve(nNumCandidates);
    // 避免重复添加
    set<KeyFrame*> spAlreadyAddedKF;
    //cout << "Candidates in score order " << endl;
    //for(list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)
    int i = 0;
    list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin();
    // 遍历lAccScoreAndMatch中所有的pair, 每个pair为<小组总相似度,组内相似度最高的关键帧指针>，nNumCandidates默认为3
    while(i < lAccScoreAndMatch.size() && (vpLoopCand.size() < nNumCandidates || vpMergeCand.size() < nNumCandidates))
    {
        //cout << "Accum score: " << it->first << endl;
        // 拿到候选关键帧的指针
        KeyFrame* pKFi = it->second;
        if(pKFi->isBad())
            continue;

        // 如果没有被重复添加
        if(!spAlreadyAddedKF.count(pKFi))
        {   
            // 如果候选帧与当前关键帧在同一个地图里,且候选者数量还不足够
            if(pKF->GetMap() == pKFi->GetMap() && vpLoopCand.size() < nNumCandidates)
            {
                // 添加到回环候选帧里
                vpLoopCand.push_back(pKFi);
            }
            // 如果候选者与当前关键帧不再同一个地图里, 且候选者数量还不足够, 且候选者所在地图不是bad
            else if(pKF->GetMap() != pKFi->GetMap() && vpMergeCand.size() < nNumCandidates && !pKFi->GetMap()->IsBad())
            {   
                // 添加到融合候选帧里
                vpMergeCand.push_back(pKFi);
            }
            // 防止重复添加
            spAlreadyAddedKF.insert(pKFi);
        }
        i++;
        it++;
    }
    //-------

    // Return all those keyframes with a score higher than 0.75*bestScore
    /*float minScoreToRetain = 0.75f*bestAccScore;
    set<KeyFrame*> spAlreadyAddedKF;
    vpLoopCand.reserve(lAccScoreAndMatch.size());
    vpMergeCand.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))
            {
                if(pKF->GetMap() == pKFi->GetMap())
                {
                    vpLoopCand.push_back(pKFi);
                }
                else
                {
                    vpMergeCand.push_back(pKFi);
                }
                spAlreadyAddedKF.insert(pKFi);
            }
        }
    }*/
}

参考文献

从ORB-SLAM2的源码工程中学习C++编程——关键帧数据库