单目ORB-SLAM流程梳理
单目ORB-SLAM
刚刚完成高博《视觉SLAM十四讲》的第一遍学习,读了单目SLAM比较有代表性的ORB-SLAM的文章,结合B站泡泡机器人冯兵老师的分享,对单目ORB-SLAM的流程做了一个简单的梳理。
1.流程
这三部分是并行执行的。
输入:摄像头(图像 + 时间码)
输出:轨迹(每帧图像对应相机位姿) + 地图(关键帧 + Map point)
Map point就是特征对应的三维空间点。
1.1 具体输入
-
相机内参(标定好的)
- 相机的焦距尽量不要改变
- 如果对采集的图像进行缩放处理,相机内参也要进行相应的缩放
-
相关配置文件,如指定特征检测描述子,给出对应特征描述子的词汇表,具体内部设置的一些经验值等。
1.2 基本流程
启动四个线程:
- Tracking
- Local Mapping(等待关键帧出现)
- Loop Closing(等待关键帧出现)
- Viewer(启动,需要绘制调用),就是需要绘制什么的时候再调用这部分就行了
1.3 Tracking
-
Tracking主要完成两件事情
- 确定每帧的初步位姿
- 确定关键帧提供给Local Mapping线程
-
Tracking具体模块
- 初始化
- Initialize
- 跟踪
- Track Reference Key Frame
- Track With Motion Model
- Track Local Map
- 重定位
- Relocalization
- 确定关键帧
- Create New Key Frame
- 初始化
1.3.1 初始化
初始化目的
- 确定较为准确的3D点
- 2D-2D计算位姿(使用单应矩阵H或者是基础矩阵F),但是这种尺度不能统一,所以最后要使用3D-2D的方式统一尺度(PnP)
具体ORB-SLAM提供了自动初始化的方式
- 由于特征点共面与非共面的约束关系不一样,共面计算单应矩阵,不共面计算基础矩阵
- 开辟两个线程同时计算这两个约束,计算好约束关系以后,根据约束关系计算累积误差,根据比值确定使用哪种约束
1.3.2 Tracking
跟踪要处理每帧图像数据
面向对象思想->封装
- 将获取的图像数据封装成帧,后续处理都是对于帧的处理
具体Frame的结构
-
Image -> 存储图像数据(输入)
-
Camera -> 存储相机内参信息(输入)
-
Feature -> 存储帧对应的特征数据(计算)(一对多的关系)
-
Mat -> 存储帧对应相机姿态(计算)
-
Keyframe
- 直接继承自Frame类
-
Feature
- Feature对特征封装(keypoint + descriptor)
- Feature是帧与MapPoint的纽带
- Feature与Frame是多对一的关系
- Feature与MapPoint是一对一的关系
-
MapPoint
- MapPoint对特征对应3d点进行的封装
- MapPoint对应的世界坐标
- MapPoint对应最好的特征描述子
1.3.3 Track Reference Key Frame
每次都是处理当前帧,根据词袋向量进行匹配来找到对应的参考关键帧,然后确定两帧中对应的特征匹配,再根据2D-2D的对极约束方法来求解两帧之间的位姿变化。
1.3.4 Track With Motion Model
这里根据运动模型进行估计,相当于知道当前帧与上一帧之间经过的时间 Δ t \Delta t Δt以后,再根据已知的运动模型来估计当前帧的位姿,将上一帧的map point投影到当前帧,再寻找匹配。
1.3.5 Track Local Map
前面两种跟踪方式,是帧与帧之间的联系,这里我们进行局部地图的跟踪,也就是通过更多的3D-2D的匹配来对位姿进行约束。
跟踪这个地图之前,我们先构建这个局部地图。
局部地图也就是局部keyframe和局部map point
- Covisibility Graph(共可见性图)
- 局部关键帧也就是当前帧拥有共同Map Point的关键帧,并且这些关键帧在Covisibility Graph中(共同map point的数目大于10)
- 局部map point也就是局部关键帧对应的map point
总的来说,这三种跟踪最终目标就是找到Map Point与当前的特征形成对应关系,通过PnP对当前帧的位姿进行求解。
1.3.6 重定位问题
我们会遇到跟踪丢失的情况,那接下来我们需要移动镜头进行重定位。
具体主要通过关键帧数据库进行查找,找出与当前关键帧有关系的关键帧,确定候选关键帧。
对每一个候选关键帧,通过BoW进行匹配,如果有足够的匹配(匹配特征数超过15个),则狗构建PnP进行求解,计算出当前帧的相机外参。
- EPnP算法
- 5次RANSAC算法,计算相机外参
- 优化帧,如果匹配特征数没有50个,则将候选关键帧对应的map point投影到当前帧继续寻找匹配,匹配完成之后再次优化帧,如果匹配数还是没有大于50,则更改投影匹配阈值,再次匹配,只有匹配的个数大于50,才说明重定位成功,最后对大于50个匹配对的帧再次对帧进行优化。
1.3.7 跟踪确定关键帧
跟踪除了计算每帧的位姿,还有一个很重要的目的是确定关键帧。
关键帧的选取策略:
- 关键帧不要太稠密(相邻关键帧之间有一定的间隔)
- 当前帧至少匹配到了50个Map Point==(怎么确定匹配到的map point的个数)==
- 当前帧匹配到的Map Point的个数不能超过参考关键帧(与当前帧拥有最大多的map point的关键帧)对应Map Point的90%,否则的话说明这个时候的参考关键帧可以替代当前帧,还没有必要创建关键帧
- 局部地图优化空闲的时候
1.4 Local Mapping
在tracking线程中,确定好了keyframe以后,则在Local Mapping中进行添加,Local Mapping线程主要是确定更多的约束,对关键帧的位姿和map point的位置进行修正
具体步骤为:
- 添加关键帧
- Mao point的剔除
- 新的Map point的创建
- 局部优化
- 局部关键帧的剔除
1.4.1 添加关键帧
每添加一个keyframe,维护Covisibility Graph, Spanning Tree, Map 以及计算该帧的词袋表示确定匹配,为三角化做准备。
1.4.2 新的Map Point创建
我的理解:之前在Tracking的过程中只考虑了当前帧跟之前帧之间的相对位姿,而且我们选取的关键帧是中间有间隔的,所以并没有计算相邻的几个关键帧之间的相对位姿,这里我们通过特征匹配和三角化就可以得到新的map point
得到当前关键帧(cur_keyframe)在Covisibility Graph中邻接的一些关键帧(共同的map point数单目设置为20)
对邻接的关键帧进行遍历(每一个设为ref_keyframe),在极线上进行搜索并三角化
- 基线(cur_keyframe与ref_keyframe)与ref_keyframe对应的深度均值比不能太小,这样形成的3d点不够精确
- 对未匹配的特征点,首先通过orb的词汇树进行加速匹配
根据匹配点对,通过三角化计算3d点
- 检测三角化之后的点在相机前
- 对参考帧的重投影误差进行检测
- 对尺度进行检查
确定Map Point的相关属性(平均观察方向,观测距离,最佳描述子等)