之前研究过一些回环检测的内容，首先要看的自然是用词袋回环的鼻祖和正当继承人(没有冒犯VINS和LDSO的意思）ORB-SLAM。下面是我的代码注释。因为代码都是自己手打的，不是在源码上注释的，所以一些我觉得不是太重要的被略过了，可能也会有一些typo.
ORB的回环策略比较偏向seq-SLAM的思路，通过共视帧打包的关系，比较每个包的相似值，而非只是关注单帧和单帧的匹配，这个思路是比较合适的，但是VINS和LDSO两位后来者用实际行动证明了我不太看中你这种思路，两个都没有用。后续我会介绍一些VINS和LDSO里的回环方法。
System Constructor
// System constructor
System::System()
{
mpVocabulary = new ORBVocabulary(); // -- create keyframe database
mpVocabulary->LoadFromTextFile(strVocFile); // load file
mpLoopCloser = new LoopClosing(...);
mptLoopClosing = new thread(&ORB_SLAM2:LoopClosing::Run, mploopCloser); // a thread
// -- mpTracker, mpLocalMapper, mpLoopCloser have to know each other's pointer
...
}
ComputeBow
在系统中的多个地方会计算Frame或者Keyframe的Bow，具体如下：
void Frame::ComputeBow()
{
if(mBowVec.empty())
{ // -- mDescriptors: cv::Mat
vector<cv::Mat> vCurrentDesc = Converter::toDescriptorVector(mDescriptors);
mpORBVocabulary->transform(vCurrentDesc, mBowVec, mFeatVec, 4);
}
}
// -- LocalMapping::ProcessNewKeyFrame() 调用
// -- Tracking::CreatInitialMapMonocular() 调用，开始两个关键帧
// -- Tracking::TrackReferenceKeyFrame()
// -- Tracking::Relocalization()
void KeyFrame::ComputeBow()
{
if(mBowVec.empty() || mFeatVec.empty())
{
同上；
}
}
LoopClosing
void LoopClosing::Run()
{
whlie(1)
{
if(CheckNewKeyFrames()) // -- lock and check LoopKeyFrameQueue
{
if(DetectLoop())
{
if(ComputeSim3())
{
CorrectLoop();
}
}
}
}
}
bool LoopClosing::DetectLoop()
{
{ // -- 得到当前帧
unique_lock<mutex> lock(...); // -- lock
mpCurrentKF = mlpLoopKeyFrameQueue.front();
mlpLoopKeyFrameQueue.pop_front();
mpCurrentKF->SetNotErase(); // -- 暂时不销毁
}
if(mpCurrentKF->mnId<mLastLoopKFid + 10) // -- 如果上次发生回环在10kf以内，回false
{
mpKeyFrameDB->add(mpCurrentKF); //加入到关键帧数据集里面
mpCurrentKF->SetErase(); //销毁咯
return false;
}
// -- 计算BoW相似分
// -- 当前帧和当前帧的共视帧
// -- 如果分很低更新minScore
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(socre < minScore)
{
minScore = score;
}
}
// -- 用currentKF和minScore找回环候选帧
vector<KeyFrame*> vpCanndidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);
// -- 如果没有候选帧，回家吃饭
if(vpCandidateKFs.empty())
{
mpKeyFrameDB->add(mpCurrentKF);
mvConsistentGroup.clear();
mpCurrentKF->SetErase();
return false;
}
//TO BE CONTINUED
...
}
KeyframeDatabase
初始化
KeyFrameDatabae::KeyFrameDatabase (const ORBVocabulary &voc) : mpVoc(&voc)
{
mvInvertedFile.resize(voc.size());
}
mvInvertedFile是一个按单次ID顺序存储的vector，每一个空间存储一个KeyFrame*的list。
add函数存储关键帧指针到每一个ID的对应list里面。
void KeyFrameDatabase::add(KeyFrame *pKF)
{
unique_lock<mutex> lock(mMutex);
for(auto vit = pKF->mBowVec.begin(), vend; ;vit++ )
{
mvInvertedFile[vit->first].push_back(pKF); // -- std::vector<list<KeyFrame*>>
}
}
void KeyFrameDatabase::erase(KeyFrame* pKF)
{
unique_lock<mutex> lock(mMutex);
for(auto vit = pKF->mBoWVec.begin(), vend;; vit++)
{
list<KeyFrame*> &lKFs = mvInvertedFile[vit->first]; // -- 获取存有该ID单词的list
for(auto lit=lKFs.begin(),lend;;lit++)
{
找到pKF然后删除这个迭代器;
}
}
}
看如果用这个数据集来寻找回环候选帧
vector<KeyFrame*> KeyFrameDatabase::detectLoopCandidate(KeyFrame* pKF, float minScore)
{
set<KeyFrame*> spConnectedKeyframes = pKF->GetConnectedKeyFrames();
list<KeyFrame*> lKFsSharingWords;//找到有相同单次的帧加入list
// -- 找到所有和当前帧有相同word的帧
{
unique_lock<mutex> lock<mMutex>;
for(auto vit=pkF->mBowVec, vend; vit != vend; vit++) //遍历当前帧的words
{
list<KeyFrame*> &lKFs = mvInvertedFile[vit->first]; // 找到对应的list
for(auto lit = lKFs.begin()..; ;lit++) //遍历list
{
KeyFrame* pKFi = *lit;
if(pKFi->mnLoopQuery!=pKF->mnId)//如果这个关键帧的**最近询问**不是当前帧-111
{
pKFi->mnLoopWords = 0;// 第一次找到单词
if(!spConnectedKeyFrames.count(pKFi))//查询不是当前帧的共视帧
{
pKFi->mnLoopQuery = pKF->mnId;//和-111遥相呼应，不重复操作
lKFSSharingWords.push_back(pKFi);//加入set
}
}
pKFi->mnLoopWords++;//记录一共共有的单词个数
}
}
}
if(lKFsSharingWords.empty()) // -- 没有任何帧有相同的单词，GG
{
return vector<KeyFrame*>();
}
list<pair<float, KeyFrame*>> lScoreAndMatch;
int maxCommonWords = 0;
{
//遍历找到共享单词最多的更新给maxCommonWords;
}
int minCommonWords = maxCommonWords * .8f; // 至少是最多共视的80%
int nscore = 0;
//轮询lKFsSharingWords
//计算相似得分，保留得分比minScore高的
for(auto lit = lKFsSharingWords.begin(), 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)); //记录得分和大于minScore的回环帧
}
}
}
if(lScoreAndMatch.empty()) // -- 没有得分够的，GG
{
return vector<KeyFrame*>();
}
list<pair<float, KeyFrame*>> lAccScoreAndMatch;
float bestAccScore = minScore;
//翻云覆雨，用共视估计得分
for(auto it = lScoreAndMatch.begin(), itend; ; it++) //遍历lScoreAndMatch
{
KeyFrame* pKFi = it->second;
vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10); // -- 找10个共视最多的帧 这里Neigh应该是想说Neighbor
float bestScore = it->first;
float accScore = it->first; // -- accumulate累计的分数
KeyFrame* pBestKF = pKFi;
for(auto vit = vpNeights.begin(), vend; ;vit++)
{
KeyFrame* pKF2 = *vit;
if(pKF2->mnLoopQuery == pKF->mnId && pKF2->mnLoopWords > minCommonWords) //共视的帧也和当前帧有很强的BoW的反应
{
accScore += pKF2->mLoopScore;
if(pkF2->mLoopScore > bestScore)
{
pBestKF = pKF2; //更新最强回环帧
bestScore = pKF2->mLoopScore;
}
}
}
lAccScoreAndMatch.push_back(make_pair(accScore, pBestKF)); //把之前算的每一帧的，以最近10个共视帧的分时累计分数，并选出10帧中个分数最高的做pointer
if(accScore > bestAccScore) // -- 更新累计最高分
{
bestAccScore = accScore;
}
}
float minScoreToRetain = 0.75f * bestAccSCore;
set<KeyFrame*> spAlreadyAddedKF;
vector<KeyFrame*> vpLoopCandidates;
vpLoopCandidates.reserve(lAccScoreAndMatch.size());
for(auto it = lAccScoreAndMatch.begin...;;) //遍历lAccScoreAndMatch
{
if(it->first > minScoreToRetain)
{
KeyFrame* pKFi = it->second;
if(!spAlreadyAddedKF.count(pKFi)) // -- 滤除一样的帧
{
vpLoopCandiates.push_back(pKFi);
spAlreadyAddedKF.insert(pkFi);
}
}
}
return vpLoopCandidates;
}