Table of Contents
在上一篇笔记 ORB-SLAM2代码阅读笔记（七）：LocalMapping线程 中，可以看到LocalMapping线程中会将关键帧插入到LoopClosing线程的列表中，对应代码语句如下：
//关键帧插入到LoopClosing线程当中的关键帧队列里，LoopClosing线程执行操作
mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);闭环检测是标准SLAM中必须有的一步，主要是检查相机当前所在的位置是否和相机之前的位置有重合。ORB-SLAM2中LoopClosing线程就是进行闭环检测的，ORB-SLAM2中主要利用词袋模型来进行检测的，这个可以参考高博《视觉SLAM十四讲》中的第十二讲进行了解。这一篇笔记来捋一捋LoopClosing线程中都做了哪些事情。
1.LoopClosing线程工作内容
还是先从ORB-SLAM2论文中给的这个框架图看起。从这个图中可以看到，LoopClosing线程接收来自LocalMapping线程的关键帧，然后进行了Loop Detection（回环检测）和Loop Correction（回环校正）操作。概括来说，LoopClosing线程就是根据关键帧检查是否存在回环的情况，如果存在则进行校正。
2.LoopClosing线程代码流程分析
LoopClosing线程的执行函数为Run函数，在系统初始化过程中该线程就已经开始执行了。Run函数代码如下：
/**
* LoopClosing线程的执行函数
* 在KeyFrameDataBase中查找与mlpLoopKeyFrameQueue中新加入的关键帧相似的闭环候选帧
* 主要步骤：
* 1.闭环检测，获得闭环候选帧；
* 2.计算sim3,根据sim3的计算值更新地图点的位姿；
* 3.进行地图点融合和位姿优化；
*/
void LoopClosing::Run()
{
mbFinished =false;
while(1)
{
// Check if there are keyframes in the queue
if(CheckNewKeyFrames())
{
// Detect loop candidates and check covisibility consistency
//检测闭环。在共视关系的关键帧中找到与当前关键帧Bow匹配最低得分minScore，在除去当前帧共视关系的关键帧数据库中，检测闭环候选帧
if(DetectLoop())
{
// Compute similarity transformation [sR|t] 计算相似变换
// In the stereo/RGBD case s=1
//计算旋转平移的相似性，也就是相似变换
if(ComputeSim3())
{
// Perform loop fusion and pose graph optimization
// 进行闭环校正，执行闭环融合和位姿图优化
CorrectLoop();
}
}
}
ResetIfRequested();
if(CheckFinish())
break;
usleep(5000);
}
SetFinish();
}1）闭环检测
/**
* 检测闭环
*/
bool LoopClosing::DetectLoop()
{
//1.从队列中取出头部的关键帧
{
unique_lock<mutex> lock(mMutexLoopQueue);
mpCurrentKF = mlpLoopKeyFrameQueue.front();
//删除mlpLoopKeyFrameQueue中第一个元素
mlpLoopKeyFrameQueue.pop_front();
// Avoid that a keyframe can be erased while it is being process by this thread
mpCurrentKF->SetNotErase();
}
//If the map contains less than 10 KF or less than 10 KF have passed from last loop detection
//2.判断距离上次闭环检测是否超过10帧。如果不超过10帧，则将当前帧添加到mpKeyFrameDB中的列表中；超过10帧才进行后边的处理
if(mpCurrentKF->mnId < mLastLoopKFid+10)
{
mpKeyFrameDB->add(mpCurrentKF);
mpCurrentKF->SetErase();
return false;
}
// Compute reference BoW similarity score
// This is the lowest score to a connected keyframe in the covisibility graph
// We will impose loop candidates to have a higher similarity than this
//3.计算当前帧及其共视关键帧的词袋模型匹配得分，获得minScore
const vector<KeyFrame*> vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames();
//获取当前关键帧的词袋向量表
const DBoW2::BowVector &CurrentBowVec = mpCurrentKF->mBowVec;
float minScore = 1;
//遍历和当前关键帧共视的关键帧列表，计算当前帧与和它共视的所有帧之间的Bow最低分值
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);
//minScore中存放的是当前关键帧和其共视关系的关键帧之间单词向量比较的最小分值
if(score < minScore)
minScore = score;
}
// Query the database imposing the minimum score
// 4.检测当前关键帧的闭环候选帧
vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);
// If there are no loop candidates, just add new keyframe and return false
// 没有闭环候选帧
if(vpCandidateKFs.empty())
{
mpKeyFrameDB->add(mpCurrentKF);
mvConsistentGroups.clear();
mpCurrentKF->SetErase();
return false;
}
// 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
//对每一个闭环候选帧和之前的闭环候选帧进行连续性性检查
//每个候选帧扩展出了一个共视组(在共视图中关键帧和闭环候选帧相连接)
//如果一个组和前一个组至少共享一个关键帧，则该组与前一个组是连续的。
//我们必须在几个连续的关键帧中检测到一个连续的循环才能接受它
mvpEnoughConsistentCandidates.clear();
vector<ConsistentGroup> vCurrentConsistentGroups;
vector<bool> vbConsistentGroup(mvConsistentGroups.size(),false);
//5.遍历闭环候选帧，从中选出真正存在闭环的关键帧，这里主要是做连续性检测
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++)
{
//判断sit这个关键帧在sPreviousGroup中出现过，则认为具有连续性
if(sPreviousGroup.count(*sit))
{
bConsistent=true;
bConsistentForSomeGroup=true;
break;
}
}
if(bConsistent)
{
int nPreviousConsistency = mvConsistentGroups[iG].second;
int nCurrentConsistency = nPreviousConsistency + 1;
if(!vbConsistentGroup[iG])
{
//将spCandidateGroup和其连续个数作为一个pair存入vCurrentConsistentGroups中
ConsistentGroup cg = make_pair(spCandidateGroup,nCurrentConsistency);
vCurrentConsistentGroups.push_back(cg);
vbConsistentGroup[iG] = true; //this avoid to include the same group more than once
}
//mnCovisibilityConsistencyTh=3
if(nCurrentConsistency >= mnCovisibilityConsistencyTh && !bEnoughConsistent)
{
//mvpEnoughConsistentCandidates中存储候选关键帧
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
//如果这个组和任何前边的组都没有连续性关系，则将连续性计数设置为0
if(!bConsistentForSomeGroup)
{
ConsistentGroup cg = make_pair(spCandidateGroup,0);
vCurrentConsistentGroups.push_back(cg);
}
}
// Update Covisibility Consistent Groups
mvConsistentGroups = vCurrentConsistentGroups;
// Add Current Keyframe to database
mpKeyFrameDB->add(mpCurrentKF);
if(mvpEnoughConsistentCandidates.empty())
{
mpCurrentKF->SetErase();
return false;
}
else
{
//当前候选帧和之前的帧之间存在闭环关系
return true;
}
mpCurrentKF->SetErase();
return false;
}
/**
* 计算当前关键帧和闭环候选帧之间的Sim3,这个Sim3变换就是闭环前累计的尺度和位姿误差
* 该误差也可以帮助检验该闭环在空间几何姿态上是否成立
*/
bool LoopClosing::ComputeSim3()
{
// For each consistent loop candidate we try to compute a Sim3
//对每一个连续的闭环候选帧，我们都尝试计算sim3
//获得候选关键帧的个数
const int nInitialCandidates = mvpEnoughConsistentCandidates.size();
// We compute first ORB matches for each candidate
// If enough matches are found, we setup a Sim3Solver
// 我们为每一个候选帧计算第一个ORB匹配项
// 如果足够的匹配项发现了，我们就启动Sim3Solver
ORBmatcher matcher(0.75,true);
vector<Sim3Solver*> vpSim3Solvers;
vpSim3Solvers.resize(nInitialCandidates);
vector<vector<MapPoint*> > vvpMapPointMatches;
vvpMapPointMatches.resize(nInitialCandidates);
vector<bool> vbDiscarded;
vbDiscarded.resize(nInitialCandidates);
int nCandidates=0; //candidates with enough matches
//1.遍历候选关键帧，对每一个候选关键帧和当前关键帧之间匹配的特征点进行sim3求解
for(int i=0; i<nInitialCandidates; i++)
{
KeyFrame* pKF = mvpEnoughConsistentCandidates[i];
// avoid that local mapping erase it while it is being processed in this thread
pKF->SetNotErase();
if(pKF->isBad())
{
vbDiscarded[i] = true;
continue;
}
/**
* 这里主要是通过SearchByBow搜索当前关键帧中和闭环候选帧匹配的地图点
* BoW通过将单词聚类到树结构node的方法，这样可以加快搜索匹配速度
* vvpMapPointMatches[i]用于存储当前关键帧和候选关键帧之间匹配的地图点
*/
int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);
/**
* 若nmatches<20，剔除该候选帧
* 注意这里使用Bow匹配较快，但是会有漏匹配
*/
if(nmatches<20)
{
vbDiscarded[i] = true;
continue;
}
else
{
//构建Sim3求解器
Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i],mbFixScale);
pSolver->SetRansacParameters(0.99,20,300);
vpSim3Solvers[i] = pSolver;
}
nCandidates++;
}
bool bMatch = false;
// Perform alternatively RANSAC iterations for each candidate
// until one is succesful or all fail
/**
* RANSAC：利用上面匹配上的地图点（虽然匹配上了，但是空间位置相差了一个Sim3），用RANSAC方法求解Sim3位姿
* 这里有可能求解不出Sim3,也就是虽然匹配满足，但是空间几何姿态不满足vvpMapPointMatches
*/
while(nCandidates>0 && !bMatch)
{
for(int i=0; i<nInitialCandidates; i++)
{
if(vbDiscarded[i])
continue;
KeyFrame* pKF = mvpEnoughConsistentCandidates[i];
// Perform 5 Ransac Iterations
vector<bool> vbInliers;
int nInliers;
bool bNoMore;
Sim3Solver* pSolver = vpSim3Solvers[i];
//用RANSAC方法求解SIM3，一共迭代五次，可以提高优化结果准确性
cv::Mat Scm = pSolver->iterate(5,bNoMore,vbInliers,nInliers);
// If Ransac reachs max. iterations discard keyframe
if(bNoMore)
{
vbDiscarded[i]=true;
nCandidates--;
}
// If RANSAC returns a Sim3, perform a guided matching and optimize with all correspondences
if(!Scm.empty())
{
vector<MapPoint*> vpMapPointMatches(vvpMapPointMatches[i].size(), static_cast<MapPoint*>(NULL));
for(size_t j=0, jend=vbInliers.size(); j<jend; j++)
{
if(vbInliers[j])
vpMapPointMatches[j]=vvpMapPointMatches[i][j];
}
//根据计算出的Sim3(s, R, t)，去找更多的匹配点(SearchBySim3),更新vpMapPointMatches
cv::Mat R = pSolver->GetEstimatedRotation();
cv::Mat t = pSolver->GetEstimatedTranslation();
const float s = pSolver->GetEstimatedScale();
//使用sim3求出来的s,R,t通过SearchBySim3得到更多匹配
matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5);
/**
* 使用更新过的匹配，使用g2o优化Sim3位姿，这是内点数nInliers>20,才说明通过。
* 一旦找到闭环帧mpMatchedKF,则break,跳过对其他候选帧的判断
*/
g2o::Sim3 gScm(Converter::toMatrix3d(R),Converter::toVector3d(t),s);
const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);
// If optimization is succesful stop ransacs and continue
//如果当前帧和回环帧之间存在超过20个匹配点，则认为这个当前帧和回环帧的sim3变换是有效的
if(nInliers>=20)
{
bMatch = true;
mpMatchedKF = pKF;
g2o::Sim3 gSmw(Converter::toMatrix3d(pKF->GetRotation()),Converter::toVector3d(pKF->GetTranslation()),1.0);
mg2oScw = gScm*gSmw;
mScw = Converter::toCvMat(mg2oScw);
mvpCurrentMatchedPoints = vpMapPointMatches;
break;
}
}
}
}
if(!bMatch)
{
for(int i=0; i<nInitialCandidates; i++)
mvpEnoughConsistentCandidates[i]->SetErase();
mpCurrentKF->SetErase();
return false;
}
// Retrieve MapPoints seen in Loop Keyframe and neighbors
//将MatchedKF共视帧取出
vector<KeyFrame*> vpLoopConnectedKFs = mpMatchedKF->GetVectorCovisibleKeyFrames();
//匹配的关键帧mpMatchedKF加入到和它共视的关键帧列表中
vpLoopConnectedKFs.push_back(mpMatchedKF);
mvpLoopMapPoints.clear();
for(vector<KeyFrame*>::iterator vit=vpLoopConnectedKFs.begin(); vit!=vpLoopConnectedKFs.end(); vit++)
{
KeyFrame* pKF = *vit;
vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();
for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++)
{
MapPoint* pMP = vpMapPoints[i];
if(pMP)
{
if(!pMP->isBad() && pMP->mnLoopPointForKF != mpCurrentKF->mnId)
{
mvpLoopMapPoints.push_back(pMP);
pMP->mnLoopPointForKF=mpCurrentKF->mnId;
}
}
}
}
// Find more matches projecting with the computed Sim3
/**
* 获取mpMatchedKF及其相连关键帧对应的地图的地图点。将这些地图点通过上面优化得到的Sim3(gScm>mScw)
* 变换投影到当前关键帧进行匹配，若匹配点>=40个，则返回true,进行闭环调整，否则，返回false,
* 线程暂停5ms后继续接收Tracking发送来的关键帧队列
* 注意这里得到的当前关键帧中匹配上闭环关键帧共视地图点(mvpCurrentMatchedPoints)
* 将用于后面CorrectLoop时当时关键帧地图点的冲突融合
* 到这里，不仅确保了当前关键帧与闭环帧之间匹配度高，
* 而且保证了闭环帧的共视图中的地图点和当前帧的特征点匹配度更高,说明该闭环帧是正确的
*/
//SearchByProjection得到更多匹配点，mvpCurrentMatchedPoints
matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);
// If enough matches accept Loop
int nTotalMatches = 0;
for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++)
{
if(mvpCurrentMatchedPoints[i])
nTotalMatches++;
}
// 如果最终的匹配点超过40个，则认为当前帧和闭环帧确实存在闭环关系
if(nTotalMatches>=40)
{
for(int i=0; i<nInitialCandidates; i++)
if(mvpEnoughConsistentCandidates[i]!=mpMatchedKF)
mvpEnoughConsistentCandidates[i]->SetErase();
return true;
}
else
{
for(int i=0; i<nInitialCandidates; i++)
mvpEnoughConsistentCandidates[i]->SetErase();
mpCurrentKF->SetErase();
return false;
}
}其中调用DetectLoopCandidates函数代码如下：
/**
* 检测得到回环候选帧
* pKF 当前关键帧
* minScore 与当前帧共视的所有关键帧中Bow的最小分值
*/
vector<KeyFrame*> KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF, float minScore)
{
//spConnectedKeyFrames是一个set定义的变量，set中装的是在covisibility graph中与关键帧pKF相关联的关键帧。
// 1.获取和当前关键帧有共视关系的关键帧
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);
//遍历当前帧的BowVector，找出与当前帧有公共单词的关键帧,但不包括与当前帧相连的关键帧
//2.遍历当前关键帧的词向量，通过词向量查看这些词都在哪些关键帧中出现过
for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit != vend; vit++)
{
//在InvertedFile中查找该word都在哪些关键帧中出现过
list<KeyFrame*> &lKFs = mvInvertedFile[vit->first];
//遍历和当前帧共享相同单词的关键帧列表
for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
{
/**
* 下面一段程序是计算pKFi和pKF共视的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;
//判断pKFi没有在spConnectedKeyFrames容器中出现过，则将pKFi加入到共享词的帧列表中
//spConnectedKeyFrames.count(pKFi)是查询pKFi在spConnectedKeyFrames中出现的次数，返回值为0或者1
//进入以下分支，表示pKFi和pKF不是相连的关键帧
if(!spConnectedKeyFrames.count(pKFi))
{
pKFi->mnLoopQuery=pKF->mnId;
lKFsSharingWords.push_back(pKFi);
}
}
pKFi->mnLoopWords++;
}
}
}
//此时，lKFsSharingWords中存储的是与当前帧有公共单词，但是不与当前帧相连的关键帧
if(lKFsSharingWords.empty())
return vector<KeyFrame*>();
list<pair<float,KeyFrame*> > lScoreAndMatch;
// Only compare against those keyframes that share enough words
// 3.统计候选帧中与pKF具有共同单词最多的单词数
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;
// Compute similarity score. Retain the matches whose score is higher than minScore
//4. 筛选共有单词大于minCommonWords且Bow得分大于minscore的关键帧，连同分值存放在lScoreAndMatch中
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));
}
}
//此时，lScoreAndMatch中存放的是和当前帧共有单词大于minCommonWords且Bow得分大于minscore的关键帧
if(lScoreAndMatch.empty())
return vector<KeyFrame*>();
list<pair<float,KeyFrame*> > lAccScoreAndMatch;
float bestAccScore = minScore;
// Lets now accumulate score by covisibility
/**
* 5.遍历lScoreAndMatch列表，对其中每个关键帧计算其和共视关系最好的10个关键帧中的最高得分bestScore和他们的累计分值
* lAccScoreAndMatch中存储累计分值和最高分值的关键帧
*/
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;
}
}
}
//pBestKF为得分最高的关键帧
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());
// 6.遍历每组中得分最高的关键帧，对累计分值大于最好累计分值的75%的关键帧视为闭环候选关键帧，并加入到闭环候选关键帧列表中
for(list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)
{
//得分大于阈值
if(it->first > minScoreToRetain)
{
KeyFrame* pKFi = it->second;
//该帧pKFi没有加入过spAlreadyAddedKF
if(!spAlreadyAddedKF.count(pKFi))
{
vpLoopCandidates.push_back(pKFi);
spAlreadyAddedKF.insert(pKFi);
}
}
}
return vpLoopCandidates;
}
2）计算sim3
/**
* 计算当前关键帧和闭环候选帧之间的Sim3,这个Sim3变换就是闭环前累计的尺度和位姿误差
* 该误差也可以帮助检验该闭环在空间几何姿态上是否成立
*/
bool LoopClosing::ComputeSim3()
{
// For each consistent loop candidate we try to compute a Sim3
//对每一个连续的闭环候选帧，我们都尝试计算sim3
//获得候选关键帧的个数
const int nInitialCandidates = mvpEnoughConsistentCandidates.size();
// We compute first ORB matches for each candidate
// If enough matches are found, we setup a Sim3Solver
// 我们为每一个候选帧计算第一个ORB匹配项
// 如果足够的匹配项发现了，我们就启动Sim3Solver
ORBmatcher matcher(0.75,true);
vector<Sim3Solver*> vpSim3Solvers;
vpSim3Solvers.resize(nInitialCandidates);
vector<vector<MapPoint*> > vvpMapPointMatches;
vvpMapPointMatches.resize(nInitialCandidates);
vector<bool> vbDiscarded;
vbDiscarded.resize(nInitialCandidates);
int nCandidates=0; //candidates with enough matches
//1.遍历候选关键帧，对每一个候选关键帧和当前关键帧之间匹配的特征点进行sim3求解
for(int i=0; i<nInitialCandidates; i++)
{
KeyFrame* pKF = mvpEnoughConsistentCandidates[i];
// avoid that local mapping erase it while it is being processed in this thread
pKF->SetNotErase();
if(pKF->isBad())
{
vbDiscarded[i] = true;
continue;
}
/**
* 这里主要是通过SearchByBow搜索当前关键帧中和闭环候选帧匹配的地图点
* BoW通过将单词聚类到树结构node的方法，这样可以加快搜索匹配速度
* vvpMapPointMatches[i]用于存储当前关键帧和候选关键帧之间匹配的地图点
*/
int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);
/**
* 若nmatches<20，剔除该候选帧
* 注意这里使用Bow匹配较快，但是会有漏匹配
*/
if(nmatches<20)
{
vbDiscarded[i] = true;
continue;
}
else
{
//构建Sim3求解器
Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i],mbFixScale);
pSolver->SetRansacParameters(0.99,20,300);
vpSim3Solvers[i] = pSolver;
}
nCandidates++;
}
bool bMatch = false;
// Perform alternatively RANSAC iterations for each candidate
// until one is succesful or all fail
/**
* RANSAC：利用上面匹配上的地图点（虽然匹配上了，但是空间位置相差了一个Sim3），用RANSAC方法求解Sim3位姿
* 这里有可能求解不出Sim3,也就是虽然匹配满足，但是空间几何姿态不满足vvpMapPointMatches
*/
while(nCandidates>0 && !bMatch)
{
for(int i=0; i<nInitialCandidates; i++)
{
if(vbDiscarded[i])
continue;
KeyFrame* pKF = mvpEnoughConsistentCandidates[i];
// Perform 5 Ransac Iterations
vector<bool> vbInliers;
int nInliers;
bool bNoMore;
Sim3Solver* pSolver = vpSim3Solvers[i];
//用RANSAC方法求解SIM3，一共迭代五次，可以提高优化结果准确性
cv::Mat Scm = pSolver->iterate(5,bNoMore,vbInliers,nInliers);
// If Ransac reachs max. iterations discard keyframe
if(bNoMore)
{
vbDiscarded[i]=true;
nCandidates--;
}
// If RANSAC returns a Sim3, perform a guided matching and optimize with all correspondences
if(!Scm.empty())
{
vector<MapPoint*> vpMapPointMatches(vvpMapPointMatches[i].size(), static_cast<MapPoint*>(NULL));
for(size_t j=0, jend=vbInliers.size(); j<jend; j++)
{
if(vbInliers[j])
vpMapPointMatches[j]=vvpMapPointMatches[i][j];
}
//根据计算出的Sim3(s, R, t)，去找更多的匹配点(SearchBySim3),更新vpMapPointMatches
cv::Mat R = pSolver->GetEstimatedRotation();
cv::Mat t = pSolver->GetEstimatedTranslation();
const float s = pSolver->GetEstimatedScale();
//使用sim3求出来的s,R,t通过SearchBySim3得到更多匹配
matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5);
/**
* 使用更新过的匹配，使用g2o优化Sim3位姿，这是内点数nInliers>20,才说明通过。
* 一旦找到闭环帧mpMatchedKF,则break,跳过对其他候选帧的判断
*/
g2o::Sim3 gScm(Converter::toMatrix3d(R),Converter::toVector3d(t),s);
const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);
// If optimization is succesful stop ransacs and continue
//如果当前帧和回环帧之间存在超过20个匹配点，则认为这个当前帧和回环帧的sim3变换是有效的
if(nInliers>=20)
{
bMatch = true;
mpMatchedKF = pKF;
g2o::Sim3 gSmw(Converter::toMatrix3d(pKF->GetRotation()),Converter::toVector3d(pKF->GetTranslation()),1.0);
mg2oScw = gScm*gSmw;
mScw = Converter::toCvMat(mg2oScw);
mvpCurrentMatchedPoints = vpMapPointMatches;
break;
}
}
}
}
if(!bMatch)
{
for(int i=0; i<nInitialCandidates; i++)
mvpEnoughConsistentCandidates[i]->SetErase();
mpCurrentKF->SetErase();
return false;
}
// Retrieve MapPoints seen in Loop Keyframe and neighbors
//将MatchedKF共视帧取出
vector<KeyFrame*> vpLoopConnectedKFs = mpMatchedKF->GetVectorCovisibleKeyFrames();
//匹配的关键帧mpMatchedKF加入到和它共视的关键帧列表中
vpLoopConnectedKFs.push_back(mpMatchedKF);
mvpLoopMapPoints.clear();
for(vector<KeyFrame*>::iterator vit=vpLoopConnectedKFs.begin(); vit!=vpLoopConnectedKFs.end(); vit++)
{
KeyFrame* pKF = *vit;
vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();
for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++)
{
MapPoint* pMP = vpMapPoints[i];
if(pMP)
{
if(!pMP->isBad() && pMP->mnLoopPointForKF != mpCurrentKF->mnId)
{
mvpLoopMapPoints.push_back(pMP);
pMP->mnLoopPointForKF=mpCurrentKF->mnId;
}
}
}
}
// Find more matches projecting with the computed Sim3
/**
* 获取mpMatchedKF及其相连关键帧对应的地图的地图点。将这些地图点通过上面优化得到的Sim3(gScm>mScw)
* 变换投影到当前关键帧进行匹配，若匹配点>=40个，则返回true,进行闭环调整，否则，返回false,
* 线程暂停5ms后继续接收Tracking发送来的关键帧队列
* 注意这里得到的当前关键帧中匹配上闭环关键帧共视地图点(mvpCurrentMatchedPoints)
* 将用于后面CorrectLoop时当时关键帧地图点的冲突融合
* 到这里，不仅确保了当前关键帧与闭环帧之间匹配度高，
* 而且保证了闭环帧的共视图中的地图点和当前帧的特征点匹配度更高,说明该闭环帧是正确的
*/
//SearchByProjection得到更多匹配点，mvpCurrentMatchedPoints
matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);
// If enough matches accept Loop
int nTotalMatches = 0;
for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++)
{
if(mvpCurrentMatchedPoints[i])
nTotalMatches++;
}
// 如果最终的匹配点超过40个，则认为当前帧和闭环帧确实存在闭环关系
if(nTotalMatches>=40)
{
for(int i=0; i<nInitialCandidates; i++)
if(mvpEnoughConsistentCandidates[i]!=mpMatchedKF)
mvpEnoughConsistentCandidates[i]->SetErase();
return true;
}
else
{
for(int i=0; i<nInitialCandidates; i++)
mvpEnoughConsistentCandidates[i]->SetErase();
mpCurrentKF->SetErase();
return false;
}
}3）闭环校正
void LoopClosing::CorrectLoop()
{
cout << "Loop detected!" << endl;
// Send a stop signal to Local Mapping
// Avoid new keyframes are inserted while correcting the loop
//1.暂停LocalMapping，防止在闭环校正过程中插入新的关键帧
mpLocalMapper->RequestStop();
// If a Global Bundle Adjustment is running, abort it
//2.暂停全局BA优化
if(isRunningGBA())
{
unique_lock<mutex> lock(mMutexGBA);
mbStopGBA = true;
mnFullBAIdx++;
if(mpThreadGBA)
{
mpThreadGBA->detach();
delete mpThreadGBA;
}
}
// Wait until Local Mapping has effectively stopped
while(!mpLocalMapper->isStopped())
{
usleep(1000);
}
// Ensure current keyframe is updated
//3.更新当前帧的共视相连关系
mpCurrentKF->UpdateConnections();
// Retrive keyframes connected to the current keyframe and compute corrected Sim3 pose by propagation
//4.得到与当前帧相连关键帧(包括当前关键帧)
mvpCurrentConnectedKFs = mpCurrentKF->GetVectorCovisibleKeyFrames();
mvpCurrentConnectedKFs.push_back(mpCurrentKF);
/**
* 使用计算出的Sim3对当前位姿进行调整，并且传播到当前帧相连的的关键帧
* (相连关键帧之间相对位姿是知道的，通过当前帧的Sim3计算相连关键帧的Sim3).
* 这样回环的两侧关键帧就对齐了，利用调整过的位姿更新这些向量关键帧对应的地图点
*/
KeyFrameAndPose CorrectedSim3, NonCorrectedSim3;
CorrectedSim3[mpCurrentKF]=mg2oScw;
cv::Mat Twc = mpCurrentKF->GetPoseInverse();
{
// Get Map Mutex
unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
//5.计算当前关键帧有共视关系的关键帧的Sim3位姿
for(vector<KeyFrame*>::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++)
{
KeyFrame* pKFi = *vit;
cv::Mat Tiw = pKFi->GetPose();
if(pKFi!=mpCurrentKF)
{
cv::Mat Tic = Tiw*Twc;
cv::Mat Ric = Tic.rowRange(0,3).colRange(0,3);
cv::Mat tic = Tic.rowRange(0,3).col(3);
g2o::Sim3 g2oSic(Converter::toMatrix3d(Ric),Converter::toVector3d(tic),1.0);
//得到sim3更新后的位姿
g2o::Sim3 g2oCorrectedSiw = g2oSic*mg2oScw;
//Pose corrected with the Sim3 of the loop closure
CorrectedSim3[pKFi]=g2oCorrectedSiw;
}
cv::Mat Riw = Tiw.rowRange(0,3).colRange(0,3);
cv::Mat tiw = Tiw.rowRange(0,3).col(3);
g2o::Sim3 g2oSiw(Converter::toMatrix3d(Riw),Converter::toVector3d(tiw),1.0);
//Pose without correction
NonCorrectedSim3[pKFi]=g2oSiw;
}
// Correct all MapPoints obsrved by current keyframe and neighbors, so that they align with the other side of the loop
//6.利用调整过的位姿更新这些相连关键帧对应的MapPoint
for(KeyFrameAndPose::iterator mit=CorrectedSim3.begin(), mend=CorrectedSim3.end(); mit!=mend; mit++)
{
KeyFrame* pKFi = mit->first;
g2o::Sim3 g2oCorrectedSiw = mit->second;
g2o::Sim3 g2oCorrectedSwi = g2oCorrectedSiw.inverse();
g2o::Sim3 g2oSiw =NonCorrectedSim3[pKFi];
vector<MapPoint*> vpMPsi = pKFi->GetMapPointMatches();
for(size_t iMP=0, endMPi = vpMPsi.size(); iMP<endMPi; iMP++)
{
MapPoint* pMPi = vpMPsi[iMP];
if(!pMPi)
continue;
if(pMPi->isBad())
continue;
if(pMPi->mnCorrectedByKF==mpCurrentKF->mnId)
continue;
// Project with non-corrected pose and project back with corrected pose
//将闭环帧及其相连帧的地图点都投影到当前帧以及相连帧上
cv::Mat P3Dw = pMPi->GetWorldPos();
Eigen::Matrix<double,3,1> eigP3Dw = Converter::toVector3d(P3Dw);
Eigen::Matrix<double,3,1> eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oSiw.map(eigP3Dw));
cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);
pMPi->SetWorldPos(cvCorrectedP3Dw);
pMPi->mnCorrectedByKF = mpCurrentKF->mnId;
pMPi->mnCorrectedReference = pKFi->mnId;
pMPi->UpdateNormalAndDepth();
}
// Update keyframe pose with corrected Sim3. First transform Sim3 to SE3 (scale translation)
//将Sim3转为SE3,并调整关键帧位姿
Eigen::Matrix3d eigR = g2oCorrectedSiw.rotation().toRotationMatrix();
Eigen::Vector3d eigt = g2oCorrectedSiw.translation();
double s = g2oCorrectedSiw.scale();
eigt *=(1./s); //[R t/s;0 1]
cv::Mat correctedTiw = Converter::toCvSE3(eigR,eigt);
//调整关键帧位姿
pKFi->SetPose(correctedTiw);
// Make sure connections are updated
//更新关键帧连接关系
pKFi->UpdateConnections();
}
// Start Loop Fusion
// Update matched map points and replace if duplicated
//7.开始进行闭环融合。投影匹配上的和Sim3计算过的地图点进行融合(就是替换成高质量的)
for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++)
{
if(mvpCurrentMatchedPoints[i])
{
MapPoint* pLoopMP = mvpCurrentMatchedPoints[i];
MapPoint* pCurMP = mpCurrentKF->GetMapPoint(i);
if(pCurMP)
pCurMP->Replace(pLoopMP);//进行地图点替换
else
{
mpCurrentKF->AddMapPoint(pLoopMP,i);
pLoopMP->AddObservation(mpCurrentKF,i);
pLoopMP->ComputeDistinctiveDescriptors();
}
}
}
}
// Project MapPoints observed in the neighborhood of the loop keyframe
// into the current keyframe and neighbors using corrected poses.
// Fuse duplications.
//8.将这些已纠正位姿的MapPoints与闭环MapPoints融合
SearchAndFuse(CorrectedSim3);
// After the MapPoint fusion, new links in the covisibility graph will appear attaching both sides of the loop
//9.得到由闭环形成的连接关系，存储在LoopConnections中
map<KeyFrame*, set<KeyFrame*> > LoopConnections;
for(vector<KeyFrame*>::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++)
{
KeyFrame* pKFi = *vit;
vector<KeyFrame*> vpPreviousNeighbors = pKFi->GetVectorCovisibleKeyFrames();
// Update connections. Detect new links.
//更新关键帧连接关系
pKFi->UpdateConnections();
LoopConnections[pKFi]=pKFi->GetConnectedKeyFrames();
for(vector<KeyFrame*>::iterator vit_prev=vpPreviousNeighbors.begin(), vend_prev=vpPreviousNeighbors.end(); vit_prev!=vend_prev; vit_prev++)
{
LoopConnections[pKFi].erase(*vit_prev);
}
for(vector<KeyFrame*>::iterator vit2=mvpCurrentConnectedKFs.begin(), vend2=mvpCurrentConnectedKFs.end(); vit2!=vend2; vit2++)
{
LoopConnections[pKFi].erase(*vit2);
}
}
// Optimize graph
//10.Essential Graph优化
Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);
mpMap->InformNewBigChange();
// Add loop edge
mpMatchedKF->AddLoopEdge(mpCurrentKF);
mpCurrentKF->AddLoopEdge(mpMatchedKF);
// Launch a new thread to perform Global Bundle Adjustment
// 11.启动一个新线程去进行全局BA优化
mbRunningGBA = true;
mbFinishedGBA = false;
mbStopGBA = false;
//新建一个线程进行全局BA优化，对于地图中地图点和关键帧位姿更新都是在这里进行的
mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment,this,mpCurrentKF->mnId);
// Loop closed. Release Local Mapping.
mpLocalMapper->Release();
//12.更新最后一个闭环关键帧id
mLastLoopKFid = mpCurrentKF->mnId;
}3.小结
LoopClosing线程代码结构图如下：
这个线程中，有些内容还需要进一步梳理，比如sim3究竟是怎么回事？关键帧之间的连接关系、全局优化等，都需要仔细深挖来理解。
为了快速理解代码流程，阅读代码中参考了许多别人的文章，感谢他们整理的文档。有些别人的注释我直接拿过来放在了代码注释中，可以帮助自己和他人更好的理解代码。主要参考相关文档如下：
https://www.cnblogs.com/shang-slam/p/6444037.html
https://blog.csdn.net/Felaim/article/details/79446854
https://blog.csdn.net/chishuideyu/article/details/76165461
扫一扫
2311
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
