摘要
本文提出了ORB-SLAM,这是一种在大小场景、室内室外环境下都可以实时运行的基于特征的单目SLAM系统。系统对复杂的剧烈运动具有鲁棒性,允许宽基线的闭环和重定位,且包含完整的自动初始化。在最近几年的优秀算法基础上,我们从头开始设计了一种新颖的对包括追踪、建图、重定位、闭环所有SLAM任务在内都使用相同特征的系统。选择点和重建关键帧的适者生存策略具有很好的鲁棒性,并能够生成紧凑的可追踪的地图,只有当场景内容发生变化地图才改变,从而允许长时间运行。本文从最受欢迎的数据集中提供了27个序列的详尽评估。相对于其他最先进的单目SLAM方法,ORB-SLAM实现了前所未有的性能。为了造福社区,我们将源代码公开。
关键字:长期建图,定位,单目视觉,识别,同时定位和建图(SLAM)
I. 引言
我们知道,在我们提供优质(strong)的匹配网络和良好的初始值预测的情况下,BA(bundle adjustment)方法可以给出相机定位的精确估计以及稀疏几何重建[1,2]。在很长一段时间内,在诸如VSLAM这种实时性应用中,BA方法(由于其理论算力的大开销)被认为是无法使用的。VSLAM的目标是在建图的同时估计相机的轨迹。如今我们知道,为了使BA计算成本在可承受范围内的同时还获得较为准确的结果,一个实时SLAM算法必须向BA提供以下信息:
- 在选定帧(关键帧)子集中场景特征(地图点)的相应观察
- 复杂性随着关键帧数量增长,关键帧的筛选要避免不必要的冗余
- 关键帧和地图点之间强大的网络配置用以产生准确的结果,也就是说关键帧中的观察到的地图点能够提供显著的视差以及足够的回环匹配
- 提供一个尽可能准确的用于BA非线性优化的关键帧位姿初始值
- 探索中的局部地图,优化的重点是具备更好的可扩展性(对于局部地图,优化只在局部进行而与全局尺寸无关)
- 可以实时进行快速全局优化(比如位姿图)以结束(close)闭环的能力
BA第一次实时应用是在Mouragon等人[3]提出的视觉里程计算法中,接着是Klein和Murray做出的突破性SLAM工作,其被称为并行追踪和建图(PTAM)[4]。尽管受制于小场景的应用,PTAM算法对关键帧的选择、特征匹配、点的三角化、每一帧的相机位姿估计、追踪失败后的重定位等提供了简单而有效的方法。不幸的是,缺少闭环检测和足够的对遮挡的处理(机制)、重定位的视图不变性差、在地图初始化时需要人工干预等因素严重限制了其应用。
在本文中,我们基于PTAM算法的主要框架,采用Gálvez-López和Tardós提出的place recognition(场景/位置识别)[5]算法,Strasdat等人提出的scale-aware loop closing(具备尺度感知的闭环检测)[6]算法以及文献[7]和[8]中的大尺度操作中共视(covisibility)信息的使用方法,重新设计了一种新的单目SLAM系统:ORB-SLAM,贡献主要包括:
- 对所有的任务采用相同的特征:追踪、地图构建、重定位和闭环检测。这使得我们的系统更有效率、简单可靠。采用的ORB特征[9]在没有GPU的情况下也有很好的实时性,且具有旋转不变性和光照不变性。
- 支持在大场景中实时运行。由于共视图(covisibility graph)的使用,特征点的跟踪与构图主要集中在局部共视区域,而与全局地图的尺寸无关。
- 使用一种我们称为Essential Graph的位姿图(pose graph)来优化位姿实现实时回环检测。它是由系统维护的生成树、闭环的链接和共视图(covisibility graph)的强边共同构建的。
- 实时相机重定位具有明显的旋转不变特性和光照不变性。这就使得跟踪丢失后可以恢复,增强了地图的重用性。
- 一种新的基于模型选择的自动和鲁棒的初始化程序,其允许创建平面和非平面场景的初始建图
- 提出了一种用来选择地图点和关键帧的“适者生存”方法:生成时放宽要求但剔除时提高要求。这种策略可以剔除冗余的关键帧,从而增强追踪的鲁棒性以及长时间运行的能力。
我们使用公共数据集对算法在室内和室外环境下的性能进行了评估,这些环境包括手持设备、汽车和机器人。值得一提的是,我们的方法与目前最优秀的直接SLAM算法[10]相比(直接SLAM算法直接通过对像素点的灰度进行优化而不是最小化特征重投影误差)能够实现更准确的摄像头定位。我们在文章的第IX-B部分还讨论了基于特征的SLAM算法比直接法更准确的可能原因。
闭环检测和重定位的方法是基于我们之前的工作[11]。系统最初的版本是论文[12]。本文中我们添加了初始化的方法、Essential graph、并完善了其他所有方法。我们详细描述了系统的各个板块,并且进行了详尽的实验验证。
据我们所知,这是目前最完整最可靠的单目SLAM系统。为了造福,我们将源代码开放。视频演示和源代码放在我们的项目网站上。
II. 相关工作
A、位置识别
Williams等人在综述[13]中比较了几种用于位置识别的方法,得出的结论是:基于外观的技术(即图像到图像的匹配)在大环境下大概率比地图到地图或图像到地图的方法更准确。在基于外观匹配的方法中,bags of words(词袋)[14]技术(比如概率方法FAB-MAP[15]算法)以高效率脱颖而出。DBoW2方法[5]首次使用了从BRIEF描述子[16]和非常高效的FAST特征检测算法[17]中获得的二进制词袋。与迄今为止用于词袋方法中的SURF[18]和SIFT[19]特征相比,这将特征提取所需的时间减少了一个数量级以上。尽管系统表现的非常高效和鲁棒,但采用不具有旋转不变性和尺度不变性的BRIEF描述子,系统被限制在只能运行在同一平面内(否则会造成尺度变化),闭环检测也只能从相似的视角中进行。在我们之前的工作[11]中,我们提出了一个使用ORB特征的DBoW2词袋位置识别器。ORB特征是具有旋转不变和尺度不变特性(在一定范围内)的二进制特征,因此,用它生成的快速识别器具有较好的视角不变性。我们在四个不同的数据集中证明了识别器的高召回率和鲁棒性:从10K图像数据库中提取一个候选闭环的运算时间少于39毫秒。在本文的工作中,我们提出了一种改进版本的采用共视(covisibility)信息的位置识别方法,其在检索数据库时返回几个假设情况而不仅仅是最好的匹配。
B、地图初始化
单目SLAM系统需要设计专门的程序来生成初始化地图,因为深度信息不可能从仅仅一幅图像中恢复出来。解决该问题的一个方法是一开始跟踪一个已知结构的对象[20]。而在使用滤波的方法中,可以用一个具有高不确定度的逆深度参数[21]来初始化点的深度信息,理想情况下该参数会在后期逐渐收敛到真值。最近Engel提出的半稠密方法[10]中就采用类似的方法将像素的深度信息初始化为一个具有高方差的随机值。
从两个视图的初始化方法要么假设局部场景平面性[4] [22],并使用Faugeras等人的方法[23]从单应矩阵(homography)中恢复相对相机姿态,要么使用Nister[26]的五点算法(该方法存在多解的问题)计算一个建模了平面和一般场景的本质矩阵(essential matrix)[24] [25](并利用本质矩阵恢复相对相机姿态)。这两种位姿重构方法在低视差下都没有很好的约束,如果平面场景内的所有点都过于接近摄像机中心之一,则结果会出现双重歧义[27](twofold ambiguity solution)。另一方面,非平面场景可以通过线性8点算法[2]来计算一个唯一的基础矩阵( fundamental matrix),相机的相对位姿就可以无歧义的恢复出来。
针对这一问题,我们在本文IV提出了一个新的基于模型选择的自动初始化方法,对平面场景算法选择单应性矩阵(homography),而对于非平面场景则选择基础矩阵(fundamental matrix)。模型选择的统计方法可参见Torr等人的论文[28]。基于类似的理论,我们设计了一种启发式初始化算法,算法会考虑到在接近退化情况(比如:平面,近似平面,或是低视差)下选择基础矩阵进行位姿估计存在的风险,则转而选择单应性计算。在平面的情况下,为了保险起见,如果最终解存在双重歧义则会避免进行初始化(因为可能会因为错误选择解而导致算法崩溃)。因此我们会延迟初始化过程,直到所选的模型在明显的视差下产生唯一的解。
C、单目SLAM
单目SLAM最初采用滤波方法[20],[21],[29],[30]来建模。在该类方法中,每一帧都通过滤波器联合估计地图特征位置和相机位姿。这样做带来的问题有:处理没什么新信息的连续帧图像时对计算资源的浪费、线性误差的累积。而另外一种SLAM方法是采用少数筛选过的图像(关键帧)来建图[3] [4]。因为建图不再与帧率相关联,因此基于关键帧的SLAM方法不但节省了计算资源,还可以进行高精度的BA优化。Strasdar等人在论文[31]中证明了在相同的计算成本上基于关键帧的方法比滤波方法结果更精确。
最具代表性的基于关键帧SLAM系统可能是由Klein和Murray等人提出的PTAM算法[4]。它第一次提出将相机追踪track和建图mapping拆分成两个并行的线程运行,并在小环境的实时增强现实应用中取得了成功。随后[32]在原始版本基础上改进了边缘特征,在跟踪过程中增加了旋转估计步骤,实现了更好的重定位方法。PTAM中的地图点通过图像区块与FAST角点匹配,这使得地图点仅适合于特征跟踪而不适合用于位置识别。实际上,PTAM算法并没有进行大闭环检测,其重定位也仅是基于关键帧低分辨率缩略图的相关性进行的,因此视角不变性较差。
Strasdat等人在论文[6]中提出了一个基于GPU实现的大尺度单目SLAM系统,该系统前端采用光流算法,其次用FAST特征匹配和仅运动(motion-only)BA;后端是基于滑动窗口(sliding-window)的BA。闭环检测通过具有相似性约束(7自由度)的位姿图(pose graph)优化来进行,该方法可以矫正在单目SLAM系统中出现的尺度偏移(scale drift)问题。在本文中,我们也将采用这种7自由度的位姿图优化方法,并将其应用到III-D节中定义的Essential Graph中。
Strasdat等人在文献[7]中采用了PTAM的前端,但其跟踪部分仅在一个从共视图(covisibility graph)提取的局部图(local map)中进行。他们提出了一个双窗口优化后端,在内部窗口中连续进行BA,在有限大小的外部窗口中构建位姿图。然而, 只有当外部窗口尺寸足够大到可以包含整个闭环的情况下,闭环检测才能起作用。在我们的系统中,我们采用了Strasdat等人提出的使用基于共视信息的局部地图并通过共视图来构建位姿图的优秀想法,但(实际是)将它们应用在(我们)完全重新设计的前端和后端里。另一个区别是,我们没有使用特别的特征做闭环检测(比如SURF),而是基于与追踪、建图相同的特征进行位置识别,因此获得了鲁棒的帧率重定位和闭环检测。
在Pirker等人的论文[33]中作者提出了CD-SLAM,这是一个非常完善的系统,它包括闭环检测,重定位,大尺度运行以及对动态环境运行所做的工作。但文中并没有提及地图初始化。(而且,)公共接口的缺乏使我们没法对其进行精确性、鲁棒性和大场景下能力的测试对比。
Song等人在论文[34]提出的视觉里程计方法中使用了ORB特征做追踪和一个短暂滑动窗口做BA后端。相比之下,我们的系统更加普适,因为他们(的系统中)没有涉及全局重定位、闭环检测,而且地图也不能复用。他们还使用了相机到地面的真实距离来限制单目SLAM算法的尺度漂移。
Lim等人在我们提交本文最初的版本[12]之后发表了论文[25],他们也采用相同的特征进行跟踪、建图和闭环检测。但是,由于Lim等人的(对不具备尺度不变性)BRIEF描述子的选择,因此其系统运行受限在平面轨迹上。他们的系统仅从最后一帧关键帧跟踪特征点,因此访问过的地图不能复用(与视觉里程计很像),并且存在系统无限增长的问题。我们在VIII-E小节里面将我们的系统与该系统进行了定性比较。
Engel等人在最近的论文[10]里提出了LSD-SLAM,其可以构建大场景的半稠密地图。系统并没有采用使用特征点的BA方法,而是选择直接法(优化也是直接通过图像像素灰度进行)。系统结果让人印象深刻,其在没有GPU加速的情况下可以实时构建一个半稠密地图,相比基于特征点的SLAM系统输出的稀疏地图而言,LSD-SLAM方法在机器人领域有更大的应用潜力。然而,该系统的运行依旧需要基于特征点做闭环检测,且相机定位的精度也明显低于PTAM和我们的系统。相关实验结果我们将在VIII-B小节中展示,这一令人惊讶的结果在IX-B小节进行讨论。
Forster等人在论文[22]中提出了介于直接法和基于特征点法之间的半直接视觉里程计SVO。该系统不需要对每帧图像都提取特征点,且可以以很高的帧率运行,在四轴飞行器上取得了令人印象深刻的效果。然而,SVO没有进行闭环检测,且目前的实现主要基于下视摄像头运行。
最后,我们想讨论一下关键帧的选择。所有的视觉SLAM工作相关的文献都同意选择所有的地图点和图像帧运行一起进行BA是不可行的。因此在论文[31]中,Strasdat等人证明性价比最高(cost-effective)的方法是保留尽可能多的(地图)点,同时只保留非冗余关键帧。PTAM通过非常谨慎的插入关键帧以避免运算复杂度过度增长。然而,这种严格限制关键帧插入的策略可能会在困难的探索条件下导致追踪失败。我们的适者生存(survival of the fifittest)策略是在困难的场景下尽快插入关键帧,并稍后删除冗余的关键帧以避免额外的(计算)成本,这实现了前所未有的鲁棒性。
III. 系统架构
A、特征选择
我们系统的主要设计思想是将SLAM系统的建图、跟踪使用的相同特征应用在位置识别以达到(高)帧率(级别的)重定位和闭环检测。这就避免了像之前一些工作[6] [7] 需要额外插入一些识别性强的特征(recognition features)(以用于后期的闭环检测),因此我们的系统更有效率。我们每张图像的特征提取时间要远少于33毫秒,这就排除了流行的SIFT(300ms)[19]、SURF(300ms)[18],或者是最近的A-KAZE(~100ms)[35]。为了获得更广泛的位置识别能力,我们需要(提取的特征具备)旋转不变性,这就排除了BRIEF[16]和LDB[36]。
我们选择了ORB[9]特征,它是关联了256位描述子(descriptor)的带方向(oriented)的多尺度(multi-scale)FAST角点。它们的计算和匹配速度非常快,同时对视角具有很好的旋转不变性(invariance)。这允许在更宽的基准线上匹配它们,所以提高了BA的精度。我们已经在论文[11]中证明了基于ORB特征的位置识别良好的性能。虽然本文的方案中采用ORB特征,但所提出的技术并不仅限于该特征。
B、三个线程:追踪、局部建图和闭环检测
我们的系统框架如图1所示,包括三个并行的线程:跟踪、局部建图和闭环检测。跟踪线程负责对每帧图像的相机进行定位,并决定什么时候插入新的关键帧。我们首先执行与前一帧图像的初始特征匹配,然后采用仅运动(motion-only)BA优化位姿。如果跟踪丢失(比如由于遮挡或是突然运动),则由位置识别模块进行全局重定位。一旦获得最初的相机位姿估计和特征匹配,则使用由系统维护的关键帧的共视图(covisibility graph)提取一个局部可视化地图,如图2(a),图2(b)所示。然后通过重投影方法搜索(当前帧)与局部地图点对应的匹配,并利用所有的匹配点再次优化当前相机位姿。最后,跟踪线程决定是否(将当前帧)插入新的关键帧。所有的跟踪步骤将在V小节详细阐述。创建初始化地图的新程序将在IV小节进行介绍。
局部建图线程负责处理新的关键帧,使用局部BA对相机位姿周围环境进行优化重构。在共视图(covisibility graph)已连接的所有关键帧中搜索新关键帧中未匹配的ORB特征新对应项,然后将之三角化为新的地图点。在创建后的一段时间内,根据跟踪过程中收集的信息,会应用严格苛刻的点剔除策略,以只保留高质量的点。局部建图线程也负责剔除(culling)冗余的关键帧。我们将在VI节详细说明局部建图的步骤。
闭环检测线程负责对每个新的关键帧进行闭环搜索(以确认是否形成闭环)。如果检测到了闭环,我们就计算一个通知闭环累积误差的相似变换。然后将闭环的两侧对齐,并将重复的点融合。最后,执行基于相似性约束[6]的位姿图优化以实现全局一致性。主要创新点在于,我们执行了基于Essential Graph的优化,它是一个基于共视图covisibility graph的一个稀疏子图,更多细节将在III-D小节解释。VII节详细解释了闭环检测和校正步骤的细节。
我们使用在g2o[37]中实现的列文伯格-马夸尔特法(Levenberg-Marquardt)算法来进行所有的优化。在附录中,我们描述了每个优化中涉及的误差项、误差函数和变量。
C、地图点、关键帧及其选择标准
每个地图点 \(P_{i}\) 存储以下信息:
- 它在世界坐标系中的3D坐标 \(X_{w,i}\)
- 平均观测方向 \(n_{i}\),即该点所有观测方向(该方向是指连接该点和其对应观测关键帧光心的射线方向)的平均单位向量
- 一个具有代表性的ORB特征描述子 \(D_{i}\),它是(与该地图点)关联的ORB描述子,其汉明距离相对于能观察到该点的关键帧中的所有其他关联的描述子都最小
- 可观测到该点的最大 \(d_{max}\) 和最小 \(d_{min}\) 观测距离,根据ORB特征的尺度不变性限制(得出的)
每个关键帧 \(K_{i}\) 存储以下信息:
- 相机位姿 \(T_{i,w}\),这是一个将点从世界坐标系转换到相机坐标系下的刚体变换(矩阵)
- 相机内参,包括主点(principal point)和焦距(focal length)
- 从图像帧提取的所有ORB特征点,不管其是否已经关联了地图点。如果提供了畸变模型(distortion model)(参数),则其坐标将不会失真(undistorted)
地图点云和关键帧的创建策略较为宽松,但是之后则会通过一个非常严格苛刻的删选机制进行挑选,该机制会检测出冗余的关键帧和匹配错误的或不可跟踪的地图点然后进行删除。这样做的好处在于探索时,地图在扩建过程中具有一定的弹性,使得系统在外界条件比较恶劣的情况下(比如:旋转,相机快速运动)仍然可以实现鲁棒的跟踪。而(与此同时)当相机对同一个环境不断重新访问时,地图的大小是可控的,这就利于该系统的长期工作。另外,与PTAM算法相比我们构建的地图中包含极少的异常值(outliers),但付出的代价就是点的总数更少。地图点和关键帧的剔除程序分别将在VI-B节和VI-E节解释。
D、共视图和本质图
关键帧之间的共视(covisibility)信息在我们系统中几个模块上都非常有用,我们像论文[7]一样将其表示成一个无向加权图(undirected weighted graph)。(共视)图中每个节点代表一个关键帧,如果两个关键帧能同时观测到至少15个地图点,则这两个节点之间用一条边相连,两个关键帧能共同观测到的地图点数量作为这条边的权重θ。
为了矫正闭环回路,我们像论文[6]那样做位姿图优化,优化方法是延着位姿图将闭环回路的误差进行传播。考虑到共视图(covisibility graph)提供的所有边可能非常密集,我们提出构建一个保留了所有节点(关键帧)但边数却更少的本质图(Essential Graph)。本质图仍旧保持了一个强大的网络以获得精确的结果。系统从初始关键帧开始增量式地构建一个生成树,该树提供了一个边数量最少的共视图(covisibility graph)的连接子图。当插入新的关键帧时,将该关键帧与树上与其有最多共同观测点的关键帧相链接;反之,当一个关键帧通过剔除策略被删除时,系统将更新受该关键帧影响的链接。本质图(Essential Graph)包含了生成树(spanning tree)、具有高共视信息(covisibility)(即 \(θ_{min}=100\))共视图的边的子集、以及闭环回路的边,(这样的组合共同)构建了一个强大的相机网络。图2展示了一个共视图、生成树和相关的本质图的例子。在本文VIII-E节的实验里,当(系统)运行位姿图优化时,结果就可以达到非常高的精度以至于(再运行)额外的全局BA优化几乎无法提升其精度。本质图的效率和 \(θ_{min}\) (对系统)的影响将在VIII-E节的结尾部分讨论。
E、基于词袋模型的位置识别
系统嵌入了基于DBoW2[5]算法来执行闭环检测和重定位的模块。视觉单词(visual words)是一个离散化的特征描述子空间,被称为视觉词典(visual vocabulary)。视觉词典是通过大量图像中提取ORB描述子离线创建的。正如我们之前的论文[11]所展现的:如果图像的通用性足够强,则相同的视觉词典在不同的环境下也能获得很好的性能。SLAM系统增量地构建一个包含倒置索引的数据库,用于存储视觉词典里的每个视觉单词以及它们被哪些关键帧观测到,因此数据库的检索效率非常高。当一个关键帧通过剔除程序被删除时,数据库也会进行相应更新。
由于关键帧之间可能会存在视图上的重叠,因此检索数据库时,可能返回不止一个高分值的关键帧结果。原版的DBoW2考虑到了(视图)重叠,(然后)提高了时间上更近的图像的分值。这里的局限性(limitation)在于:没有包括那些观测了同一地点但在不同时间插入的关键帧。为了解决这一问题,我们将这些在共视图中相连的关键帧进行分组。另外,我们的数据库返回分值高于最好分值75%以上的所有关键帧。
用词袋模型来表示特征匹配的另外一个优势在论文[5]里有详细介绍。当我们想计算两组ORB特征的对应关系时,我们可以强制限制在视觉词典树上某一层(我们在6个里面选第2个?)的相同节点里进行暴力匹配,(以此)加快搜索速度。我们将这个技巧应用到 三角化新的(地图)点,闭环检测和重定位 的搜索匹配中。我们还通过方向一致性测试来改进对应性,这可以去除异常值,确保所有对应(匹配点的)旋转方向一致,具体请看论文[11]。
IV. 地图自动初始化
略
V. 跟踪
在这一部分,我们将详细介绍跟踪线程在每一帧相机图像上执行的步骤。在几个步骤中都提到的相机位姿优化,包括仅运动(motion-only)BA,将在附录部分进行阐述。
A、ORB特征提取
我们在8层图像金字塔上提取FAST角点,图像金字塔尺度因子为1.2。我们发现:图像分辨率从512×384到752×480之间,提取1000个角点比较合适。对于更高的分辨率,如KITTI数据集[40]中提供的1241×376分辨率,则提取2000个角点。为了确保特征点均匀分布,我们将每层图像分成网格(grid),每个单元格(cell)提取至少5个角点。然后检测每个单元格(cell)内的角点,如果(某个单元格)角点数量不够,就调整检测器的阈值(再检测一次)。如果某些单元格内检测不出角点(无纹理或者低对比度),则每个单元格保留的角点数量也相应调整。最后,根据保留的FAST角点计算其方向和ORB特征描述子。ORB特征描述子将用于系统后续所有的特征匹配,而不是像PTAM那样根据图像区块相关性(patch correlation)进行搜索。
B、通过前一帧估计初始位姿
如果上一帧图像跟踪成功,我们就使用恒速运动(constant velocity motion)模型来预测相机的位姿(即认为摄像头处于匀速运动),然后对在最后一帧中观察到的地图点进行引导搜索。如果没有找到足够多的匹配点(比如明显违反了运动模型(的规律),即非匀速运动),我们就加大以上一帧中地图点位置(为中心的)搜索范围。然后根据找到的匹配关系对位姿进行优化。
C、通过全局重定位来初始化位姿
如果(扩大了搜索范围还是)跟踪失败,则将当前帧转化为词袋(BoW)向量,并查询识别数据库中用于全局重定位的候选关键帧。然后如III-E节所描述的那样,计算每个候选关键帧中与地图点相对应的ORB特征对应关系。接着,对每个候选关键帧执行RANSAC迭代,并使用PnP算法尝试计算当前帧的位姿。如果我们找到一个具有足够多的有效点(inliers)的位姿,那么优化该位姿,并执行和候选关键帧的地图点进行更多匹配的引导搜索。最后,基于找到的所有匹配点对相机位姿进一步优化,如果有效点(inliers)足够多,则跟踪程序将持续执行。
D、跟踪局部地图
一旦我们有了对相机姿态的估计和一组初始特征匹配,我们就可以将地图投影到帧上,并搜索更多的地图点对应关系。为了降低大地图的复杂性,我们只投影一个局部地图。该局部地图包含一组关键帧 \(K1\) (它们和当前帧有共同的地图点),还包括与关键帧 \(K1\) 在共视图中相邻的一组关键帧 \(K2\) 。这个局部地图中有一个参考关键帧\(K_{ref} ∈ K1\),它与当前帧具有最多共同的地图点。现在对 \(K1\) , \(K2\) 中观测到的每个地图点,在当前帧中进行如下搜索:
- 计算该地图点在当前帧中的投影点 \(x\) 。如果投影位置超出图像边缘,就将该地图点删除
- 计算当前地图点观测方向 \(v\) 和地图点平均观测方向 \(n\) 的夹角。如果 \(v\cdot n< \cos(60°)\) ,就将该地图点删除
- 计算从地图点到相机光心的距离 \(d\) 。如果它不在地图点的尺度不变区间内,即 \(d∉[d_{min},d_{max}]\),就将该地图点删除
- 按照 \(d/d_{min}\) 计算当前帧的尺度
- 将地图点的代表特征描述子 \(D\) 和当前帧(位于预测的尺度层和靠近 \(x\) )的未匹配ORB特征进行比较,做出最优匹配
相机位姿最后通过当前帧中获得所有的地图点进行优化。
补充:这个环节的目的是在当前帧和局部地图之间找到更多的匹配点对,来优化当前帧的位姿
E、新关键帧的判断
最后一步是决定当前帧是否可以作为新的关键帧。由于局部建图的过程中有一个剔除冗余关键帧的机制,所以我们将尝试尽可能快的插入新的关键帧以保证跟踪线程对具有挑战性的相机运动更具鲁棒性(尤其是对旋转运动)。要插入一个新的关键帧,必须满足以下所有条件:
距离上一次全局重定位后已超过20帧
局部建图处于空闲状态,或距上一个关键帧插入后已超过20帧
当前帧跟踪至少有50个地图点
追踪中,当前帧与参考关键帧 \(K_{ref}\) 同时可观测到的(地图)点少于90%
与PTAM中用关键帧之间的距离作为判断标准不同,我们加入一个最小的视图变换(即条件4)。条件1确保好的重定位,条件3确保好的跟踪。如果局部建图处于工作(busy)状态(即条件2的后半部分)的时候插入关键帧,则发送信号去暂停局部BA,以便尽可能快处理新的关键帧。
VI. 局部建图
这章我们将描述根据每个新的关键帧 \(K_{i}\) 构建局部地图的步骤。
A、关键帧插入
首先,我们更新共视图:为新关键帧 \(K_{i}\) 添加一个新节点,并更新因与其他关键帧有共同地图点(大于15个)产生的边。然后,我们更新与 \(K_{i}\) 具有最多共同(地图)点的关键帧的生成树的链接。最后,我们计算该关键帧的词带表示,这将有助于通过数据关联来三角化新的(地图)点。
B、最近的地图点剔除
地图点要想保留在地图中,必须在其创建后的前三个关键帧中通过一个严格的测试。该测试确保(留下的点都)能被跟踪,即这些点不是错误三角化(生成)的(比如由于错误的数据关联)。一个(地图)点必须满足下面两个条件:
- 跟踪线程在预测该(地图)点可见的所有帧中找到该点的帧数占总帧数的25%以上
- 创建地图点之后,接下来应该至少有3个关键帧可以观察到这个地图点
一旦一个地图点通过测试后,如果在任何时刻不能从三个及以上关键帧中观察到它,那么该地图点就会被剔除。当关键帧被剔除和局部BA丢弃异值点时,可能会发生这种情况。这个策略使得我们的地图包含很少的异常值(outliers)。
C、新地图点创建
新地图点的创建是通过对共视图(covisibility graph)中连接的关键帧 \(K_{c}\) 中的ORB(特征点)进行三角化实现的。对 \(K_{i}\) 中每个未匹配的ORB(特征点),我们在其他关键帧的未匹配点中进行查找(看是否有匹配上的特征点)。这个匹配过程在III-E节中有阐述,然后将剔除那些不满足对极约束的匹配项。ORB(特征点)对三角化后,为了将其接受为新(地图)点,该点要进行位置是否在相机前方、重投影误差(小)以及尺度一致性方面的检查。起初,(虽然)一个地图点被两个关键帧观测到,但它可以与其他关键帧中进行搜索匹配,所以它可以投影到其他相连的关键帧中。搜索算法的细节在本文V-D节中有讲述。
D、局部BA
局部BA针对当前处理的关键帧 \(K_{i}\) 、所有在共视图中与 \(K_{i}\) 连接的关键帧 \(K_{c}\) 以及这些被关键帧观测的所有地图点进行优化。所有其他可以观测到这些(地图)点但(在共视图中)与 \(K_{i}\) 没有连接的关键帧也包含在优化中(作为约束条件),但保持不变。优化过程中或者优化结束后,被标记为异常点的观测值会被剔除。有关此优化的更多细节,请参见附录。
E、局部关键帧筛选
为了使重建保持简洁,局部建图尽量检测冗余的关键帧然后剔除它们。这样会有很大好处,因为随着关键帧数量的增加,BA优化的复杂度也随之增加。当系统在同一场景下运行时,除非场景有所变化,否则关键帧的数量不会无限增长。这样一来,就增强了系统在相同环境下的可持续运行性。如果关键帧 \(K_{c}\) 中90%的(地图)点都可以被其他至少三个关键帧在相同或更精细的尺度下同时观测到(那认为 \(K_{c}\) 是冗余的),我们就将其剔除。尺度条件确保了地图点维护的关键帧的观测它们的尺度是最精细的。这个策略受Tan等人的工作[24]的启发,在这项工作中,关键帧在经过一个变换检测的过程后被丢弃。
VII. 闭环检测
当局部建图线程处理完一个关键帧 \(K_{i}\) 之后,闭环检测线程会尝试利用 \(K_{i}\) 做回环检测。具体步骤如下。
A、候选回环检测
我们先计算 \(K_{i}\) 和它在共视图中相邻帧(\(θ_{min}=30\))的的词袋向量相似度,保留最低分值 \(s_{min}\) 。然后,我们检索识别数据库,丢掉那些分值低于 \(s_{min}\) 的关键帧。这和DBoW2中均值化分值的操作类似,可以获得好的鲁棒性,DBoW2中计算的是前一帧图像,而我们是使用的共视(covisibility)信息。另外,所有直接连接到 \(K_{i}\) 的关键帧都会从结果中删除。为了获得候选回环,我们必须连续检测到3个一致的候选回环(在共视图中相连的关键帧)。如果有几个与 \(K_{i}\) 相似的地方,就可能会有几个候选回环。
B、计算相似变换
单目SLAM系统有7个自由度可能产生漂移(drift),3个平移,3个旋转和1个尺度因子 [6]。因此,为了闭合回环,我们需要计算从当前关键帧 \(K_{i}\) 到回环关键帧 \(K_{l}\) 的相似变换,以获得回环的累积误差。计算相似变换也可以作为回环的几何验证。
我们首先按照III-E节中解释的过程,计算与 \(K_{i}\) 中ORB相关联的地图点和候选回环关键帧之间的对应关系。此时,对每个候选回环,我们有了一个3D到3D的对应关系。或者,我们对每个候选(回环关键帧)执行RANSAC迭代,通过Horn方法[42]找到相似变换。如果我们用足够的有效数据(inliers)找到了相似变换 \(S_{il}\) ,我们就可以优化它,并执行一个引导搜索(匹配)更多的对应关系。再此进行优化,如果 \(S_{il}\) 有足够多的有效数据支持时, \(K_{l}\) 回环就被接受。
C、回环融合
回环矫正的第一步是融合重复的地图点,在共视图中插入与回环相关的的新边。首先,通过相似变换 \(S_{il}\) 矫正当前关键帧位姿 \(T_{iw}\) ,并将该矫正传播到所有与 \(K_{i}\) 相邻的关键帧,连接转换,以便使回环的两侧进行对齐。然后,回环关键帧及其相邻(的关键帧)能观测到的所有地图点都投影到 \(K_{i}\) 以及与 \(K_{i}\) 相邻(的关键帧)上,并在投影的区域附近小范围内搜索它的对应匹配点,就像V-D节所做的那样。所有匹配的地图点和计算 \(S_{il}\) 过程中的有效数据(inliers)都被融合。融合过程中涉及的所有关键帧将会更新它们在共视图中的边,创建的新边将用于闭合回环。
D、本质图优化
如III-D节所示,为了有效地闭合回环,我们通过本质图(Essential Graph)优化位姿图,这样可以将回环闭合的误差通过(本质)图传播。对相似性变换进行优化,以校正尺度漂移[6]。误差条件和损失函数详见附录。优化过后,每个地图点都根据可以观测到它的一个关键帧的校正进行变换。
VIII. 实验
略
IX. 结论和讨论
A、结论
略
B、稀疏/特征点 vs. 稠密/直接法
略
C、未来工作
我们系统的精度可以通过结合无限远点跟踪来进一步增强。这些没有足够视差的点,并没有包含在本文系统构建的地图中,但它们提供了相机旋转的信息[21]。
另一条路是将稀疏地图升级到一个更加稠密、对重建更实用的地图。由于我们关键帧的选择机制,关键帧包括一个紧凑的环境总结与非常高的姿态精度和丰富的共视信息。所以,ORB-SLAM稀疏地图是一个非常优秀的初始估计框架,可以在此基础上构建一个稠密而准确的场景地图。本条路第一个尝试工作在[47]中展示。
附录:非线性优化
略
参考文献
略
PDF原文
参考链接:
- https://blog.csdn.net/weixin_42905141/article/details/102857958
- https://zhehangt.github.io/2017/04/20/SLAM/ORBSLAM/ORBSLAMPaper/