算法原理

Tracking

特征点的提取
在此之前，特征点的提取方法有sift，surf等，但相比ORB都比较耗时。ORB算法先提取FAST角点，再计算BRIEF描述子，最终对每个FAST角点，都生成了只由0、1组成的128维特征向量，并且具有良好的旋转不变性和尺度不变性
- FAST角点提取（对所有像素点进行提取）
  - 将图像转为灰度图，选取像素 $p$ ，假设他的亮度为 $I_p$ - 设定一个阈值 $T$ ，其大小与 $I_p$ 成正比 - 以 $p$ 为中心，半径为3的圆上选16个像素点
  - 如果有连续 $N$ 个点亮度大于 $I_p+T$ 或小于 $I_p-T$ ，则 $p$ 可以被认为是一个特征点，并计算其圆心到质心点的方向
- BRIEF描述子（对所有特征点进行描述子计算）
  - 将每个特征点对应的圆域进行旋转，旋转至所有特征点对应的方向相同
  - 通过对圆域内选取N个点对（默认指定了128对点）亮度的比较结果，特征点映射为只由0、1组成的128维向量
- BoW 词袋模型（对需要比较的图片进行描述）
  - 词袋模型作为所有描述子的字典，用于量化两张图篇间的相似性
  - 字典的数据结构为k叉树，上层是下层的聚类，每个叶子节点对应一个描述子

旋转不变性来自：提取FAST特征时，计算了每个角点的中心点到质心点的方向向量，即使物体发生了旋转，也可利用方向向量来识别。
尺度不变性来自：对每一帧，建立4层的高斯金字塔，分别在每层金字塔上做特征提取，即使A图中的特征在B图中变小，也可以在A图金字塔高层提取到B图中同尺度的特征

点云初始化
这部分在论文中没有提及，只发生在整个算法刚开始运行时，但点云的初始化是此后每一帧能成功定位的前提
- 作用：特征匹配、三角化，得到一个初始的点云图，对于单目相机来说，就是将P2P问题转为PnP问题
- 步骤(以单目为例）：
  - 初始选取2个特征点较多的帧（ $特征点数 > 100$ )，并在词袋中进行特征匹配
  - 并行计算单应性矩阵 $H$ 和基础矩阵 $F$
  - 当对极约束不成立的时候（相机只有旋转没有平移），或场景都在同一平面时，用单应性矩阵来计算两帧之间的变换矩阵。否则用基础矩阵计算变换矩阵
  - 将第一帧的位姿设置为基准位姿，第二帧的位姿为上一步所求出的变换矩阵
  - 创建地图点对象MapPoint，恢复出所有匹配点对应的世界坐标
位姿初始化
对 $C u r r F r a m e$ (此刻的帧)位姿优化之前，先要有一个大概的估计，作为之后优化位姿的初值。根据当前跟踪状态的不同，一共有三种初始化方法：根据参考关键帧估计，根据匀速运动模型估计，重定位。除此之外，还要对此帧观测到的特征点进行估计
- 作用：估计帧的位置，以及观测到的局部地图点
- 步骤：
  - 跟踪状态为LOST时：
    - 用重定位估计位姿，在词袋中寻找与 $C u r r F r a m e$ 最相似的帧 $R e l o c F r a m e$ ，进行特征匹配
    - 统计 $C u r r F r a m e$ 上匹配点数量，数量太少则认为匹配效果差，保持状态为LOST，结束对此帧的处理，等待下一帧；数量足够的话，进行一次仅优化位姿的BA（motion-only BA）
  - 跟踪状态NORMAL时：
    - 根据匀速运动模型估计位姿，默认上一帧到 $C u r r F r a m e$ 的变换矩阵等于上上帧到上一帧的变换矩阵。
    - 将上一帧（LastFrame）观测到的MapPoint 投影到估计的位姿上，观测有多少投影点与特征点位置重合，重合则认为是一对匹配点，如果重合的个数太少，表明匹配失败改用参考关键帧估计；
  - 匀速运动模型估计失败时：
    - 根据参考关键帧估计位姿，用词袋模型，将 $C u r r F r a m e$ 与局部地图的参考关键帧（最新的关键帧）进行特征匹配
    - 匹配后执行一次仅优化位姿的BA（motion-only BA）
    - 统计匹配特征点数，数量少则认为匹配失败，将状态改为LOST，结束对此帧的处理，等待下一帧；
局部位姿优化
经过上面的的跟踪步骤，跟踪失败的 $C u r r F r a m e$ ，说明其与局部地图关系不大，无法建立足够稳健的对极约束；跟踪成功的 $C u r r F r a m e$ 经过运动模型或 “motion-only BA” 得到了初始位姿。

需要注意的是： $C u r r F r a m e$ 中仍会有特征点未建立匹配关系 ，接下来，我们在局部图中寻找这些匹配关系，并利用这些匹配关系更进一步优化 $C u r r F r a m e$ 的位姿
- covisibility graph：共视图由所有关键帧以及他们所观测到的MapPoint组成
- 在covisibility graph中，将四种类型的帧作为我们纳入考虑的局部关键帧
  - 与 $C u r r F r a m e$ 有共视点的帧（一级共视帧）
  - 与一级共视帧有共视点的前十个帧（二级共视帧）（按共视点个数排序）
  - 一级共视帧的子关键帧
  - 一级共视帧的父关键帧（与一级共视帧共视程度最高的帧）
- 将 $C u r r F r a m e$ 、局部关键帧 、以及这些帧对应的 MapPoint 作为局部图 $l o c a l m a p$
- 将 $l o c a l m a p$ 中的MapPoint向 $C u r r F r a m e$ 投影
- 对 $l o c a l m a p$ 进行一次仅位姿BA（motion-only BA）优化，估计出各个局部关键帧的位姿
关键帧的审核与生成
- 检测系统是否允许关键帧的插入
  - only_Tracking 模式下，不需要关键帧
  - $l o c a l m a p$ 被 loopclosing线程占用，不能插入关键帧
  - 关键帧总数较多，且距离上一个关键帧距离很近，不需要插入关键帧
- 审核 $C u r r F r a m e$ 是否有资格做关键帧
  - 记录 $C u r r F r a m e$ 观察到的MapPoint总数， $num_{curr}$
  - 记录 $R e f F r a m e$ 观察到的MapPoint总数， $num_{ref}$
  - 如果 $\frac{num_{curr}}{num_{ref}}$ 小于一定阈值，则允许 $C u r r F r a m e$ 作为一个关键帧
- 将 $C u r r F r a m e$ 设置为关键帧
  - 将自己生成的关键帧 $P k F$ 作为 $C u r r F r a m e$ 的参考关键帧
  - 在地图中新建地图点
总结
- Tracking线程根据状态的不同，以三种方式来跟踪（估计）每一帧的位置和姿态，这三种方式速度由慢到快为：
- 可以看出，Tracking的一个重要作用是提速（不到万不得已，不进行耗时的匹配操作）
- 如果系统的状态合适，且 $C u r r F r a m e$ 位姿估计的效果足够好，则将 $C u r r F r a m e$ 设置为关键帧

LocalMapping

步骤
- 当待处理 $K e y F r a m e$ 队列非空， localMapping线程就开始工作
- 将KeyFrame插入共视图中，更新共视图中的边
- 删除多余的MapPoint
  此前LocalMapping线程可能删除了一些关键帧，导致对应的地图点处于“无帧观测”的状态，这时需要将此类MapPoint移除
- 创建新的MapPoint
  - 对与当前关键帧（ $K_c$ ）的未匹配特征点，在所有其他关键帧（ $K_i$ ）中寻找特征匹配（利用词袋模型），找到匹配后进行三角化，计算出新MapPoint （ $P$ ）的位置
  - 得出MapPoint的位置后，此时 $P$ 点只被 $K_c$ 和 $K_i$ 两个帧所观测到，还需要将 $P$ 分别投影到其他关键帧，观察是否与其他关键帧中的特征点匹配
  - 进行全局BA优化，优化所有的 $M a p P o i n t$ 、所有的 $K e y P o i n t$
- 删除冗余的关键帧
  为了防止关键帧数量无上限增长，需要不停检测冗余的关键帧，如果 $K_c$ 中 90%的点已经在其他帧中被观测到，就将冗余关键帧 $K_c$ 删除