现在开始学习下闭环检测线程。
LocalMapping线程把关键帧送到了mlploopKeyFrameQueue队列中,我们检查该队列是否为空,如果有先的关键帧进来,那么就开始进行回环检测。LoopClosing中的关键帧都是LocalMapping中送过来的:送过来一帧,就检查一帧.
Funtion1:DetectLoop()
从队列中取出一个关键帧,作为当前关键帧,并设为不被擦除的特性,接下来判断距离上一次闭环检测是否超过10帧,如果是的话那么把当前关键帧加入mpKeyFrameDB中,并擦除当前关键帧。
遍历所有共视关键帧,计算当前关键帧与每个共视关键的bow相似度得分,并得到最低得分minScore,这是一个阈值,如果得分比这个最低得分还要低的话,那么就是说肯定不会成为闭环关键帧。
Funtion1.1:pKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore)
在所有关键帧(pKeyFrameDB)中找出闭环备选帧。
* 1. 找出和当前帧具有公共单词的所有关键帧(不包括与当前帧相连的关键帧),统计所有闭环候选帧中与pKF具有共同单词最多的单词数,并以这个最多单词数的0.8倍作为一个阈值。
* 2. 只和共同单词大于最多单词数的0.8倍这个阈值的关键帧进行相似度计算,选出相似度大于 minScore的关键帧。
* 3.单单计算当前帧和某一关键帧的相似性是不够的,这里将与关键帧相连(权值最高,共视程度最高)的前十个关键帧归为一组,计算累计得分,选出最高的组得分,然后以此的0.75 倍作为阈值,得分大于这个阈值的组,作为得分较高的组。
* 4. 只返回累计这些得分较高的组中分数最高的关键帧。
ok,这样我们就得到了闭环候选关键帧组。
如果检测结束后,得不到这个候选关键帧组,那么在关键帧数据库里添加此帧,返回false。
接下来检查关键帧的连续性。计算组得分的目的是剔除单独一帧得分较高,但是没有共视关键帧,作为闭环来说不够鲁棒。对于通过了闭环检测的关键帧,还需要通过连续性检测(连续 三帧都通过上面的筛选),才能作为闭环候选帧。
最终要达到的目的就是如上图所示,接连3帧检测到的闭环关键帧是联系的。
在程序上的实现:先是进来一组候选关键帧,每一帧将自己以及与自己相连的关键帧构成一个“子候选组”。当系统刚开始,或者未检测到有连续时,”子连续组“里的每个”子候选组“的序号 都为0。之后的关键帧的候选关键帧组来的时候,每个组里的候选关键帧组成自己的”子候选组“,与之前的”子连续组“里的”子候选组“依次比较。如果相连的话,把相连的”子候选帧组“和序 号压入当前连续组,这个序号比前一组”子连续组“的序号+1。如果nCurrentConsistency大于等于3,那么该”子候选组“代表的候选帧过关,进入mvpEnoughConsistentCandidates。最后,循 环结束,当前连续组送给系统连续组用于下一次检测。并把当前帧压入关键帧数据库。
Funtion1.2:ComputeSim3()