论文:ORB-SLAM: a Versatile and Accurate Monocular SLAM System
主要贡献
- 对追踪、地图构建、重定位、闭环检测采用相同的特征,使得系统更高效、简单、可靠。采用的ORB特征在没有GPU的情况下也有很好的实时性,且具有旋转不变性和光照不变性。
- 支持在大场景中实时运行。由于共视图(covisibility graph)的使用,特征点的跟踪与构图主要集中在局部共视区域,而与全局地图的尺寸无关。
- 使用一种我们称为Essential Graph的位姿图优化位姿,实现实时回环检测。它是由系统维护的生成树、闭环的链接和共视图(covisibility graph)的强边共同构建的。
- 实时相机重定位具有明显的旋转不变特性和光照不变性。这就使得跟踪丢失后可以恢复,增强了地图的重用性。
- 一种新的基于模型选择的自动和鲁棒的初始化程序,它允许创建平面和非平面场景的初始建图
- 提出了一种用来选择地图点和关键帧的“适者生存”方法:生成时宽松但剔除时严格。这种策略可以剔除冗余的关键帧,从而增强追踪的鲁棒性以及长时间运行的能力。
回环和重定位方法基于他们之前提出的:Fast relocalisation and loop closing in key-frame-based SLAM
该系统的初步版本在:ORB-SALM:Tracking and mapping recognizable featrues
地图点和关键帧
每个地图点$p_i$存储:
- 世界坐标系下3D位置$X_{wi}$
- 观测方向$n_i$(相机光心到观测点的方向)
- ORB描述子$D_i$,它的汉明距离相对于被观察到的关键帧中的所有其他相关描述子是最小的
- 根据尺度不变性可以看到的该点的最大尺寸和最小尺寸$d_{min}$, $d_{max}$
每个关键帧$K_i$存储:
- 相机位姿$T_{iw}$,即刚体从世界坐标系到相机坐标系的转换
- 相机内参,包括焦距和主点
- 在帧中提取的所有ORB特征
共视图和本质图
共视图:
- 无向加权图
- 每个节点代表一个关键帧
- 若两关键帧同时观测到15个地图点以上,则这2个节点用一条边相连
- 其共同观测到的地图点数,作为该边的权重
本质图:
位姿优化时需将回路误差进行传播,因为共视图会非常密集,所以需要包含所有节点但边更稀疏的图
- 从初始关键帧开始增量式构建一个生成树,该树提供了一个边数最少的共视图的连接子图
- 当插入新关键帧时,将该关键帧与树上与其有最多共同观测点的关键帧相连接
- 当一个关键帧被剔除时,系统将更新受该关键帧影响的连接
本质图包含了生成树、共视图的边的子集、以及回环的边
一、Tracking
作用:计算相机位姿、插入关键帧
流程:
- 计算初始特征和上一帧匹配,用motion-only BA优化位姿
- 如果跟踪失败,进行全局重定位
- 如果跟踪顺利,用关键帧的共可视图得出局部地图
- 用重投影搜索局部地图点
- 决定是否插入新关键帧
自动初始化地图:
因为单目相机无法获取深度信息,就无法获取尺度信息,所以需要初始化地图。我们需要计算两帧间的位姿关系,通过三角化算出初始地图点。
步骤:
- 在当前帧中寻找与参考帧匹配的特征
- 计算单应矩阵H和基础矩阵F
- 选择模型,平面用H,非平面用F
- 根据选择的模型计算运动,H用8点法,F用4点法
- BA
tracking
步骤:
- 提取ORB
- 在8层图像金字塔上提取FAST角点
- 为了确保特征点分布均匀,先将每层图像分成网格,每格提取至少5个特征点
- 如果某格不够提取5个,就调整检测器阈值再检测一次
- 根据FAST角点计算其方向和ORB特征描述子
- 通过上一帧(运动模型)估计初始位姿
- 如果跟踪成功,利用匀速运动模型估计相机位姿,对上一帧观测到的地图点进行搜索
- 如果跟踪失败,在上一帧地图点的基础上加大搜索范围,然后根据找到的匹配关系优化位姿
- 通过全局重定位估计初始位姿
- 如果跟踪失败,将当前帧转化为词袋(BoW)向量,在数据库中查询关键帧进行重定位
- 计算每个候选关键帧的ORB与地图点的对应关系
- 对每个候选关键帧进行RANSAC迭代,并用PnP计算当前位姿
- 若重定位成功,则跟踪继续进行
- 跟踪局部地图
- 局部地图包含一组关键帧$K_1$,他们和当前帧有部分共同的地图点;其中与当前帧有最多公共地图点的为参考帧$K_{ref}$
- 共可视图中和$K_1$相连的关键帧记为$K_2$
- 在当前帧中搜索$K_1$和$K_2$中的地图点:
- 计算地图点在当前帧中的投影点,若超出边缘则舍弃
- 计算当前观测方向和地图点平均观测方向的夹角,若小于60°则舍弃
- 计算地图点到相机光心的距离d,若超出尺度不变区域[$d_{min}$ , $d_{max}$]则舍弃
- 计算当前帧的尺度$d/d_{min}$
- 将地图点的描述子和当前帧中未匹配的ORB进行匹配 - 通过当前帧的所有地图点进行位姿优化
- 插入新关键帧
- 由于相机运动,要尽可能快地插入新关键帧,回头再剔除冗余关键帧
- 插入新关键帧要满足以下要求:
- 距上一次全局重定位超过20帧(确保好的重定位)
- 局部建图处于空闲状态,或距上一关键帧超过20帧
- 当前帧跟踪至少50个地图点(确保好的跟踪)
- 当前帧与参考帧$K_{ref}$共同观测到的地图点少于90%(最小视图变换)
二、Local Mapping
作用:负责处理新的关键帧,使用局部BA对相机位姿周围环境进行优化重构
流程:
- 在共视图连接的所有参考帧中寻找当前帧中未匹配ORB的对应项,将之三角化为新的地图点
- 剔除冗余关键帧
步骤:
对于每一帧关键帧Ki:
- 插入关键帧
- 更新共视图:增加$K_i$节点,更新它和关键帧之间的边(存在共同地图点)
- 更新生成树
- 更新词袋(BoW)
- 剔除最近地图点
满足以下条件的地图点才能保留,否则剔除
- 追踪线程在预测可见该地图点的关键帧中有超过25%确实可以找到该地图点
- 创建地图点后,至少往后3帧都可看到该地图点
- 创建新地图点
- 在其他关键帧的未匹配ORB特征中查找$K_i$中的未匹配ORB
- 三角化ORB特征对后,检查是否在相机前方、视差、重投影误差、尺度一致性
- 局部BA
- 优化对象:当前帧、共视图中与其相连的关键帧、这些帧中的地图点
- 优化后的异常值会被删去
- 剔除局部关键帧
- 若该关键帧有90%地图点被其他3帧在相同尺度下观测到,则剔除
三、Loop Closing
作用:对每个新关键帧进行闭环搜索,检测到闭环就进行优化
步骤:
- 检测候选回环
- 计算$K_i$和其共视图邻边的BoW向量,记最低分为$s_{min}$
- 检索数据库,舍弃分数低于$s_{min}$的关键帧
- 舍弃在共视图中与Ki相连的关键帧
- 连续检测到3帧回环才算回环
- 计算相似变换
- 计算当前帧与候选回环关键帧之间的ORB对应关系
- 对每个候选回环执行PANSAC迭代,用Horn方法计算相似变换
- 回环融合
- 融合重复的点,在共视图中插入与回环相关的新边
- 通过相似变换矫正当前帧位姿,将矫正传播到相连边,对其回环两侧
- 将回环关键帧及其邻帧的地图点都投影到当前帧及其邻帧上,并在投影附近小范围搜索对应匹配点,进行融合
- 本质图优化