ORBSLAM三大线程之Tracking部分。

1. 概述

1.1 回环检测的意义

在Slam14讲中提到过，回环检测可以显著减小累计误差的影响，但具体的作用机理并没有讲清楚，这里做一个补充。

由于机器移动过程中，误差会逐步累积导致其计算路径偏离实际路径（误差最明显的是尺度漂移），我们希望能将计算路径修正。

当检测出回环帧，确认路径闭环时，我们可以认为当前帧和回环帧应该是重合的。而实际情况却不相同，这中间差了一个相似变换矩阵S=[sRt01]S=[sRt01]，通过计算当前帧和回环帧的信息，我们能求出这个矩阵。再通过位姿传播原理，对之前所有的计算路径进行修正，最终再全局BA得到最优结果。

可以看到，如果不进行回环检测，画出的地图非常糟糕：

img

而通过回环检测，能有效提高地图效果：

img

1.2 总体结构

回环检测的过程就是一个不断筛选择优的过程，总的来说主要的筛选条件有：词袋相似度检测，孤点检测，连续性检测，词袋特征点匹配检测，Sim3匹配检测，重投影匹配检测。

2. 检测回环DetectLoop

这一步我们准确地检测出回环帧，检测的办法就是通过不断的筛选。筛选的过程可以分为两类：单帧对单帧的筛选，多帧对多帧的筛选。

前者是要筛选出和当前帧有可能回环关系的候选帧，包括二者的相似性，候选帧是否孤立等等。后者是一系列帧的比较，主要是检测连续性。

2.1 总体步骤

  1. 取出一帧

    先从mlpLoopKeyFrameQueue队列中取一帧，这个队列是在LocalMapping的最后插入的关键帧。

  2. 检测与上一次回环的距离

    如果距离上次闭环没多久（小于10帧），或者map中关键帧总共还没有10帧，则不进行闭环检测

  3. 计算当前帧与共视帧的Bow得分

    这一步需要遍历所有共视关键帧呢，计算他们之间的Bow相似度得分，并得到最低得分minScore

  4. 找出闭环备选帧

    和当前关键帧具有回环关系的关键帧,不应该低于当前关键帧的相邻关键帧的最低的相似度，且候选帧不应该孤立

  5. 连续性检测

    实现多帧与多帧的闭环

2.2 找出闭环备选帧

这一步为了在闭环检测中找到与该关键帧可能闭环的关键帧。筛选时有两个阈值：最大共词数，最大组得分。最大共词数用于筛选那些和当前帧长得不像的候选帧，最大组得分用于筛选长得像但孤立的候选帧。

  1. 找出和当前帧具有公共单词的所有关键帧

    需要排除与当前帧链接的关键帧，把找到的候选帧放入lKFsSharingWords，同时记录当前帧与候选帧具有相同word的个数mnLoopWords

  2. 统计候选帧中的最大共词数

    在上一步中所有闭环候选帧与当前帧的共次数都存在了mnLoopWords，遍历然后找到最大共词数。

  3. 挑选共次数合格的候选帧并计算得分

    程序设定的条件是，共词数大于0.8最大共词数。然后调用DBoW2自带的score函数计算得分。将合格的候选帧和得分组成pair放入lScoreAndMatch

  4. 计算组得分去除孤立点

    单单计算当前帧和某一关键帧的相似性是不够的，这里将与关键帧相连归为一组，计算累计得分。具体做法是：

    1. 构建组

      利用pKFi->GetBestCovisibilityKeyFrames(10)得到最佳共视的10帧放入容器vpNeighs中形成一组。

    2. 计算组得分

      遍历组，如果组中的帧满足上面步骤3的条件，则将它的分数累加。

    3. 记录最高得分

      将所有组中的得分最高组的分数记录下来，作为阈值

    4. 组筛选

      排除分数低于0.75倍最高分数的组。将筛选后的候选帧插入vpLoopCandidates

2.3 连续性检测

需要实现多帧与多帧闭环，所以要做连续性检测。

img

如图所示，在闭环候选帧中，紫色那一帧连续被三个当前帧匹配到，所以它通过了合格性检测，可以被作为良好的候选帧。这种检测方法是将一系列当前帧和一系列闭环候选帧比较，所以是多帧对多帧闭环。

这里的”匹配”非常特殊，因为闭环候选帧不是连续的，他们的拓扑结构是这样：

img

彩色三角代表候选帧，蓝色圆形则为普通帧。我们将候选帧和与他有良好共视关系的普通帧圈起来，组成子候选组，后面比较连续性就是以组为单位比较。

检测时，当前帧如果和某一子候选组发生关系(和组员有良好共视关系)，那么这一子候选组连续性+1，并传递到下一个当前帧。如果当前帧没有和某子候选组发生关系，则此候选组连续性直接清零。如果能连续通过3次考验，则就算通过连续性检测。

3. Sim3计算ComputeSim3

该函数计算相似变换，从mvpEnoughConsistentCandidates中找出真正的闭环帧mpMatchedKF。

这个部分要达成两个目的：第一，准确计算Sim变换矩阵，为后面的校正做铺垫；第二，进一步筛选闭环帧。

筛选分为三个阶段：阶段一利用词袋匹配的办法剔除，阶段二利用Sim3匹配剔除，阶段三利用重投影匹配剔除。

Step1 一次筛选

  1. 从闭环候选帧容器中取出一帧

    之前通过连续性检测的结果都放在mvpEnoughConsistentCandidates，从这里面取出一帧。

  2. 当前帧与候选帧匹配

    通过bow加速得到mpCurrentKF与pKF之间的匹配特征点，调用函数SearchByBoW，如果匹配的特征点少于20个，vbDiscarded[i]打上标记。

  3. 构造Sim3求解器

    Ransac参数：迭代300次，至少20个内点才能通过。结束后重复执行1。

Step2 二次筛选

  1. 从候选帧容器取出一帧

    如果vbDiscarded有标记，则放弃此帧，再取一帧。

  2. 求解Sim3

    Step1.3构造的求解器存储在vpSim3Solvers[i]中，取出来，求解，迭代5次。如果得不到好的结果，打上discard标记。

  3. Sim3弥补漏匹配

    在Step1.2中进行了一次匹配，但由于尺度误差，很多特征点没有进行有效匹配，现在成功计算出了相似变换矩阵，用它进行弥补。调用函数SearchBySim3

  4. Sim3优化

    引入弥补后的匹配点，调用OptimizeSim3进行优化。如果优化得到的内点数大于20，则表示通过考验，此帧就是闭环帧。然后立马break。结束后重复执行1。

  5. 清理垃圾

    如果把候选帧容器都遍历完了，依然没有任何一帧被确立为回环帧，则说明当前帧没有发生回环。清除mvpEnoughConsistentCandidates

Step3 三次筛选

  1. 提取闭环帧的相连关键帧

    把相连关键帧的所有MapPoint放入mvpLoopMapPoints

  2. 投影到当前帧匹配

    调用SearchByProjection，统计匹配成功的点数

  3. 清理候选帧容器

    如果匹配成功的点数nTotalMatches大于40，说明完成了最后的考验，返回true，清空容器mvpEnoughConsistentCandidates；若小于40，则表示闭环失败，清空容器，返回false。

4. 回环校正CorrectLoop

通过前面的操作我们得到了最终的回环帧和Sim变换矩阵，现在要利用这些条件，进行全局校正。

校正之前需要先停止LocalMapping线程，停止全局BA。

4.1 主要步骤

  1. 更新连接

    回环检测和Sim3计算需要消耗一定时间，这时候机器人依然在移动，依然有关键帧传输进来，因此在这一步需要重新更新一下帧与帧之间的共视关系。

  2. Sim3优化位姿和地图点

    主要是通过位姿传播原理，通过相对位姿关系，可以确定这些相连的关键帧与世界坐标系之间的Sim3变换。这一 步开始遍历相连关键帧

  3. 检查地图点冲突

    检查当前帧与闭环帧的MapPoints是否存在冲突，对冲突的MapPoints进行替换或填补。有的时候可能会产生一个特征点对应两个地图点的情况，需要用Step3匹配的结果替换当前帧的结果。和下面不同的是：这里并没有做投影匹配，只是调用replace函数将重复的地图点踢掉了。

  4. 地图点融合

    这一步跟上一步有点像，不同的是融合的对象是闭环时所有相连关键帧对应的地图点mvpLoopMapPoints。此处调用了Fuse函数，它通过投影作用，将mvpLoopMapPoints投影到校正后的当前帧，在阈值为4的范围内搜索。如果MapPoint能匹配关键帧的特征点，并且该点有对应的MapPoint，那么将两个MapPoint合并（选择观测数多的）。如果如果MapPoint能匹配关键帧的特征点，并且该点没有对应的MapPoint，那么为该点添加MapPoint。

  5. 更新连接

    与第一步不同的是，这一步更新是因为闭环校正，第一步是因为新插入了关键帧。调用UpdateConnections后得到了新的连接关系，然后删除之前存在的一级连接关系和二级连接关系（防止冲突）

  6. 优化

    1. EssentialGraph优化

      对形成闭环后新生成的重要的关键帧的Sim3位姿进行优化。回环边不参与优化。调用OptimizeEssentialGraph优化，采用g2o方案

    2. 全局BA优化

      上一步没有优化回环边，这里添加进去。新建了一个线程执行全局BA。

4.2 Sim3传播优化

针对第二步Sim3优化位姿和地图点进行详细分析。

  1. 传播计算Sim3

    位姿传播公式PoselPosec=SimlSimcPoselPosec=SimlSimc，我们只需要得到回环帧的位姿，当前帧的位姿，回环帧的Sim矩阵，就可以求出当前帧的Sim矩阵，把计算后的结果放在CorrectedSim3(这里只算出来，还没有校正)

  2. 修正MapPoint

    利用Sim修正的结果计算地图点

  3. 校正关键帧位姿

    将Sim3转换为SE3才能更新位姿

  4. 更新连接