论文:ORB-SLAM2原理详解

太空小孩 2017-12-25

前言

介绍

算法流程

论文:ORB-SLAM2原理详解

启动四个线程

  • 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)

论文:ORB-SLAM2原理详解

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

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

论文:ORB-SLAM2原理详解

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

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

论文:ORB-SLAM2原理详解

前面的两种跟踪方式,是当前帧与之前帧之间的联系(一对一),这里我们进行局部地图的跟踪,是当前帧与关键帧数据库的联系(一对多),也就是通过更多的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

相关推荐