/函数的三要素是:函数返回值类型,函数名称,函数参数
函数的返回值是装有关键帧指针的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;
}