算法流程
bool LoopClosing::DetectLoop() 函数解析
- 从队列中取最早进来的一个关键帧，作为当前检测闭环关键帧
mpCurrentKF = mlpLoopKeyFrameQueue.front();
mlpLoopKeyFrameQueue是通过下面的函数插入的
void LocalMapping::InsertKeyFrame(KeyFrame *pKF)
- 如果距离上一次闭环小于10帧，则不进行闭环检测
- 遍历当前回环关键帧 mpCurrentKF 的所有相连关键帧（>15个共视地图点），计算当前关键帧与每个共视关键帧的bow相似得分，并得到最低得分minScore
- minScore作用：用于判断其它所有关键帧与当前回环关键帧是否为闭环候选关键帧
原因：与当前帧具有回环关系的关键帧，不应该低于当前关键帧的相邻关键帧的最低相似度minScore
- 在所有关键帧中找出闭环候选关键帧，所有关键帧是指与当前回环关键帧不相连的关键帧，
vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);
- 待加入DetectLoopCandidates函数解析
-
在候选帧中检测具有连续性的候选帧
5.1 遍历刚刚得到的候选帧5.2 将自己以及与候选帧自己相连的关键帧构成一个“子候选组”，每个候选组都构成一个子候选组
5.3 遍历前一次闭环检测到的连续组链，每次遍历取出一个子连续组中的关键帧集合用set容器装的，子连续组是连续组链中的其中一个组
5.4 在5.3的遍历中所有“子候选组”中的关键帧，判断子连续组中是否有与子候选组相同的关键帧 ，由于用set容器装的子连续组，所以.count()就能判断了
5.5 在5.3的遍历中，与5.4同级，判断子候选组与子连续组****有连续性后，将子连续组的 .second 连续长度取出来+1，+1是因为原本连续组链的子连续组假设至于一个组连接，现在又与现在的子候选组连接，则连续长度为2，将这个连续长度为2放到子候选组的 .second 中 ，将子候选组与2组成一个pair放入vCurrentConsistentGroups（本次闭环检测的连续组）中
notic：之所以能将“2”当作子候选组的.second是因为这个遍变量反映的是组的连续长度，假设连续组链为A–B–C–D,则.second = 3
5.6 将vCurrentConsistentGroups（本次闭环检测的连续组）更新到mvConsistentGroups，则下一次循环时就是5.3的连续组链了
个人学习总结，有错误的地方欢迎指出一起讨论
闭环检测代码注释
/**
* @brief 闭环检测
*
* @return true 成功检测到闭环
* @return false 未检测到闭环
*/
bool LoopClosing::DetectLoop()
{
{
// Step 1 从队列中取出一个关键帧,作为当前检测闭环关键帧
unique_lock<mutex> lock(mMutexLoopQueue);
// 从队列头开始取，也就是先取早进来的关键帧
mpCurrentKF = mlpLoopKeyFrameQueue.front();
// 取出关键帧后从队列里弹出该关键帧
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
// Step 2：如果距离上次闭环没多久（小于10帧），或者map中关键帧总共还没有10帧，则不进行闭环检测
// 后者的体现是当mLastLoopKFid为0的时候
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
// Step 3：遍历当前回环关键帧所有连接（>15个共视地图点）关键帧，计算当前关键帧与每个共视关键的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(score<minScore)
minScore = score;
}
// Query the database imposing the minimum score
// Step 4：在所有关键帧中找出闭环候选帧（注意不和当前帧连接）
// minScore的作用：认为和当前关键帧具有回环关系的关键帧,不应该低于当前关键帧的相邻关键帧的最低的相似度minScore
// 得到的这些关键帧,和当前关键帧具有较多的公共单词,并且相似度评分都挺高
vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);
// If there are no loop candidates, just add new keyframe and return false
// 如果没有闭环候选帧，返回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
// Step 5：在候选帧中检测具有连续性的候选帧
// 1、每个候选帧将与自己相连的关键帧构成一个“子候选组spCandidateGroup”， vpCandidateKFs-->spCandidateGroup
// 2、检测“子候选组”中每一个关键帧是否存在于“连续组”，如果存在 nCurrentConsistency++，则将该“子候选组”放入“当前连续组vCurrentConsistentGroups”
// 3、如果nCurrentConsistency大于等于3，那么该”子候选组“代表的候选帧过关，进入mvpEnoughConsistentCandidates
// 相关的概念说明:（为方便理解，见视频里的图示）
// 组(group): 对于某个关键帧, 其和其具有共视关系的关键帧组成了一个"组";
// 子候选组(CandidateGroup): 对于某个候选的回环关键帧, 其和其具有共视关系的关键帧组成的一个"组";
// 连续(Consistent): 不同的组之间如果共同拥有一个及以上的关键帧,那么称这两个组之间具有连续关系
// 连续性(Consistency):称之为连续长度可能更合适,表示累计的连续的链的长度:A--B 为1, A--B--C--D 为3等;具体反映在数据类型 ConsistentGroup.second上
// 连续组(Consistent group): mvConsistentGroups存储了上次执行回环检测时, 新的被检测出来的具有连续性的多个组的集合.由于组之间的连续关系是个网状结构,因此可能存在
// 一个组因为和不同的连续组链都具有连续关系,而被添加两次的情况(当然连续性度量是不相同的)
// 连续组链:自造的称呼,类似于菊花链A--B--C--D这样形成了一条连续组链.对于这个例子中,由于可能E,F都和D有连续关系,因此连续组链会产生分叉;为了简化计算,连续组中将只会保存
// 最后形成连续关系的连续组们(见下面的连续组的更新)
// 子连续组: 上面的连续组中的一个组
// 连续组的初始值: 在遍历某个候选帧的过程中,如果该子候选组没有能够和任何一个上次的子连续组产生连续关系,那么就将添加自己组为连续组,并且连续性为0(相当于新开了一个连续链)
// 连续组的更新: 当前次回环检测过程中,所有被检测到和之前的连续组链有连续的关系的组,都将在对应的连续组链后面+1,这些子候选组(可能有重复,见上)都将会成为新的连续组;
// 换而言之连续组mvConsistentGroups中只保存连续组链中末尾的组
// 最终筛选后得到的闭环帧
mvpEnoughConsistentCandidates.clear();
// ConsistentGroup数据类型为pair<set<KeyFrame*>,int>
// ConsistentGroup.first对应每个“连续组”中的关键帧，ConsistentGroup.second为每个“连续组”的已连续几个的序号
vector<ConsistentGroup> vCurrentConsistentGroups;
// 这个下标是每个"子连续组"的下标,bool表示当前的候选组中是否有和该组相同的一个关键帧
vector<bool> vbConsistentGroup(mvConsistentGroups.size(),false);
// Step 5.1：遍历刚才得到的每一个候选关键帧
for(size_t i=0, iend=vpCandidateKFs.size(); i<iend; i++)
{
KeyFrame* pCandidateKF = vpCandidateKFs[i];
// Step 5.2：将自己以及与自己相连的关键帧构成一个“子候选组”
set<KeyFrame*> spCandidateGroup = pCandidateKF->GetConnectedKeyFrames();
// 把自己也加进去
spCandidateGroup.insert(pCandidateKF);
// 连续性达标的标志
bool bEnoughConsistent = false;
bool bConsistentForSomeGroup = false;
// Step 5.3：遍历前一次闭环检测到的连续组链
// 上一次闭环的连续组链 std::vector<ConsistentGroup> mvConsistentGroups
// 其中ConsistentGroup的定义：typedef pair<set<KeyFrame*>,int> ConsistentGroup
// 其中 ConsistentGroup.first对应每个“连续组”中的关键帧集合，ConsistentGroup.second为每个“连续组”的连续长度
for(size_t iG=0, iendG=mvConsistentGroups.size(); iG<iendG; iG++)
{
// 取出之前的一个子连续组中的关键帧集合
set<KeyFrame*> sPreviousGroup = mvConsistentGroups[iG].first;
// Step 5.4：遍历每个“子候选组”，检测子候选组中每一个关键帧在“子连续组”中是否存在
// 如果有一帧共同存在于“子候选组”与之前的“子连续组”，那么“子候选组”与该“子连续组”连续
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)
{
// Step 5.5：如果判定为连续，接下来判断是否达到连续的条件
// 取出和当前的候选组发生"连续"关系的子连续组的"已连续次数"
int nPreviousConsistency = mvConsistentGroups[iG].second;
// 将当前候选组连续长度在原子连续组的基础上 +1，
int nCurrentConsistency = nPreviousConsistency + 1;
// 如果上述连续关系还未记录到 vCurrentConsistentGroups，那么记录一下
// 注意这里spCandidateGroup 可能放置在vbConsistentGroup中其他索引(iG)下
if(!vbConsistentGroup[iG])
{
// 将该“子候选组”的该关键帧打上连续编号加入到“当前连续组”
ConsistentGroup cg = make_pair(spCandidateGroup,nCurrentConsistency);
// 放入本次闭环检测的连续组vCurrentConsistentGroups里
vCurrentConsistentGroups.push_back(cg);
//this avoid to include the same group more than once
// 标记一下，防止重复添加到同一个索引iG
// 但是spCandidateGroup可能重复添加到不同的索引iG对应的vbConsistentGroup 中
vbConsistentGroup[iG]=true;
}
// 如果连续长度满足要求，那么当前的这个候选关键帧是足够靠谱的
// 连续性阈值 mnCovisibilityConsistencyTh=3
// 足够连续的标记 bEnoughConsistent
if(nCurrentConsistency>=mnCovisibilityConsistencyTh && !bEnoughConsistent)
{
// 记录为达到连续条件了
mvpEnoughConsistentCandidates.push_back(pCandidateKF);
//this avoid to insert the same candidate more than once
// 标记一下，防止重复添加
bEnoughConsistent=true;
// ? 这里可以break掉结束当前for循环吗？
// 回答：不行。因为虽然pCandidateKF达到了连续性要求
// 但spCandidateGroup 还可以和mvConsistentGroups 中其他的子连续组进行连接
}
}
}
// If the group is not consistent with any previous group insert with consistency counter set to zero
// Step 5.6：如果该“子候选组”的所有关键帧都和上次闭环无关（不连续），vCurrentConsistentGroups 没有新添加连续关系
// 于是就把“子候选组”全部拷贝到 vCurrentConsistentGroups， 用于更新mvConsistentGroups，连续性计数器设为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())
{
// 未检测到闭环，返回false
mpCurrentKF->SetErase();
return false;
}
else
{
// 成功检测到闭环，返回true
return true;
}
// 多余的代码,执行不到
mpCurrentKF->SetErase();
return false;
}
3472
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
