/函数的三要素是:函数返回值类型,函数名称,函数参数
函数的返回值是装有关键帧指针的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最大值 maxCommonWordsif((*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//遍历所有上一贯留下的关键帧pKFifor(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;//而最佳帧选也初始化为pKFiKeyFrame* 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以及累计得分accScorelAccScoreAndMatch.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;
}