这里分两次分析SLamLAM2dui对于回环检测的实现，第一部分介绍回环帧的选择，第二部分介绍使用选择的回环帧对当前正的优化。
第一部分 介绍回环帧的选择：
回环检测的代码在LoopClosing，通过独立线程run运行
while(1)
{
// Check if there are keyframes in the queue
if(CheckNewKeyFrames())
{
// Detect loop candidates and check covisibility consistency
if(DetectLoop())
{
// Compute similarity transformation [sR|t]
// In the stereo/RGBD case s=1
if(ComputeSim3())
{
// Perform loop fusion and pose graph optimization
CorrectLoop();
}
}
}
第一步，检查当前有没有新的keyframe，有就进入DetectLoop。
接下来我们所要说的重点就在DetectLoop这个函数里。
函数的一开始检擦回环检测的条件，
if(mpCurrentKF->mnId<mLastLoopKFid+10) //当前帧的ID < 上一个帧的ID
{
mpKeyFrameDB->add(mpCurrentKF);
mpCurrentKF->SetErase();
return false;
}也就是说如果当前关键帧距离上一帧关键帧之间小于10个帧，说明运动的太短了，没有做回环检测的必要。
如果满足条件，就开始进行检测动作。
遍历所有的loop关键帧（这些帧是在添加keyframe的时候也添加进loop数组里的），获取一个回环关键帧，并计算获取这个帧和所有共视帧之间的最小分数。 共视帧是ORBSLAM使用的一个很有用的数据，用于表示和当前帧能看到大多数一样目标的那些帧，其实简单的理解就是那些在这个关键帧附近的帧，并且这些帧基本是连续的
const vector<KeyFrame*> vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames();
const DBoW2::BowVector &CurrentBowVec = mpCurrentKF->mBowVec;
float minScore = 1;
for(size_t i=0; i<vpConnectedKeyFrames.size(); i++)
{
KeyFrame* pKF = vpConnectedKeyFrames[i];
if(pKF->isBad())
continue;
const DBoW2::BowVector &BowVec = pKF->mBowVec;
float score = mpORBVocabulary->score(CurrentBowVec, BowVec);
if(score<minScore)
minScore = score;
}有了这个最小值以后，就需要通过这个值来过滤挑选出>这个值的帧作为候选回环帧。这部分算法比较复杂，一点点顺着代码看，注意我添加的注释：
vector<KeyFrame*> KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF, float minScore)
{
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);
//遍历当前帧中的所有的单词
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)) //看看有这个单词的帧是不是当前帧的共视帧
{
pKFi->mnLoopQuery=pKF->mnId;
lKFsSharingWords.push_back(pKFi); //是的话，加入队列
}
}
pKFi->mnLoopWords++; //共同看到的单词总数++
}
}
}
//以上处理完，就获得了和当前回环帧有可能相似的帧的列表，权重是LoopWords
if(lKFsSharingWords.empty())
return vector<KeyFrame*>();
list<pair<float,KeyFrame*> > lScoreAndMatch;
// Only compare against those keyframes that share enough words
//统计这些相似帧中最大的权重
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; //设定一个80%的阈值
int nscores=0;
// Compute similarity score. Retain the matches whose score is higher than minScore
//只有满足mnLoopWords > minCommonWords并且和当前关键帧的 bow score > minScore才可以作为候选
for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
{
KeyFrame* pKFi = *lit;
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));
}
}
if(lScoreAndMatch.empty())
return vector<KeyFrame*>();
//接着是又是是又是一步trick，进一步筛选
list<pair<float,KeyFrame*> > lAccScoreAndMatch;
float bestAccScore = minScore;
// Lets now accumulate score by covisibility
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;
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
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++)
{
if(it->first>minScoreToRetain)
{
KeyFrame* pKFi = it->second;
if(!spAlreadyAddedKF.count(pKFi))
{
vpLoopCandidates.push_back(pKFi);
spAlreadyAddedKF.insert(pKFi);
}
}
}
return vpLoopCandidates; //最终的候选帧
}
获取了候选帧之后，就要从候选帧中选出最好的那一帧来对当前的关键帧进行优化调整位姿。这里orbslam设计了一个一致性检查的方法，用来挑选出最合适的那个回环帧。
先看下图，理解下为什么需要做一致性检查：
如果我们从候选的回环帧中挑选出第一个，并且发现这个帧和当前帧的相似度很高，那么我们是否可以认为这个帧就是回环帧呢？答案是这样不一定准确，为什么？因为这个候选回环队列中的帧并不是共视的（或者是相连的），所以有可能，当我们获取出队列中第二个帧的时候，也和当前帧相似，但是这个两个队列中的帧差个10万8千里，这样我们就不知道相信是回环到哪个帧了。
ORBSLAM首先通过从队列中拿出第一个回环帧，然后获取这一帧的连接队列（共视图），将这个队列成为一个组，然后在拿第二帧，同样获取共视图，成为第二组。
关键步骤就是，会通过这两个组来查看，里面是否第二组的帧在第一组的帧里，依次类推，一直查看是否又连续有三组的帧你中有我我中有你，这样就可以判断这个这帧应该就是回环。我觉得ORBSLAM在这里用组的概念来删选2而不是直接判断是不是三个帧之间连续的原因也是在于候选帧里的帧并不是相连的这个原因，用组的概念比较正确虽然损失了一点性能。
// For each loop candidate check consistency with previous loop candidates
// Each candidate expands a covisibility group (keyframes connected to the loop candidate in the covisibility graph)
// A group is consistent with a previous group if they share at least a keyframe
// We must detect a consistent loop in several consecutive keyframes to accept it
//又是ORBSLAM的一个trick,一致性检查
mvpEnoughConsistentCandidates.clear();
vector<ConsistentGroup> vCurrentConsistentGroups;
vector<bool> vbConsistentGroup(mvConsistentGroups.size(),false);
for(size_t i=0, iend=vpCandidateKFs.size(); i<iend; i++)
{
KeyFrame* pCandidateKF = vpCandidateKFs[i];
//获取相连的组
set<KeyFrame*> spCandidateGroup = pCandidateKF->GetConnectedKeyFrames();
spCandidateGroup.insert(pCandidateKF); //把自己也加在里面
bool bEnoughConsistent = false;
bool bConsistentForSomeGroup = false;
for(size_t iG=0, iendG=mvConsistentGroups.size(); iG<iendG; iG++)
{
set<KeyFrame*> sPreviousGroup = mvConsistentGroups[iG].first;
bool bConsistent = false;
for(set<KeyFrame*>::iterator sit=spCandidateGroup.begin(), send=spCandidateGroup.end(); sit!=send;sit++)
{
if(sPreviousGroup.count(*sit))
{
bConsistent=true;
bConsistentForSomeGroup=true;
break;
}
}
if(bConsistent)
{
int nPreviousConsistency = mvConsistentGroups[iG].second;
int nCurrentConsistency = nPreviousConsistency + 1;
if(!vbConsistentGroup[iG])
{
ConsistentGroup cg = make_pair(spCandidateGroup,nCurrentConsistency);
vCurrentConsistentGroups.push_back(cg);
vbConsistentGroup[iG]=true; //this avoid to include the same group more than once
}
//超过三次就不查了，就用第三个帧作为回环帧，提供给后续的优化例程
if(nCurrentConsistency>=mnCovisibilityConsistencyTh && !bEnoughConsistent)
{
mvpEnoughConsistentCandidates.push_back(pCandidateKF);
bEnoughConsistent=true; //this avoid to insert the same candidate more than once
}
}
}
// If the group is not consistent with any previous group insert with consistency counter set to zero
if(!bConsistentForSomeGroup)
{
ConsistentGroup cg = make_pair(spCandidateGroup,0);
vCurrentConsistentGroups.push_back(cg);
}
}之后，就是将获取到的回环帧做sim3优化了。
评论（0）
您还未登录，请登录后发表或查看评论