前言

介绍

算法流程

启动四个线程

• Tracking（等待帧）
• Local Mapping（等待关键帧）
• Loop Closing（等待关键帧）
• Viewer（启动，需要绘制时调用）

1、特征点追踪（Tracking）

Tracking线程主要做以下两件事：（1）估计每一帧相机的位姿，（2）选择新的关键帧。

1.1、ORB特征提取

综合考虑算法的速度（算法的运算时间上，排除SIFT、SURF等）和鲁棒性（尺度、旋转、光照上、排除BRIEF、LDB等），选择ORB特征点算法。ORB（Oriented FAST and Rotated BRIEF）改进了FAST角点不具备方向性的问题，并改进BRIEF二进制描述子，使得ORB算法不仅保持旋转和尺度不变性，而且能满足实时性要求。

在尺度因子为1.2的8个尺度图像下提取FAST角点，对于像素分辨率为512X384到752X480的图像提取1000个角点，对于图像分辨率为1241X376的KITTI数据集中提取2000个角点。为了确保提取的角点均匀分布，将图像划分网格（cell），在每个网格上至少提取5个角点。如果提取的角点不够则自适应调整角点检测阈值。最后计算FAST角点的方向及相应的ORB描述子。

1.2、地图自动初始化

初始化的目的是为了确定较为准确的3D点，通过3D-2D的方式。

由于特征点共面和非共面的约束关系是不一样的，共面时计算单应矩阵（Homography），非共面时计算基础矩阵（Essential Matrix）。开辟两个线程同时计算这两个约束，计算好约束关系后，根据约束关系的累积误差，根据比值确定选择哪种约束关系。

1.3、位姿初始化和重定位

（1）基于前一帧的位姿初始化（Initial Pose Estimation from Previous Frame）

如果可以成功追踪到上一帧，使用匀速运动模型估计当前帧的位姿。将上一帧的MapPoint投影到当前帧确定搜索范围，从而寻找相应的匹配点。

（2）基于全局重定位的位姿初始化（Initial Pose Estimation via Global Relocalization）

如果该帧追踪失败，则将该帧转化为词袋向量（bag of words）并通过查询关键帧数据库来实现全局重定位。然后使用RANSAC算法迭代匹配的特征点，找到最接近的帧，之后采用PnP算法计算相机的位姿。如果具有足够多的内点（inlier），那么就可以对丢失的帧进行重定位。

1.4、基于局部地图的位姿追踪（Track Local Map）

前面的两种跟踪方式，是当前帧与之前帧之间的联系（一对一），这里我们进行局部地图的跟踪，是当前帧与关键帧数据库的联系（一对多），也就是通过更多的3D-2D的匹配点来对位姿进行约束。

一旦我们获取了相机的初始化位姿和一组特征匹配点，我们可以将地图和当前帧联系起来从而得到更多的MapPoint对应关系。为了减少大地图的复杂性，我们只映射局部地图。局部地图包括一个帧集合K1（与当前帧有相同MapPoint的帧），和一个帧集合K2（与K1有相同的MapPoint但是与当前帧无相同的MapPoint）。局部地图中定义一个参考关键帧Kf，它是与当前帧共有最多MapPoint的帧。针对K1和K2中都可见的每一个MapPoint做以下操作，在当前帧中搜索：

（1）计算MapPoint点X在当前帧中的投影，如果该投影超出图像边界则删除该MapPoint；

（2）计算当前帧的视角射线v与MapPoint平均视角方向n的夹角，如果vn < cos(60°)则删除该MapPoint；

（3）计算MapPoint点到相机中心的距离d，如果该距离不在MapPoint的尺度不变区域内（d∉[dmin, dmax]）则剔除该MapPoint。

（4）计算当前帧的尺度因子，比值为d/dmin。

（5）对比MapPoint的特征描述子D和当前帧中还未匹配的ORB特征点，在尺度因子和靠近X的MapPoint作为最优匹配。

相机的位姿最终通过当前帧所获得所有MapPoint进行优化。

1.5、选择新关键帧

跟踪线程除了计算每帧的位姿，还有一个很重要的目的是确定当前帧是否可作为关键帧，我们根据下列的要求选择新的关键帧。

（1）每次全局重定位间隔至少需要超过20帧；

（2）局部地图处于空闲状态，或者当前帧距离上一个关键帧超过20帧；

（3）当前帧至少能追踪50个MapPoint；

（4）当前帧能追踪到的MapPoint要少于参考关键帧Kf中MapPoint的90%。

条件1确保一个良好的重定位，条件3保证良好的追踪。

2、局部地图构建（Local Mapping）

Local Mapping线程的主要做以下两件事：（1）管理MapPoint和关键帧，（2）局部BA优化

2.1、插入关键帧

每添加一个关键帧，更新Covisibility Graph和Essential Graph（更新节点和边约束），以及计算该关键帧的词袋表示，用于准备数据关联三角化产生新的MapPoint。

2.2、MapPoint剔除

保留在地图中的MapPoint必须通过严格的测试，在创建MapPoint后的三帧中可被追踪，这样为了避免三角化错误的MapPoint。一个良好的MapPoint点必须满足下列两个条件：

（1）实际观测到该点的帧数和理论上观测到该点的帧数的比值大于1/4；

（2）在创建MapPoint点后至少可以在3个关键帧中观察到。

当然即使MapPoint满足以上条件，还是有可能会被删除，比如某个关键帧被删除、局部BA的outlier剔除等。

2.3、创建新的MapPoint

通过三角化当前关键帧在共视图（Covisibility Graph）中相邻的关键帧Kc的ORB特征点来创建新的MapPoint。对于关键帧Ki中为匹配的ORB特征点，在其他关键帧中未匹配的点中查找匹配。去掉不满足对极约束的匹配。对ORB特征点三角化后，检查景深、视差、重投影误差和尺度一致性，然后创建为新的MapPoint。具体步骤如下：

（1）得到当前关键帧在共视图中相邻的关键帧Kc（共同的MapPoint数目超过20）；

（2）对相邻关键帧Kc进行遍历，在极线上进行搜索并三角化；

• 基线（当前帧与相邻关键帧）与相邻关键帧对应的MapPoint的深度均值比值不能太小，太小的话形成的3D点不够精确；
• 对未匹配的特征点，首先通过ORB的词汇树进行加速匹配。

（3）根据匹配点对，通过三角化计算3D点

• 检查三角化后的点的深度是否为正值；
• 检查相邻关键帧的重投影误差
• 检查尺度一致性

（4）确定MapPoint的相关属性（平均观察方向，观测距离、最佳描述子等）

（5）MapPoint融合，检查当前关键帧与相邻关键帧（两级相邻）重复的MapPoint。

2.4、局部地图的BA优化

添加完了关键帧及对应map point，有了这些新的约束，进行一次局部bundle adjustment
具体做法：
- 根据Covisibility Graph确定局部关键帧，设为图节点
- 根据局部关键帧，确定局部的map point，设为图节点
- 再找到那些可以看到局部map point的关键帧，但是不是局部关键帧的，到时在优化的时候，设置为fixed，不进行优化
- 把局部map point中每一个map point与观察到该map point的所有关键帧构建边，边的观测值为map point对应的特征坐标，边的信息矩阵，考虑误差是一个像素*尺度
- 去除一些不好的边，再次进行优化，即对keyframe对应的相机外参和mappoint的世界坐标系中的位置进行了修正

2.5、关键帧去冗余

主要剔除冗余的关键帧，这样不至于增加bundle adjustment的压力，而且可以保证在相同的环境下，关键帧的数目不会无限制的增长，这样也减小了存储的压力

具体删除策略
- 根据Covisibility Graph确定局部关键帧，对所有的局部关键帧进行遍历（each_keyframe）
- 对each_keyframe所对应的每一个map point（each_map_point）进行分析，与该each_map_point对应的特征的个数大于3（要求each_map_point 对应each_keyframe 上特征的尺度要大于其它的特征对应的尺度）
- 总的来说也就是each_keyframe上对应的map point能被其它至少3个关键帧观测到90%以上

3、闭环检测

3.1、检测候选的闭环帧

3.2、计算Sim3相似变换

3.3、闭环融合

3.4、关键图的优化

代码

相关工作

重定位

初始化

MapPoint和关键帧

共视图和本质图

Bundle Adjustment

DBoW2