ORB-SLAM论文解读

主要是参考泡泡机器人slam中冯兵老师的课程

算法输入

• 图像(单目相机采集)+时间码(用于跟其他传感器数据融合)
• 相机内参(在采集图像时，相机的焦距光心都不能改变)
• 用于特征检测的特征描述子字典

算法输出

相机的运动轨迹+地图(关键帧和Map Point)

三条独立的线程

• Tracking:初始化Map Point，计算每一帧的位置，根据计算的每一帧位置对Map Point进行优化，并筛选出Key Frame传给Local Mapping
• Local Mapping:将Tracking得到的Key Frame插入到局部地图中，添加新的帧和Map Point，对MAP进行更新，利用局部BA，将无效观测数据丢弃，并删除冗余的关键帧
• Loop Closing:检测当前帧和之前帧是否为相同场景，若是，则将之前帧的Map Point坐标给当前帧，并将两帧之间的位姿误差分散到每一关键帧中（第一帧除外）

Map包含四个部分

• Map Point
  路标点的3D坐标位置
  观测的方向向量
  ORB描述子
  该点可被观测到的最大最小距离
• Key Frame
  相机的位姿(变换矩阵)
  相机的内参
  该帧中的所有特征点的ORB描述子
• Covisibility Graph
  由Map Point和Key Frame组成
  将共同观测到Map Point数量大于15的Key Frame相连
• Spanning Tree
  一个连接covisibility graph中所有边的最大生成树(这个概念不太清楚)

Place Recognition

用于位置识别和回环检测

• Visual Vocabulary：词袋模型(BoW)中的字典
• Recognition Database：存放关键帧的词袋向量的数据库

(一) Tracking线程

1. 提取输入帧的ORB特征

2. Track With Motion Model
   若当前帧的上一帧没有跟踪丢失，则利用运动模型对当前帧的位姿进行计算。步骤如下：

   • 根据运动模型，估计当前帧的位姿。但这是基于一个假设：当前帧的运动速度和上一帧的运动速度相同。但是这个假设对于运动出现偏移的情况不成立，因此需要进行以下验证
   • 验证：将上一帧的Map Point重投影到当前帧(投影位置为v)，寻找当前帧中是否有该Map Point(相机采集到的图像点位置u)匹配，若点v在点u的一定半径范围内，则认为该点重投影成功。若重投影成功的点数量达到阈值，则认为运动模型是有效的，求得的位姿有效。
   • 若运动模型误差过大，则在上一帧中找到更多的3D-2D匹配点，利用BA(Bundle Adjustment)进行位姿的优化

3. Relocalisation
   若当前帧的上一帧丢失，则需要利用之前建立的关键帧的词袋数据库搜索与当前帧相关的参考帧，利用参考帧计算当前帧的位姿。
   

   • 将当前帧转换为词袋向量，通过关键帧词袋数据库进行查找，找到与当前帧有关的关键帧，确定候选关键帧。

   • 对每一个关键帧，通过词袋进行匹配，如果有足够的匹配（匹配的特征数超过15个），则构建PnP进行求解。

4. Track Local Map：前两种跟踪方式是帧与帧之间的联系，这一步进行局部地图的跟踪，通过更多的3D-2D的匹配对来对位姿进行约束。
   图中已经建立了local map，有keyframes和map points，以及它们之间的映射关系即连线，在加入当前帧时，当前帧可能会与已有的map points有映射关系，但并未提取出它们之间的对应关系，需要将已有的map point投影到当前帧上，判断是否能构建映射关系，若能则添加这个3D-2D的匹配。判断是否能映射的方法如下：

   • 首先确定Map Point能否投影到当前帧上
   • Map Point的世界坐标转到相机坐标系下，看点的深度是否为正
   • Map Point投影到图像上，是否超出图像边界
   • Map Point在深度范围内（每个Map Point产生的时候都会计算最大最小深度）
   • 当前视图方向与MapPoint的平均视图方向的夹角不要大于60度

5. 判断当前帧是否为关键帧：

   • 每次全局重定位过程需要超过20个图像帧
   • Local Mapping线程处于空闲状态，或者距上一关键帧插入已有超过20个关键帧
   • 当前帧跟踪至少50个Map Point
   • 当前帧跟踪少于参考关键帧中Map Point数量的90%

Local Mapping线程

1. 添加关键帧

   • 首先要对covisibility graph进行更新，增加一个节点，并根据新增的3D-2D点对来更新图中连接的边
   • 更新spanning tree
   • 计算关键帧的词袋向量，添加进关键帧的词袋数据库中

2. 创建新的Map point

   • 找到当前关键帧在covisibility graph中邻接的一些关键帧
   • 对邻接的关键帧进行遍历，在极线上进行搜索找到匹配的map point，并进行三角化
   • 计算该Map Point的相关属性(平均观察方向，观测距离，最佳描述子)
   • 判断该Map Point是否被其他关键帧已经找到，若重复，则需判断选用哪一帧观测到三维坐标(不同帧观测计算的坐标会有一点差异)
     • 根据covisibility graph找到邻接关键帧(共同Map Point数量大于20)
       遍历邻接关键帧，对每一个邻接关键帧再次计算有关系的关键帧(共同Map Point数量大于5)
     • 遍历当前帧的一级邻接和二级邻接的关键帧，将当前帧对应的Map Point投影到这些关键帧上，确定对应的特征是否已有Map Point，若有，则选择Map Point对应观测量最多的那个，否则将当前帧的Map Point添加到这些关键帧中。
     • 将这些关键帧的Map Point与当前帧进行融合
     • 对当前关键帧对应的Map Point的属性进行修改
     • 对Covisibility graph，Spanning Tree进行更新

3. 局部Bundle Adjustment
   利用新添加的Key Frame和Map Point以及更新的Covisibility graph，Spanning Tree，进行一次局部的Bundle Adjustment，优化图中的节点参数。

4. 剔除Map point
   对每次加入的Map Point要进行检查，符合以下条件的点要进行剔除

   • 观察到该Map Point的帧数小于三帧
   • 能找到该Map Point的帧数小于理论上可以观察到该Map Point的帧数的1/4(理论能观察到的帧：该Map Point可以投影到该帧的成像平面上)

5. 剔除关键帧
   删除冗余关键帧，可以保证关键帧数目不会一直增长，减小了存储的压力

   • 在Covisibility graph中找到当前帧的局部关键帧
   • 对这些局部关键帧进行遍历，若至少有三个关键帧可以观测到该帧大于90%的Map Point，则删除该帧

Loop Closing线程

1. 检测回环
   • 根据Covisibility graph确定局部关键帧
   • 计算当前帧的词袋向量与所有局部关键帧的词袋向量的相似性，得到最小相似度
   • 在关键帧词袋向量数据库中找到相似度不小于最小相似度的关键帧作为闭环的候选关键帧
   • 遍历候选关键帧，若该帧被连续插入的三个关键帧都检测为候选关键帧，则认为该帧为闭环帧
2. 计算相似变换
   对于闭环帧的检验：
   • 通过RANSAC计算Sim3(相似变换矩阵)
   • 两帧进行互相投影，若匹配的Map Point数量大于40，则认为匹配成功
3. 闭环融合
   • 将当前帧以及Covisibility graph中与当前帧邻接的关键帧中对应的Map Point换为闭环帧对应的Map Point
   • 重新构建Covisibility graph
4. 优化Essential Graph
   对Essential Graph全局进行一次优化，把相似变换带来的误差(当前帧和闭环帧之间的误差)平均分给环路中的每个关键帧