ORB-SLAM策略思考之LoopClosing线程

ORB-SLAM策略思考之LoopClosing线程

1 关键帧回环检测

1. 统计与当前关键帧共视的最低相似性得分，并使用该得分和关键帧数据库筛选出符合条件回环候选关键帧，候选策略如下：
   1. 候选关键帧和当前关键帧之间的相似性评分应该较大
   2. 候选关键帧之间应该产生较多的共视，只有这样才能保证当前关键帧和候选关键帧可以产生回环闭合的
   3. 实现这种策略的手段如下：
      1. 计算一个相似性阈值和相同单词阈值，在关键帧数据库中筛选出同时满足这两个阈值要求的关键帧
      2. 计算一个共视累计相似性评分阈值，当候选关键帧之间满足这个要求时，才能作为最终的回环候选关键帧
2. 使用候选组和连接组策略，通过多关键帧累计共视来筛选更严格的回环候选关键帧，策略的依据如下：
   1. 当产生回环时，证明当前关键帧回到了之前去过的地方
   2. 也就是说，当局部建图线程再次产生关键帧时，这个关键帧的回环闭合候选关键帧和之前产生的回环闭合候选关键帧之间应该有充足的共视信息
   3. 这种策略的实现手段如下：
      1. 候选组定义：候选关键帧和其最佳共视关键帧组成的集合
      2. 连接组定义：统计回环集合候选关键帧和上一次回环闭合候选关键帧之间共视关系的集合
      3. 当回环第一次检测到回环候选关键帧时，将候选组都放在连接组中，只是连接部分统计为0
      4. 随着后续产生的回环闭合候选关键帧，使用连接组统计共视信息，如果连接次数超过某个阈值，认定该关键帧是更强的闭环候选关键帧

2 相似变换矩阵SIM3计算

1. 采用RANSAC+BA优化的方式计算相似性变换矩阵SIM3，并采用双重外点删除的策略
   1. 使用RANSAC算法的SIM3求解器进行SIM3矩阵初始值的估计，并判断RANSAC的迭代次数是否用完，这里使用的匹配方式是SIM3投影匹配
   2. 对RANSAC算法认定的内点匹配部分进行BA优化，也就是双重外点删除中的一重
   3. 最后对BA优化认定为外点的匹配进行删除，也就是双重外点删除中的二重
2. 对计算得到的相似性变换矩阵进行地图点和关键帧之间的匹配
   1. 这里的地图点来源是匹配关键帧和其一阶相连关键帧的地图点
   2. 使用地图点到当前关键帧投影匹配的方式进行匹配数目的增加

3 回环融合

1. 当回环闭合线程执行到这部分时，说明发生了回环闭合，这里需要停止局部建图线程，原因如下：
   1. 局部建图线程会改变某些关键帧的位姿，可能会导致回环校正部分改正的位姿被覆盖掉
   2. 可以避免不必要的程序开销
2. 对当前关键帧和与其一阶相连的关键帧进行SIM3矩阵矫正，这里默认为当前关键帧与其一阶相连关键帧之间的SIM3变换的尺度为1，可以使用SE3进行SIM3的替换
3. 在相似变换矩阵SIM3计算策略中，计算了当前关键帧和匹配回环关键帧一阶相连的关键帧地图点的匹配，这里将这些匹配的地图点进行替换和添加（对当前帧的地图点进行替换和添加）
4. 在相似变换矩阵SIM3计算策略中，计算了当前关键帧和匹配回环关键帧一阶相连的关键帧地图点的匹配，这里将这些匹配的地图点进行替换和添加（对与当前帧一阶相连的关键帧中进行替换和添加）
5. 对当前关键帧和与闭环匹配关键帧进行地图点的融合后，会产生一些新的连接，统计这些新生成的连接，或者说新生成的共视
6. 这时，通知跟踪线程进行强制重定位（这里关键帧矫正的位姿只有当前关键帧和与之相连的一阶关键帧，地图点校正矫正的部分只有当前关键帧、闭环候选关键帧和与他们一阶相连的关键帧所匹配的地图点）
   • 在这部分执行跟踪线程的强制重定位，主要是由重定位策略决定的，因为当前跟踪线程对应的普通帧和回环闭合线程对应的关键帧之间大概率会有强共视的关系，从而在重定位阶段可以充分的利用回环闭合线程矫正的关键帧位姿来重定位新的位姿

4 本质图优化

1. 本质图涉及到的内容如下：
   1. 新产生的连接（在回环融合的第5个策略中产生的新的关键帧连接）中，含有当前关键帧或者闭环匹配关键帧的连接，或者共视权重大于100的连接
   2. 最小生成树中的共视关键帧连接
   3. 所有闭环涉及到的连接
   4. 共视图中，共视权重超过100的连接
2. 对上述涉及到的关键帧，使用位姿图进行优化
3. 对地图点的世界坐标系位置、视角向量和深度进行更新，其中世界坐标系位置更新的策略如下：
   1. 如果地图点被当前关键帧矫正过位置，使用当前关键帧的位姿进行校正，否则使用地图点的参考关键帧进行校正，我们将这些关键帧成为矫正关键帧
   2. 获取矫正关键帧未进行本质图优化的位姿和优化有的位姿，首先使用未优化的位姿，将世界坐标系的地图点变换到相机坐标系下，然后使用优化的位姿将相机坐标系下的地图点位置变换会世界坐标系下，就实现了地图点的矫正。