- void LoopClosing::Run()
回环线程主函数，死循环一直进行回环检测：
如果CheckNewKeyFrames()检测到有新关键帧进来，DetectLoop()就检测是否有回环
ComputeSim3()计算当前帧与闭环帧的Sim3变换，然后CorrectLoop()进行闭环纠正 - bool LoopClosing::DetectLoop()
检测回环：-
如果距离上次闭环没多久（小于10帧），或者map中关键帧总共还没有10帧，则不进行闭环检测
if(mpCurrentKF->mnId<mLastLoopKFid+10) { mpKeyFrameDB->add(mpCurrentKF); mpCurrentKF->SetErase(); return false; } -
遍历所有共视关键帧，计算当前关键帧与每个共视关键的bow相似度得分，并得到最低得分minScore
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; }- 在所有关键帧中找出闭环备选帧
这个函数就是用于选择出候选关键帧的，具体操作：将与当前帧相连的局部关键帧剔除然后遍历所有关键帧，找出与当前关键帧具有相同单词的关键帧，然后统计所有闭环候选帧中与当前关键帧具有共同单词最多的单词数，将最多单词的80%设置为阈值，然后找出所有单词数超过阈值，且相似度检测大于相邻关键帧最低分数的关键帧。
vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);DetectLoopCandidates()中将这些关键帧和与他自己相邻最紧密的前10个关键帧设定为一组（注意这里的组是以每一个关键帧为中心，加上与其相邻的关键帧所形成的，一个关键帧可以在多个组里面）。然后计算每组的总得分以及每组得分最高的关键帧，以组得分最高的0.75作为阈值，找出高于这个阈值的所有组里面得分最高的帧，作为候选帧。
- 进行回环检测：
每个候选帧将与自己相连的关键帧构成一个“子候选组spCandidateGroup”， vpCandidateKFs-->spCandidateGroup
检测“子候选组”中每一个关键帧是否存在于“连续组”，如果存在 nCurrentConsistency++，则将该“子候选组”放入“当前连续组vCurrentConsistentGroups”
如果nCurrentConsistency大于等于3，那么该”子候选组“代表的候选帧过关，进入mvpEnoughConsistentCandidates
1.子候选组(CandidateGroup): 对于某个候选的回环关键帧, 其和其具有共视关系的关键帧组成的一个"组";
2.连续(Consistent): 不同的组之间如果共同拥有一个及以上的关键帧,那么称这两个组之间具有连续关系
3.连续性(Consistency):称之为连续长度可能更合适,表示累计的连续的链的长度:A--B 为1, A--B--C--D 为3等;具体反映在数据类型 ConsistentGroup.second上
4.连续组(Consistent group): mvConsistentGroups存储了上次执行回环检测时, 新的被检测出来的具有连续性的多个组的集合.由于组之间的连续关系是个网状结构,因此可能存在,一个组因为和不同的连续组链都具有连续关系,而被添加两次的情况(当然连续性度量是不相同的) - 在所有关键帧中找出闭环备选帧
-
- bool LoopClosing::ComputeSim3()
Sim3求解:- 遍历每一个候选帧
- 当前帧mpCurrentKF与闭环候选关键帧pKF匹配，匹配的特征点数太少，该候选帧剔除，构造Sim3求解器
- 一直循环所有的候选帧，每个候选帧迭代5次，如果5次迭代后得不到结果，就换下一个候选帧直到有一个候选帧首次迭代成功bMatch为true，或者某个候选帧总的迭代次数超过限制，直接将它剔除。其中对有较好的匹配的关键帧求取Sim3变换，Sim3优化，只要有一个候选帧通过Sim3的求解与优化，就跳出停止对其它候选帧的判断。
- 取出闭环匹配上关键帧（也就是刚刚通过回环检验的这一帧）的相邻关键帧提取出来得到一个集合（同时把匹配上的关键帧也加入到这个集合中），再把它们所对应的Map Point取出来（这里也包括匹配上的关键帧的Map Point，不过这个需要标记一下，避免重复添加），得到一个集合。然后把这些Map Point全部投影到当前关键帧上，进行匹配，根据Sim3确定一个大致区域，然后在附近区域搜索，得到匹配。然后根据匹配判断是否超过40个匹配点，如果超过则认为匹配成功，否则将有Local Mapping送进来的队列清空，等待下一次。
- void LoopClosing::CorrectLoop()
闭环校正：- 首先通知局部地图，让他停止关键帧的插入
- 根据共视关系更新当前帧与其它关键帧之间的连接，mpCurrentKF->UpdateConnections();
- 通过位姿传播，得到Sim3优化后，与当前帧相连的关键帧的位姿，以及它们的MapPoints
- 检查当前帧的MapPoints与闭环匹配帧的MapPoints是否存在冲突，对冲突的MapPoints进行替换或填补
- 通过将闭环时相连关键帧的mvpLoopMapPoints投影到这些关键帧中，进行MapPoints检查与替换
- 更新图关系，得到了因闭环Map Point融合之后得到的图关系。
网友评论