视觉SLAM特征点法开源简介：PTAM、ORB-SLAM

2020 年 3 月 22 日 计算机视觉life

点击上方“计算机视觉life”，选择“星标”

快速获得最新干货

前面的话

VSLAM 是利用多视图几何理论，根据相机拍摄的图像信息对相机进行定位并同时构建周围环境地图。VSLAM 前端为视觉里程计和回环检测，相当于是对图像数据进行关联；后端是对前端输出的结果进行优化，利用滤波或非线性优化理论，得到最优的位姿估计和全局一致性地图。

前面已经介绍了系列一：VSLAM的前端：视觉里程计和回环检测，和系列二：VSLAM中的后端优化和建图。

按照特征法和直接法的分类，各种 VSLAM 具备不同的处理速度、轨迹精度等指标。随着 VSLAM 的研究如火如荼地开展，许多研究者发表研究成果以及公开相关代码，供学者学习与研究。

这次，我们将详细介绍视觉SLAM中特征点法的经典开源算法。

3 特征点法开源算法

3.1 PTAM

PTAM是 2007 年由牛津大学主动视觉实验室的 Georg Klein 和 David Murray 提出的。当时给研究者们带来了极大震撼，它有如下创新点：

a) PTAM 第一个使用非线性优化。之前人们未认识到后端优化的稀疏性，所以觉得优化后端无法实时处理那样大规模的数据，主流的 SLAM 均采用 EKF 滤波器等滤波方法。而 PTAM 则是一个显著的反例，将 VSLAM 研究逐渐转向了以非线性优化为主导的后端。

b) PTAM 引入了关键帧机制。不必精细地处理每一幅图像，而仅仅处理较少的关键帧图像，然后优化其轨迹和地图。

c) PTAM 引入了多线程机制。将跟踪和建图过程分开。因为跟踪部分需要实时响应图像数据，而地图则没必要实时地优化，只需在后台进行处理。这是 VSLAM 中首次区分出前后端的概念，初步确定了 VSLAM 的框架。

3.1.1 主要结构

PTAM 主要分为跟踪、建图两部分。

PTAM 的跟踪分为粗阶段和精阶段。

在粗阶段中选用图像金字塔最高层的 50 个特征点，利用恒速模型和扩大范围搜索，从这些测量中得出一个新姿态；再将近千个特征点重新投影到图像中，执行更严格的块搜索（FAST 特征的局部 8*8的方块构成 patch 作为描述符），并构建重投影误差得到最优的相机姿态。

地图构建主要是建立三维地图点的过程。它分为地图的初始化和地图的更新。

首先，系统初始化时使用三角测量构建初始地图；在此之后，随着添加新的关键帧，地图将不断地进行细化和扩展。

具体为：系统初始化时，根据前两个关键帧提供的特征对应关系，采用 5 点算法和随机采样一致（RANSAC）估计本质矩阵（或使用平面情况的单应性分解）并三角化得到初始地图。然后当插入关键帧时，使用极线搜索和块匹配（零均值距离平方和 ZMSSD）计算得到精确匹配，从而精细化地图。

PTAM 系统框图如下图所示。

3.1.2 优缺点

PTAM 不仅仅是 VSLAM 的程序，还将相机的标定和增强现实（AR）都包括进来，而且试图在手机上实现，从另外的角度也可以说它是面向小场景的一个增强现实软件。

PTAM 的最开始的版本是建议采用 5 点算法分解本质矩阵得到相机姿态，该方法用于非平面场景的初始化。后来 PTAM的初始化改变为使用单应性，其中场景假定为 2D 平面。

以现在的知识来看， PTAM 的 DEMO 可能有点过时，比如它的初始化需要用户的输入来捕捉地图中的前两个关键帧，而且它要求用户在第一与第二关键帧之间采取平行于观察场景的缓慢和平滑的平移运动。因为它采用的 2D-2D 的图像匹配算法为不考虑特征仿射变换的 ZMSSD 算法，所以容易受到运动模糊和相机旋转的影响。

PTAM 是为小场景 AR 设计的，没考虑全局的回环，而且存在明显的缺陷：场景小（实际情况是 6 000 个点和 150个关键帧）、跟踪容易丢失等，但是在当时确实是一个里程碑的标志。

3.2 ORB-SLAM

ORBSLAM由西班牙 Raul 博士于 2015 年公布，其论文发表在《IEEE Transactions on Robotics》。到目前为止，ORBSLAM 是最完整的基于特征点法 VSLAM，它可以看做是 PTAM 的一个延伸，相比 PTAM， ORBSLAM 增加了一个回环检测（loop closing）的线程。

该系统框架包括跟踪、建图、闭环三个线程，均基于 ORB 特征实现（ORB特征在之前系列文章中有介绍，可以参考系列一），所有优化环节都通过优化框架 G2O 实现。

ORBSLAM 有如下创新点：

初始化采用自动机制，不需要手工输入，也不需要假设场景是否为平面。通过匹配 ORB 特征同时计算单应性和基础矩阵并评分，选用分数高的方案。

将改进后的 ORB 特征贯穿整个工程始终，包括特征检测、匹配以及用于闭环的词袋模型（bag-of-words， BoW）。

使用 DBOW 模块，不只是用于 loop closing 时的检测，而且用于系统的重定位。 更大的意义是在图像帧间匹配时，使用词典对描述子进行分类的结果进行比对， 这种方法不仅有效，还可以大大简化运算。

后端优化是亮点， ORB 在每一层估计中都大量采用G2O 优化，不仅有单帧位姿估计到局部地图的位姿估计，而且有局部地图点与位姿联合估计，还有利用回环结果的全局位姿估计。

3.2.1 具体流程

ORBSLAM 的具体流程为：

跟踪。跟踪线程主要是得到相机位姿和关键帧。

具体为：先对图像进行 ORB 特征提取和匹配，系统初始化得到 R，t 和 3D 点云（如果系统未初始化）；然后采用参考关键帧模型或运动模型和 BoW 模块加速匹配（如果跟踪失败也是将当前帧和所有关键帧通过 BoW 加速匹配），再构建局部小图和重投影误差优化函数；最后得到优化位姿和关键帧。

建图。建图线程主要是更新 3D 点和插入关键帧。

具体为：取出一个关键帧，计算特征点的 BoW 关系，更新关键帧间的连接关系，将关键帧插入地图，验证加入的地图点，利用三角法生成新的地图点，对相邻关键帧和对应的 3D 点进行局部 BA，剔除冗余关键帧，将关键帧加入闭环；

闭环。闭环线程主要是纠正尺度漂移和全局优化。

具体为：取出一个关键帧，计算当前关键帧与每个共视关键帧的 BoW 得分，在所有关键帧中找出闭环备选帧，通过连续性检测验证候选帧，去做 sim3 优化（纠正尺度漂移，使其尺度一致），利用优化结果寻找更多的特征匹配，再做一遍优化，如果内点足够，接收这个闭环，最后固定回环帧和当前帧再做全局优化。

更加具体，细节化来说，可以是：

1、地图自动初始化：需要估计两帧间的相机运动，并利用三角化估计路标点的3D位置，最后构建初始地图。其具体步骤：

①ORB特征点提取与匹配，ORB特征点具有良好的旋转和缩放不变性，以及计算效率；

②随机一致性检验 RANSAC，同时通过 4 个匹配点计算H单应矩阵，8个匹配点计算F 基础矩阵。分别计算两个模型评分。选择F本质矩阵和H单应矩阵在RANSAC过程中同时取得最好评分的情况，否则，返回①；

③F或者 H模型的选择。

1)平面场景或低视差的情况，应该选择 H单应矩阵。在这两种情形，F计算误差大，三角化误差也大。因为低时差情况，三角化误差大，所以不进行地图初始化；

2)具有足够视差的非平面场景，应该选择F基础矩阵，因为H的计算本身存在误差；

④运动估计和三角化。模型选择完之后，通过分解H单应矩阵或者F本质矩阵使用SVD矩阵分解估计相机运动 R,t。并利用三角化计算匹配点的 3D 坐标；

⑤使用 local BA 图优化同时优化位姿和 3D 点。

2、跟踪模块：主要包括了当前帧位姿估计，局部地图跟踪和判断新的关键帧几个部分。

①从上一帧初始化位姿估计：如果上一帧跟踪成功，则用相同的运动模型来预测当前帧的位姿。将上一帧中地图点投影到当前帧，在小范围内进行特征匹配。若有足够多的匹配点，则直接PnP估计位姿；否则，加大在当前帧的搜索范围进行特征匹配，利用足够多的匹配点，通过PnP估计位姿；

②从全局定位初始化估计位姿：跟踪失败，执行全局重定位。利用 BoW 寻找匹配关键帧，运用 PnP 计算当前帧与各关键帧转移矩阵，构建位姿图，利用 g2o 优化获得当前帧位姿；

③局部地图跟踪：利用多视图进一步优化当前帧，将局部地图中的 3D点投影到当前帧，并找到相应的匹配点。通过最小化匹配点的重投影误差来进一步优化当前帧的位姿；

④宽松选择关键帧。

关键帧插入需要同时满足以下四点要求：

距离上一次全局重定位已经超过20帧(保证良好的重定位)
局部建图进程闲置，或者距离上一次关键帧插入已经超过20帧(会阻断局部BA)
当前帧至少跟踪50个地图点(保证当前是良好的跟踪)
当前帧跟踪的点数比参考帧跟踪点数的90%还要少(保证有较大的视觉变化)

3、 局部地图构建模块：创建新的路标点以及剔除冗余的关键帧。

①插入新的关键帧；

②三角化新的地图点，利用local BA 优化局部地图中关键帧的位姿和地图点3D位置；

③严格剔除冗余的关键帧和 3D 点，如果关键帧中 90%以上的地图点能被其它至少3 个共视关键帧观测到，则剔除该关键帧。

该模块由于计算量较大导致耗时多，作为一个独立的后台线程。但是新的地图点会影响到下一时刻图像帧是否能够跟踪成功，因此不能够及时创建新的地图点是导致ORB-SLAM2 算法跟丢的原因之一。可以将三角化新的地图点放到能够实时计算的跟踪模块，来解决这一不足。

4、回环检测模块：建立当前帧与过去关键帧之间的关联，减少长时间的累积误差，并通过相似变换矫正尺度偏移。

①利用基于 Bo W 的场景识别的方法检测回环，并计算Sim3相似变换，构建优化图；

②基于Sim3 变换，在本质视图上全局优化位姿图。

3.2.2 具体线程

ORB-SLAM算法和PTAM具有相同的算法框架，采用多线程构架，四个主线程：前端位姿跟踪、局部地图构建与优化、闭环检测与优化、显示与交互。

1、前端位姿跟踪线程采用恒速模型，并通过优化重投影误差优化位姿。

2、局部地图线程通过MapPoints维护关键帧之间的共视关系，通过局部BA优化共视关键帧位姿和MapPoints。

3、闭环检测线程通过bag-of-words加速闭环匹配帧的筛选，并通过Sim3优化尺度，通过全局BA优化Essential Graph和MapPoints。

4、使用bag-of-words加速匹配帧的筛选，并使用EPnP算法完成重定位中的位姿估计。

3.2.3 有关知识点

共视图和本质图

作者通过构造 共视图 存储各个关键帧之间的关系，以便在跟踪、重定位、局部建图、回环检测等多个地方发挥其作用。

那么这究竟是个什么结构呢？其实就是一个无向加权图，以关键帧为节点，关键帧之间如果有共同观测的地图点则产生边，共同观测的数量就是边的权重。

这么说可能还是有点抽象，那就以下面这张图为例子吧。

假设有六个关键帧(红黄绿蓝紫黑)，对应颜色框里的地图点为各自观测到的点，可以看出红黄两帧共同观测到的地图点个数为5，红绿共同观测到1个，黄绿共视个数为3，以此类推。

于是就可以得到右边的图结构，红黄之间边的权重为5，红绿权重为1，黄绿权重为3等等。

这有什么作用呢？直观来说，只要揪到一个，就可以把同伙都一网打尽！

比如，在重定位的时候需要在KeyFrameDatabase中搜索候选关键帧，由于关键帧之间是有重叠视图的，所以在得到匹配的候选关键帧之后，继续把每一个候选关键帧在共视图中的所有“同伙”揪出来，从他们之中重新挑选分数最高的作为最新候选关键帧，同时把他们的分数求和作为新候选帧的分数。

最后，返回分数高于新候选关键帧中最高分75(>0.75*bestScore)的所有候选帧，用于接下来的重定位匹配等操作。

那么本质图就是浓缩版的共视图，因为它是在共视图的基础上，只保留权重比较大(>100)的边，所以本质图意味着其中的关键帧共视程度较高。

它主要用在回环检测中，当检测到回环时，利用本质图对相似变换Sim3进行位姿图优化。

下图展示了共视图和本质图的区别所在，边的减少可以降低位姿图优化的负担。

论文中给出的可视化结果图对比就更明显了。

花式Bundle Adjustment

细心的你可能已经发现前面提到了三种BA方式，分别是full BA、motion-only BA、local BA。这么多花式的BA，究竟有什么不一样？

Full BA主要是在初始化时用的，它把初始化生成的所有地图点和对应的图像帧都纳入优化变量，好好调优一番。毕竟开局小地图，后面全靠补，一开始必须要稳！

Motion-only BA就是只对位姿进行优化，在跟踪进程中每次估计相机位姿或者重定位时计算出相机位姿后都会对位姿进行优化，而不考虑地图点。

而Local BA则是优化局部的位姿和地图点，在局部建图进程中把当前关键帧和他的“同伙们”以及他们对应的地图点当作优化变量，其余的关键帧则保持不变。

网上有大佬把他们画成了简单的图，更直观地解释了其中关系。图中红色的代表待优化变量，灰色的则表示保持不变。

3.2.4 优缺点

ORBSLAM 在工程上是非常完整的 SLAM 系统，里面涉及的很多参数都是通过计算得出，后续有大量的学者在其基础上改进。

随后 Raul 在前面的基础上利用宽基线做了更加精密的半稠密地图构建的工作， 2017 年又将 IMU 融入到ORBSLAM 中，由此可见ORBSLAM 的可扩展性很好。

当然与许多其他的基于特征点的 SLAM 系统一样，有很多自身的缺陷：因为特征点的原因，只能得到稀疏点云地图，这对机器人下一步的导航应用会造成很大困难，而且它不易作为环境地图的描述，也很难构建高层次地图（语义地图等），给环境的语义构建带来了诸多不便。

结语

特征点法中的特征一般为人类根据图像的一些特性，自己设计的算法，可能失去了大自然的本质和意义；在特征点法中绝大多数时间都耗费在特征的提取和匹配上，特征点法的瓶颈在于如何设计和提取更好的特征点；相比之下，直接法不依赖特征的提取和匹配，直接通过两帧之间的像素灰度值构建光度误差来求解相机运动。因此直接法可以在特征缺失的场合下使用。

该篇作为SLAM系列的第三篇，主要介绍了特征点法开源算法——PTAM和SLAM。接下来，SLAM系列文章会持续更新，会继续和大家介绍视觉SLAM的主流框架，VIO多传感器融合以及激光SLAM等内容，欢迎大家持续关注~

从零开始学习三维视觉核心技术SLAM，扫描查看介绍，3天内无条件退款

欢迎关注公众号：计算机视觉life，一起探索计算机视觉新世界~ 早就是优势，学习切忌单打独斗，这里有教程资料、练习作业、答疑解惑等，优质学习圈帮你少走弯路，快速入门！

交流群

欢迎加入公众号读者群一起和同行交流，目前有SLAM、三维视觉、传感器、自动驾驶、计算摄影、检测、分割、识别、医学影像、GAN、算法竞赛等微信群（以后会逐渐细分），请扫描下面微信号加群，备注：”昵称+学校/公司+研究方向“，例如：”张三 + 上海交大 + 视觉SLAM“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进入相关微信群。请勿在群内发送广告，否则会请出群，谢谢理解~