ORB-SLAM,这个在视觉SLAM领域家喻户晓的名字,江湖地位不用多说,看看高博的《视觉SLAM十四讲》和各家SLAM面经就知道了!
ORB-SLAM是一个完整的SLAM工程,源码在GitHub上已经有4.4K+Star了,不得不给西班牙小哥献上膝盖。
最近把它的论文重温了一遍,再配上源码,那感觉简直不要太酸爽。网上已经有各路大神把ORB-SLAM“剥皮抽筋”,今天我也鼓起勇气对它动刀了,这里就它的一些独到特性做一下总结吧。
1 系统框架
ORB-SLAM主要由三个并发进程组成:跟踪、局部建图和回环检测,下图是论文中给出的系统框图,很直观地展现了各模块的功能和步骤。
其中,Tracking是这里面的主进程,它负责对每一帧相机的定位和跟踪,通过特征匹配对相机在正常运作和跟丢的情况下进行位姿估计和优化。
Local Mapping负责插入和删除新老关键帧和地图点,并对局部的关键帧和地图点进行局部优化。
而Loop Closing则对每一个新的关键帧进行回环搜索,一旦检测到回环,就通过计算相似变换(Sim3)将回环对齐和融合,并在Essential Graph上进行位姿图优化。
作者在论文中提到,ORB-SLAM的贡献或者说特性主要包括以下几点:系统的所有任务都使用相同的特征点
鲁棒的初始化步骤
采用共视图和本质图
实时的相机重定位
关键帧和地图点的删增策略
ORB-SLAM每一个部分展开都错综复杂,一篇文章三言两语很难总结到位,所以这里先对上述的5个主要特性简单整理一下,如有错误欢迎指出。
2 特征提取
ORB-SLAM,顾名思义,它提取的是ORB特征点。为什么在茫茫特征中,作者却选择用ORB特征点呢?因为要想达到实时的效果,用特征点法的SLAM系统必须保证,特征提取的速度一定得足够快。
我们熟知的SURF和SIFT特征点,虽然效果很好,可是每张图片提取SURF要花217.3ms,提取SIFT更是要花5228.7ms,这让机器人的小身板怎么承受得住!
而用ORB就很舒服了,据《ORB: An efficient alternative to SIFT or SURF》这篇论文给出的数据,每张图片提取ORB特征只需要15.3ms,这样看来实时操作完全没问题。
ORB全称是Oriented FAST and Rotated BRIEF,这里我一直有个疑问,为啥FAST没有纳入简称中??ORB作者在论文中说:"Our proposed feature builds on the well-known FAST keypoint detector and the recently-developed BRIEF descriptor; for this reason we call it ORB. Both these techniques are attractive because of their good performance and low cost."
所以呢?为啥没有人家FAST的名号[捂脸]!当然这是题外话了。从名字上也能看出来,ORB是给FAST角点加上了方向,而且使BRIEF描述子变得可旋转性。
方法上,先利用图像金字塔对每一层图像提取FAST角点,这样可以使得相同的物体在不同分辨率(尺度)大小的场景中都能提取到关键点,因此特征点具有尺度不变性。(图片来源于网络)
然后再通过灰度质心法,计算出以FAST角点为几何中心的图像块质心,图像块几何中心指向质心的方向就是该特征点的方向。
在计算BRIEF描述子时,挑选关键点附近p和q两个位置时会将特征点方向考虑进来,从而实现旋转不变性。
这么说可能很笼统,还是很难理解ORB的特性,但是这里的重点不在ORB上,所以就不详细展开解释了,后面另外总结一篇再对ORB刨根问底吧!
ORB-SLAM中对特征提取有一个很特殊的地方,就是对图像划分网格,利用四叉树结构保存提取到的特征点,从而使得图像的特征点分布比较均匀。
具体怎么实现的呢?这里就用知乎大佬小葡萄所举的例子。我们假设一张图像要提取25个特征点,将图像均匀分为4个节点,只要节点里的特征点数目不等于1或0,抑或节点数目没有超过25,那么就对每个节点继续划分下去。
这个例子中,先将图像划分为4个子节点,因为每一个节点里特征点数目都超过1,而且节点数量也没有达到25个,所以对每个节点继续划分子节点。
这时3行4列那个节点中没有特征点了,那么就将该节点删除。同时,画蓝圈的三个节点内只剩1个特征点,那么这些节点也不会继续划分下去了。
于是,将其余的每个节点都继续划分成4个子节点,这时已经有30个节点,所以停止节点划分。那些特征点个数大于1的节点,就保留质量最好(response最大)的特征点。
对比一下就会发现,通过这种策略提取到的特征点分布会比较均匀,如果提取出的特征点不够,通过降低FAST阈值也还是可以进一步提取更多关键点的。
3 单目初始化
因为单目相机不能获取深度信息,所以单目SLAM得通过初始化确定尺度并且得到一个初始地图。
前面解释了怎样对图像进行特征提取,而在ORB-SLAM中,初始化提取的特征点设定为一般图像帧的两倍,但是如果提取的特征点不够(<100)或者匹配对不够(<100),都要找新的两帧重新进行初始化。
一旦找到了满足要求的前后两帧图像,就利用RANSAC迭代同时计算基础矩阵F(八点法)和单应矩阵H(DLT),并通过计算重投影误差得到两个模型的分数SF和SH。
如果分数比例SH/(SH+SF)>0.45(代码中设为0.4),就用单应矩阵恢复相机位姿,否则就认为相机符合对极约束模型,从基础矩阵恢复相机位姿。
这就可以让ORB-SLAM自动判断平面场景还是非平面场景,不需要人为干涉选定一种模型计算相机位姿。
在对恢复出的位姿进行选择时,会对三角测量的点检查其坐标是否有限、深度是否为正、视差是否够大、重投影误差是否够小。
如果选定的H矩阵或者F矩阵最后没能找到一个足够可靠的位姿,或者两帧之间没有足够的视差,抑或在此位姿下好的3D点不够多,那么就判断这两帧的初始化失败。
当然,如果初始化成功,便将初始帧(前一帧)设为坐标原点(R为单位阵,t为0向量),这样前一帧到后一帧的旋转矩阵和平移向量就是后一帧的位姿了。
同时,还要构建共视图关系,将三角测量得到的3D点设为初始地图点,并通过full BA对初始化位姿和初始地图点进行优化。
最后,对地图尺度归一化、局部建图进程插入关键帧、设置局部关键帧和地图点等一通操作猛如虎,看得我变二百五!
4 共视图和本质图
上面提到了共视图,这也是ORB-SLAM里的一大特色,作者借鉴了前人的这一结构,把整个系统中的关键帧都联系在一起。
作者通过构造共视图存储各个关键帧之间的关系,以便在跟踪、重定位、局部建图、回环检测等多个地方发挥其作用。
那么这究竟是个什么结构呢?其实就是一个无向加权图,以关键帧为节点,关键帧之间如果有共同观测的地图点则产生边,共同观测的数量就是边的权重。
这么说可能还是有点抽象,那就以下面这张图为例子吧。
假设有六个关键帧(红黄绿蓝紫黑),对应颜色框里的地图点为各自观测到的点,可以看出红黄两帧共同观测到的地图点个数为5,红绿共同观测到1个,黄绿共视个数为3,以此类推。
于是就可以得到右边的图结构,红黄之间边的权重为5,红绿权重为1,黄绿权重为3等等。
这有什么作用呢?直观来说,只要揪到一个,就可以把同伙都一网打尽!
比如,在重定位的时候需要在KeyFrameDatabase中搜索候选关键帧,由于关键帧之间是有重叠视图的,所以在得到匹配的候选关键帧之后,继续把每一个候选关键帧在共视图中的所有“同伙”揪出来,从他们之中重新挑选分数最高的作为最新候选关键帧,同时把他们的分数求和作为新候选帧的分数。
最后,返回分数高于新候选关键帧中最高分75%(>0.75*bestScore)的所有候选帧,用于接下来的重定位匹配等操作。
那么本质图就是浓缩版的共视图,因为它是在共视图的基础上,只保留权重比较大(>100)的边,所以本质图意味着其中的关键帧共视程度较高。
它主要用在回环检测中,当检测到回环时,利用本质图对相似变换Sim3进行位姿图优化。
下图展示了共视图和本质图的区别所在,边的减少可以降低位姿图优化的负担。
论文中给出的可视化结果图对比就更明显了。
5 相机重定位
有时提取的特征点过少或者纯旋转时可能会导致相机跟丢,那总不能接下来系统就不工作崩溃了吧,所以ORB-SLAM还提出了重定位的策略。
如果判断相机跟丢,先通过词袋模型和前面提到的方法挑选出候选关键帧,然后把当前帧和候选帧挨个进行特征匹配。
通过特征匹配可以得到3D地图点和2D像素点之间的匹配对,因此就用EPnP通过RANSAC迭代计算当前帧的位姿,接着通过motion-only BA对位姿进行优化。
如果内点不够多,就通过投影搜索更多的匹配点对位姿进一步优化。就这样挨个和候选帧不停地迭代、迭代、迭代。。
最后如果有足够的内点支持,就停止迭代,计算出位姿,重定位成功啦!ORB-SLAM的重定位功能还是挺不错的,之前跑的时候如果跟丢,没多久就又给重新掰回来了。
6 关键帧和地图点
ORB-SLAM的最后一个特点就是它对关键帧和地图点的删增策略。通过前面的讲述可以发现关键帧的词频非常高,因为关键帧真的是ORB-SLAM的关键啊。
如此重要的部分它是怎么对他们进行处理的呢?
决定是否插入关键帧的决策是在跟踪进程中进行的,因为后面局部建图进程中还会删除冗余的关键帧,所以作者认为可以尽可能快地插入新关键帧。(论文中对关键帧的可视化结果)
关键帧插入需要同时满足以下四点要求:距离上一次全局重定位已经超过20帧(保证良好的重定位)
局部建图进程闲置,或者距离上一次关键帧插入已经超过20帧(会阻断局部BA)
当前帧至少跟踪50个地图点(保证当前是良好的跟踪)
当前帧跟踪的点数比参考帧跟踪点数的90%还要少(保证有较大的视觉变化)
但删除关键帧时就不留情面了,末位淘汰,没用的就只好拜拜!因为人家BA工作负担很大,没用的就别来添乱了。
这么大的H矩阵,BA不是你想进,想进就能进!于是,作者决定把那些90%的地图点都能在别的关键帧(至少三帧)中观测到的关键帧踢出局(有点拗口,慢慢捋顺)。
说完关键帧,再说说地图点。地图点是由共视图中相连的关键帧三角测量得到的,它的要求和前面初始化中初始地图点差不多:
视差是否足够大、深度是否为正、重投影误差是否足够小、尺度是否一致(这一指标不太懂,还望指教)。
光说视差很难脑补,根据对代码的理解,我认为视差可以通过下图来表示,夹角theta就是所要计算的视差角,如果角度大于1°就认为视差足够大了。
最后,新建的地图点要想不被删除,也需要经过严格的考验:跟踪时必须在超过四分之一的可视帧中匹配到该点(可视代表该地图点在这一帧的可视范围内,但并不一定和这一帧的特征点匹配上)
必须有三个关键帧观测到该点
只要经过这道考验,该地图点就只可能在关键帧被删除时,或者local BA丢弃局外观测点时被删除了。
上面的这一切都发生在局部建图进程中。
7 花式Bundle Adjustment
细心的你可能已经发现前面提到了三种BA方式,分别是full BA、motion-only BA、local BA。这么多花式的BA,究竟有什么不一样?
Full BA主要是在初始化时用的,它把初始化生成的所有地图点和对应的图像帧都纳入优化变量,好好调优一番。毕竟开局小地图,后面全靠补,一开始必须要稳!
Motion-only BA就是只对位姿进行优化,在跟踪进程中每次估计相机位姿或者重定位时计算出相机位姿后都会对位姿进行优化,而不考虑地图点。
而Local BA则是优化局部的位姿和地图点,在局部建图进程中把当前关键帧和他的“同伙们”以及他们对应的地图点当作优化变量,其余的关键帧则保持不变。
网上大佬路游侠把他们画成了简单的图,更直观地解释了其中关系。图中红色的代表待优化变量,灰色的则表示保持不变。
8 结语
ORB-SLAM整个系统非常庞大,不费点口舌真的说不明白其中的各个机制。这一篇对它的几个主要特性做了个大概的总结,几千字写下来还只是冰山一角,九牛一毛。
接下来,希望可以继续把之前看过的内容细细剖析,因为不记录下来真的转眼就忘了,记录这一篇的时候已经有了深刻的体会,好记性不如烂笔头啊!!
鸟瞰ORB-SLAM,就足以让人对SLAM抱以敬畏之心,在此
向优秀的SLAM大神Orz。
参考资料:
1. 《ORB-SLAM: A Versatile and Accurate Monocular SLAM System》R. Mur-Artal等人
3. 《ORB: an efficient alternative to SIFT or SURF》E. Rublee等人
4. 《视觉SLAM十四讲:从理论到实践 第2版》高翔等人