目录
4.1 从队列mlpLoopKeyFrameQueue中取出一个关键帧作为当前检测闭环关键帧
4.2 如果距离上次闭环没多久（小于10帧），或者map中关键帧总共还没有10帧，则不进行闭环检测
4.3 遍历当前回环关键帧所有连接（>15个共视地图点）关键帧，计算当前关键帧与每个共视关键的bow相似度得分，并得到最低得分minScore
4.5 在候选帧中检测具有连续性的候选帧即进一步筛除候选关键帧
1.函数作用
经过一系列筛选操作筛选出当前帧mpCurrentKF的闭环候选关键帧，并将闭环候选关键帧存储在容器mvpEnoughConsistentCandidates中供其他函数使用，更新连续组关系mvConsistentGroups。
2.函数流程
Step 1 从队列中取出一个关键帧,作为当前检测闭环关键帧
Step 2：如果距离上次闭环没多久（小于10帧），或者map中关键帧总共还没有10帧，则不进行闭环检测
Step 3：遍历当前回环关键帧所有连接（>15个共视地图点）关键帧，计算当前关键帧与每个共视关键的bow相似度得分，并得到最低得分minScore
Step 4：在所有关键帧中找出闭环候选帧（注意不和当前帧连接）
Step 5：在候选帧中检测具有连续性的候选帧
3.code
/** * @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; }
4.函数解析
4.1 从队列mlpLoopKeyFrameQueue中取出一个关键帧作为当前检测闭环关键帧
回环检测线程要处理的关键帧是局部建图线程经过判断冗余关键帧判定传来的：
mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);关键帧存储在mlpLoopKeyFrameQueue这个队列中。
// 将某个关键帧加入到回环检测的过程中,由局部建图线程调用 void LoopClosing::InsertKeyFrame(KeyFrame *pKF) { unique_lock<mutex> lock(mMutexLoopQueue); // 注意：这里第0个关键帧不能够参与到回环检测的过程中,因为第0关键帧定义了整个地图的世界坐标系 if(pKF->mnId!=0) mlpLoopKeyFrameQueue.push_back(pKF); }因此我们进入回环检测的第一件事就是检测队列mlpLoopKeyFrameQueue是否为空，若不为空从中抽取一帧一帧进行闭环检测。
总结一下：Loopclosing线程中的待处理关键帧是LocalMapping线程发送过来的，LocalMapping线程中的待处理关键帧是Tracking线程中发过来的。在LocalMapping线程中通过 InsertKeyFrame 将关键帧插入闭环检测队列mlpLoopKeyFrameQueue中。
/* * 查看列表中是否有等待被插入的关键帧 * @return 如果存在，返回true */ bool LoopClosing::CheckNewKeyFrames() { unique_lock<mutex> lock(mMutexLoopQueue); return(!mlpLoopKeyFrameQueue.empty()); }我们从mlpLoopKeyFrameQueue的头部取一帧进行闭环检测，用变量mpCurrentKF存储此帧（记住这个变量的名字，它将在整个闭环线程多次出现），并设置当前关键帧不要在优化的过程中被删除。
4.2 如果距离上次闭环没多久（小于10帧），或者map中关键帧总共还没有10帧，则不进行闭环检测
先说明几个变量的含义：
mLastLoopKFid：上一次闭环帧的id
mnId：某一帧的id
if(mpCurrentKF->mnId<mLastLoopKFid+10) { mpKeyFrameDB->add(mpCurrentKF); mpCurrentKF->SetErase(); return false; }如果距离上次闭环没多久（小于10帧），或者map中关键帧总共还没有10帧，则不进行闭环检测。
/** * @brief 删除当前的这个关键帧,表示不进行回环检测过程;由回环检测线程调用 * */ void KeyFrame::SetErase() { { unique_lock<mutex> lock(mMutexConnections); // 如果当前关键帧和其他的关键帧没有形成回环关系,那么就删吧 if(mspLoopEdges.empty()) { mbNotErase = false; } } // mbToBeErased：删除之前记录的想要删但时机不合适没有删除的帧 if(mbToBeErased) { SetBadFlag(); } }并删除该关键帧。
4.3 遍历当前回环关键帧所有连接（>15个共视地图点）关键帧，计算当前关键帧与每个共视关键的bow相似度得分，并得到最低得分minScore
用vpConnectedKeyFrames变量存储当前关键帧的共视关键帧。
// 得到与该关键帧连接的关键帧(已按权值排序) vector<KeyFrame*> KeyFrame::GetVectorCovisibleKeyFrames() { unique_lock<mutex> lock(mMutexConnections); return mvpOrderedConnectedKeyFrames; }获得这一关键帧的BoWVec向量存储在CurrentBowVec中。
关于BoW向量请参阅我的这篇博客：
讲解什么是BowVec和FeatVec
https://blog.csdn.net/qq_41694024/article/details/128007040
我们计算当前关键帧mpCurrentKF和其每个共视关键帧vpConnectedKeyFrames的相似度得分，得分越低，相似度越低。
记录最低得分minScore。
minScore的作用：认为和当前关键帧具有回环关系的关键帧，不应该低于当前关键帧的相邻关键帧的最低的相似度minScore。
4.4 在所有关键帧中找出闭环候选帧（注意不和当前帧连接）
调用DetectLoopCandidates函数，在关键帧数据库mpKeyFrameDB中找出闭环候选帧并存储在vpCandidateKFs变量中：
ORB-SLAM2 --- KeyFrameDatabase::DetectLoopCandidates函数解析
如果没有闭环候选帧，返回false。
4.5 在候选帧中检测具有连续性的候选帧即进一步筛除候选关键帧
相关的概念说明:
组(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中只保存连续组链中末尾的组。
解释一下如上概念，右面的红色的当前KF代表当前寻找闭环的候选关键帧mpCurrentKF，两个红圈里面的带红圈的绿色关键帧代表闭环候选关键帧vpCandidateKFs，我们单单得到这两个裹着绿色的红色壳子的帧不能认为其闭环，因为闭环是一个连续的过程，如下图：
它不是仅仅闭环一次，是一个连续的过程，往前继续走会产生连续的闭环，在ORBSLAM中，系统认为连续产生了三次闭环才认为这是一个闭环，因此在这里会检测闭环的连续性。
遍历由4.4得到的每一个候选关键帧vpCandidateKFs：
将每一个候选关键帧和其相邻关键帧加入子候选组spCandidateGroup中，如图中的一个虚线的大圆圈的小圆圈。将此子候选组连续性达成的标志bEnoughConsistent和bConsistentForSomeGroup设置为false。
这里解释一下什么是前一次闭环检测到的子连续组：
在上一次闭环检测线程中，左面的圈是上次闭环检测线程的闭环连续组，存储在变量mvConsistentGroups中，是 std::vector<ConsistentGroup>类型变量，ConsistentGroup又是pair<set<KeyFrame*>,int>类型变量，ConsistentGroup.first对应每个“连续组”中的关键帧集合，即一个个小红色虚线圈中关键帧的集合，ConsistentGroup.second为每个“连续组”的连续长度，即连续1次就是1，连续两次就是2，连续3次就是3。
回归代码，在上文中，我们对在vpCandidateKFs中的闭环候选关键帧计算其子候选组spCandidateGroup，现在我们遍历前一次闭环检测到的所有闭环连续组mvConsistentGroups，取出mvConsistentGroups的一个子连续组中的关键帧集合sPreviousGroup，判断这个关键帧集合（sPreviousGroup）和我们的候选关键帧子候选组（spCandidateGroup）中有没有相同的关键帧，如果存在，该“子候选组”与该“子连续组”相连，将标志bConsistent置为true，表明该“子候选组”至少与一个”子连续组“相连，置bConsistentForSomeGroup为true并跳出循环。也就是说，如果当前的闭环候选关键帧的子连续组（闭环候选关键帧和它的共视帧）和上一帧的所有闭环候选关键帧组有一组有相同关键帧存在，我们认为连续性达标。
如果有连续（bConsistent = true），接下来判断是否达到连续的条件：
将将当前候选组连续长度在原子连续组的基础上 + 1，如果上述连续关系还未记录到 vCurrentConsistentGroups，那么记录一下。这里cg是当前次的连续性，即上图中右面的连续性。
ConsistentGroup cg = make_pair(spCandidateGroup,nCurrentConsistency);这里就是打包当前帧的其中一个闭环候选关键帧和它的共视关键帧spCandidateGroup以及连续次数nCurrentConsistency。并将其放入本次闭环检测的连续组mvCurrentConsistentGroups里。
如果连续长度满足要求，那么当前的这个候选关键帧是足够靠谱的。
// 连续性阈值 mnCovisibilityConsistencyTh=3 // 足够连续的标记 bEnoughConsistent if(nCurrentConsistency>=mnCovisibilityConsistencyTh && !bEnoughConsistent)将我们此次遍历的闭环候选关键帧添加到最终确定的闭环候选关键帧序列mvpEnoughConsistentCandidates中。
如果该“子候选组”的所有关键帧都和上次闭环无关（不连续），vCurrentConsistentGroups 没有新添加连续关系。于是就把“子候选组”全部拷贝到 vCurrentConsistentGroups， 用于更新mvConsistentGroups，连续性计数器设为0。
if(!bConsistentForSomeGroup) { ConsistentGroup cg = make_pair(spCandidateGroup,0); vCurrentConsistentGroups.push_back(cg); }更新连续组mvConsistentGroups = vCurrentConsistentGroups;。
当前闭环检测的关键帧添加到关键帧数据库中。
1171
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
