ORB-SLAM2学习笔记——闭环检测DetectLoop
1、理论部分
2、代码详解
#include "KeyFrameDatabase.h"
#include "KeyFrame.h"
#include "Thirdparty/DBoW2/DBoW2/BowVector.h"
#include<mutex>
using namespace std;
namespace ORB_SLAM2
{
KeyFrameDatabase::KeyFrameDatabase (const ORBVocabulary &voc):
mpVoc(&voc)
{
mvInvertedFile.resize(voc.size());
}
void KeyFrameDatabase::add(KeyFrame *pKF)
{
unique_lock<mutex> lock(mMutex);
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)
{
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++)
{
if(pKF==*lit)
{
lKFs.erase(lit);
break;
}
}
}
}
void KeyFrameDatabase::clear()
{
mvInvertedFile.clear();
mvInvertedFile.resize(mpVoc->size());
}
vector<KeyFrame*> KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF, float minScore)
{
set<KeyFrame*> spConnectedKeyFrames = pKF->GetConnectedKeyFrames();
list<KeyFrame*> lKFsSharingWords;
{
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++;
}
}
}
if(lKFsSharingWords.empty())
return vector<KeyFrame*>();
list<pair<float,KeyFrame*> > lScoreAndMatch;
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;
int nscores=0;
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));
}
}
list<pair<float,KeyFrame*> > lAccScoreAndMatch;
float bestAccScore = minScore;
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;
}
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;
}
vector<KeyFrame*> KeyFrameDatabase::DetectRelocalizationCandidates(Frame *F)
{
list<KeyFrame*> lKFsSharingWords;
{
unique_lock<mutex> lock(mMutex);
for(DBoW2::BowVector::const_iterator vit=F->mBowVec.begin(), vend=F->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->mnRelocQuery!=F->mnId)
{
pKFi->mnRelocWords=0;
pKFi->mnRelocQuery=F->mnId;
lKFsSharingWords.push_back(pKFi);
}
pKFi->mnRelocWords++;
}
}
}
if(lKFsSharingWords.empty())
return vector<KeyFrame*>();
int maxCommonWords=0;
for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
{
if((*lit)->mnRelocWords>maxCommonWords)
maxCommonWords=(*lit)->mnRelocWords;
}
int minCommonWords = maxCommonWords*0.8f;
list<pair<float,KeyFrame*> > lScoreAndMatch;
int nscores=0;
for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
{
KeyFrame* pKFi = *lit;
if(pKFi->mnRelocWords>minCommonWords)
{
nscores++;
float si = mpVoc->score(F->mBowVec,pKFi->mBowVec);
pKFi->mRelocScore=si;
lScoreAndMatch.push_back(make_pair(si,pKFi));
}
}
if(lScoreAndMatch.empty())
return vector<KeyFrame*>();
list<pair<float,KeyFrame*> > lAccScoreAndMatch;
float bestAccScore = 0;
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 = bestScore;
KeyFrame* pBestKF = pKFi;
for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)
{
KeyFrame* pKF2 = *vit;
if(pKF2->mnRelocQuery!=F->mnId)
continue;
accScore+=pKF2->mRelocScore;
if(pKF2->mRelocScore>bestScore)
{
pBestKF=pKF2;
bestScore = pKF2->mRelocScore;
}
}
lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));
if(accScore>bestAccScore)
bestAccScore=accScore;
}
float minScoreToRetain = 0.75f*bestAccScore;
set<KeyFrame*> spAlreadyAddedKF;
vector<KeyFrame*> vpRelocCandidates;
vpRelocCandidates.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))
{
vpRelocCandidates.push_back(pKFi);
spAlreadyAddedKF.insert(pKFi);
}
}
}
return vpRelocCandidates;
}
}