ORB-SLAM学习总结

一、ORB-SLAM简介

ORB-SLAM是一种基于ORB特征的三维定位与地图构建算法（SLAM）[1]。该算法由Raul Mur-Artal，J. M. M. Montiel和Juan D. Tardos于2015年发表在IEEE Transactions on Robotics。ORB-SLAM基于PTAM架构，增加了地图初始化和闭环检测的功能，优化了关键帧选取和地图构建的方法，在处理速度、追踪效果和地图精度上都取得了不错的效果。要注意ORB-SLAM构建的地图是稀疏的。

ORB-SLAM一开始基于monocular camera，后来扩展到Stereo和RGB-D sensor上。作者好像还会做Semi-dense mapping的扩展。作者的开源代码都在GIT上[2]。

ORB-SLAM算法的一大特点是在所有步骤统一使用图像的ORB特征。ORB特征是一种非常快速的特征提取方法，具有旋转不变性，并可以利用金字塔构建出尺度不变性。使用统一的ORB特征有助于SLAM算法在特征提取与追踪、关键帧选取、三维重建、闭环检测等步骤具有内生的一致性。

ORB-SLAM架构如下：

ORB-SLAM利用三个线程分别进行追踪、地图构建和闭环检测。

一、追踪

1. ORB特征提取
2. 初始姿态估计（速度估计）
3. 姿态优化（Track local map，利用邻近的地图点寻找更多的特征匹配，优化姿态）
4. 选取关键帧

二、地图构建

1. 加入关键帧（更新各种图）
2. 验证最近加入的地图点（去除Outlier）
3. 生成新的地图点（三角法）
4. 局部Bundle adjustment（该关键帧和邻近关键帧，去除Outlier）
5. 验证关键帧（去除重复帧）

三、闭环检测

1. 选取相似帧（bag of words）
2. 检测闭环（计算相似变换（3D<->3D，存在尺度漂移，因此是相似变换），RANSAC计算内点数）
3. 融合三维点，更新各种图
4. 图优化（传导变换矩阵），更新地图所有点

作者提供了ORB-SLAM在New College Data[3]上的时间统计，如下图。

1. 追踪部分，平均每帧约30毫秒，基本达到了30fps。特征提取速度是非常快的，平均11毫秒左右，非常适合于实时SLAM。姿态估计稍微耗时一些，平均需要20毫秒，特别是姿态优化需要耗费16毫秒的时间。

2. 地图构建部分，平均每关键帧约385毫秒。其中生成新的点约70毫秒，Local BA约300毫秒，相对还是比较耗时的。不知道这两部分还有没有优化的空间。

二、ORB-SLAM性能

ORB－SLAM程序提供了运行Monocular、Stereo和RGBD数据的程序。编译成功后，可以通过运行TUM的标准数据来验证程序是否成功。如果想自己测试一些数据，可以通过OpenCV提供的接口调起电脑的摄像头。

个人认为，ORB-SLAM是一个完整的单目SLAM实现，集合了当前流行的SLAM特性。作者的程序非常工程化，其中有诸多精度与运算量的权衡。具体总结如下。

ORB－SLAM的优点：

1. Tracking的平均时间约为20ms每帧，基本可以达到实时追踪（i5－5200，2.2GHz）。
2. 丢帧以后回到原来的场景，很容易就可以找回来。
3. 定位的稳定性较好，姿态流畅，没有跳变。
4. 在简单背景下，可以有效地追踪目标物体。

ORB－SLAM的缺点：

1. 旋转时比较容易丢帧，特别是pure rotation。
2. 地图中的点云很稀疏，完全不能看出任何结构。
3. 加载地图需要一定时间（10秒左右，通过二进制词典可以加速，DBoW2的作者似乎是为了兼容性放弃了二进制）。
4. 初始化时最好保持低速运动，对准特征和几何纹理丰富的物体。
5. 作者为了增强系统的鲁棒性，在很多地方采用了多重判断，引入了N多参数。不同场景下的应用可能需要花一些时间理解和调整这些参数。

简言之，对于地图密度要求不高的定位和追踪问题，ORB－SLAM是个不错的选择。

三、ORB-SLAM 初始化

单目SLAM地图初始化的目标是构建初始的三维点云。由于不能仅仅从单帧得到深度信息，因此需要从图像序列中选取两帧以上的图像，估计摄像机姿态并重建出初始的三维点云。

ORB-SLAM中提到，地图初始化常见的方法有三种。

方法一

追踪一个已知物体。单帧图像的每一个点都对应于空间的一条射线。通过不同角度不同位置扫描同一个物体，期望能够将三维点的不确定性缩小到可接受的范围。

方法二

基于假设空间存在一个平面物体，选取两帧不同位置的图像，通过计算Homography来估计位姿。这类方法在视差较小或者平面上的点靠近某个主点时效果不好。

参考文献Faugeras et al, Motion and structure from motion in a piecewise planar environment. International Journal of Pattern Recognition and Artificial Intelligence, 1988.

方法三

根据两帧之间的特征点匹配计算Fundamental matrix，进一步估计位姿。这种方法要求存在不共面的特征点。

参考文献Hartley, Richard, and Andrew Zisserman. Multiple view geometry in computer vision. Cambridge university press, 2003.

ORB-SLAM的作者提出了一种基于统计的模型选择方法。该方法优先选择第三种方法，并期望在场景退化情形下自动选择第二种方法。如果选取的两帧不满足要求，放弃这两帧并重新初始化。

第一步

提取特征点并匹配，若匹配点足够多，尝试第二步。

第二步

利用匹配的特征点分别计算方法二和方法三。

对每个模型，首先归一化所有的特征点。然后，在每步迭代中，

1. 根据特征点对计算homography或者fundamental matrix。Homography的计算方法为normalized DLT，fundamental matrix的计算方法为normalized 8 points。

2. 计算每个点对的symmetric transfer errors，和卡方分布的对应值比较，由此判定该点是否为内点。累计内点的总得分。

    

   模型选择方法参考4.7.1 RANSAC in Multiple View Geometry in Computer Vision。

    

3. 比较本次得分和历史得分，取最高分并记录相应参数。

第三步

根据一定的准则选择模型。用SH表示homography的得分，SF表示fundamental matrix的得分。如果SH / (SH + SF) > 0.4，那么选择homography，反之选择fundamental matrix。

第四步

根据选择的模型计算位姿。参考方法二和方法三。

第五步

Full bundle adjustment。

由于ORB-SLAM对初始化的要求较高，因此初始化时可以选择一个特征丰富的场景，移动摄像机给它提供足够的视差。另外，由于坐标系会附着在初始化成功的那帧图像的位置，因此每次初始化不能保证在同一个位置。

四、ORB-SLAM追踪

 最近在读ORB-SLAM的代码，虽然代码注释算比较多了，但各种类和变量互相引用，看起来有点痛苦。索性总结了一下Tracking部分的代码结构，希望能抓住主要思路，不掉坑里。

作者的程序分为两种模式：SLAM模式和Localization模式。SLAM模式中，三个线程全部都在工作，即在定位也在建图。而Localization模式中，只有Tracking线程在工作，即只定位，输出追踪结果（姿态），不会更新地图和关键帧。Localization模式主要用于已经有场景地图的情况下（在SLAM模式下完成建图后可以无缝切换到Localization模式）。Localization模式下追踪方法涉及到的关键函数是一样的，只是策略有所不同。

以下介绍SLAM模式下的追踪算法。

主要流程

初始追踪（Init pose estimation）

作者在追踪这部分主要用了几种模型：运动模型（Tracking with motion model）、关键帧（Tracking with reference key frame）和重定位（Relocalization）。

下面一一介绍。

Tracking with motion model

假设物体处于匀速运动，那么可以用上一帧的位姿和速度来估计当前帧的位姿。上一帧的速度可以通过前面几帧的位姿计算得到。这个模型适用于运动速度和方向比较一致，没有大转动的情形下，比如匀速运动的汽车、机器人、人等。而对于运动比较随意的目标，当然就会失效了。此时就要用到下面两个模型。

Tracking with reference key frame

假如motion model已经失效，那么首先可以尝试和最近一个关键帧去做匹配。毕竟当前帧和上一个关键帧的距离还不是很远。作者利用了bag of words（BoW）来加速匹配。首先，计算当前帧的BoW，并设定初始位姿为上一帧的位姿；其次，根据位姿和BoW词典来寻找特征匹配（参见ORB－SLAM（六）回环检测）；最后，利用匹配的特征优化位姿（参见ORB－SLAM（五）优化）。

Relocalization

假如当前帧与最近邻关键帧的匹配也失败了，意味着此时当前帧已经丢了，无法确定其真实位置。此时，只有去和所有关键帧匹配，看能否找到合适的位置。首先，计算当前帧的Bow向量。其次，利用BoW词典选取若干关键帧作为备选（参见ORB－SLAM（六）回环检测）；再次，寻找有足够多的特征点匹配的关键帧；最后，利用特征点匹配迭代求解位姿（RANSAC框架下，因为相对位姿可能比较大，局外点会比较多）。如果有关键帧有足够多的内点，那么选取该关键帧优化出的位姿。

姿态优化（Track local map）

姿态优化部分的主要思路是在当前帧和（局部）地图之间寻找尽可能多的对应关系，来优化当前帧的位姿。实际程序中，作者选取了非常多的关键帧和地图点。在跑Euroc数据集MH_01_easy时，几乎有一半以上的关键帧和地图点（后期>3000个）会在这一步被选中。然而，每一帧中只有200~300个地图点可以在当前帧上找到特征匹配点。这一步保证了非关键帧姿态估计的精度和鲁棒性。个人觉得这里有一定的优化空间。

更新局部地图（Local mapping thread）

这里简单提一下local mapping thread。Tracking成功以后，需要更新motion model，并判断当前帧是否是新的关键帧。如果是，将其加入并更新局部地图（local map），建立当前关键帧与其它关键帧的连接关系，更新当前关键帧与其它关键帧之间的特征点匹配关系，并利用三角法生成新的三维点，最后做一个局部优化（local BA，包括相邻关键帧和它们对应的三维点，参见ORB－SLAM（五）优化）。

程序

作者的这部分程序逻辑判断很多，如果读者对作者的程序有兴趣，那么可以参照下图来阅读追踪这部分的程序。

Map is active应改为Localization is active。从Localization is active开始，NO代表SLAM模式，YES代表Localization模式。

五、ORB-SLAM优化

ORB-SLAM作为单目SLAM，其精度很大程度上决定于帧与帧之间的位姿优化的是否准确。因此优化（optimization）在ORB-SLAM里面扮演了很重要的角色。这一小节探讨一下ORB-SLAM里用到的优化。

ORB-SLAM选用g2o作为图优化的方法，关于g2o可以参考深入理解图优化与g2o：g2o篇 - 半闲居士 - 博客园。

一、为什么要优化

因为摄像机标定（camera calibration）和追踪（tracking）的精度不够。摄像机标定的误差会体现在重建中（比如三角法重建时），而追踪的误差则会体现在不同关键帧之间的位姿中，和重建中（单目）。误差的不断累积会导致后面帧的位姿离实际位姿越来越远，最终会限制系统整体的精度。

1.1 摄像机标定

单目SLAM文献中一般假设摄像机标定的结果是准确的，并不考虑这个因素带来的误差（大概因为很多时候跑标准的数据集，认为摄像机标定的误差是相似的）。然而对于一个产品，不同类型的传感器对应的标定误差并不相同，甚至有可能差异很大。因此，如果要评估整个系统的精度，这方面的误差必须要考虑进去。

1.2 追踪

无论在单目、双目还是RGBD中，追踪得到的位姿都是有误差的。单目SLAM中，如果两帧之间有足够的对应点，那么既可以直接得到两帧之间的位姿（像初始化中那样），也可以通过求解一个优化问题得到（如solvePnP）。由于单目中尺度的不确定性，还会引入尺度的误差。由于tracking得到的总是相对位姿，前面某一帧的误差会一直传递到后面去，导致tracking到最后位姿误差有可能非常大。为了提高tracking的精度，可以1. 在局部和全局优化位姿；2. 利用闭环检测（loop closure）来优化位姿。

二、如何优化

2.1 优化的目标函数在SLAM问题中，常见的几种约束条件为： 1. 三维点到二维特征的映射关系（通过投影矩阵）；2. 位姿和位姿之间的变换关系（通过三维刚体变换）；3. 二维特征到二维特征的匹配关系（通过F矩阵）；5. 其它关系（比如单目中有相似变换关系）。如果我们能够知道其中的某些关系是准确的，那么可以在g2o中定义这样的关系及其对应的残差，通过不断迭代优化位姿来逐步减小残差和，从而达到优化位姿的目标。

2.2 局部优化

当新的关键帧加入到convisibility graph时，作者在关键帧附近进行一次局部优化，如下图所示。Pos3是新加入的关键帧，其初始估计位姿已经得到。此时，Pos2是和Pos3相连的关键帧，X2是Pos3看到的三维点，X1是Pos2看到的三维点，这些都属于局部信息，共同参与Bundle Adjustment。同时，Pos1也可以看到X1，但它和Pos3没有直接的联系，属于Pos3关联的局部信息，参与Bundle Adjustment，但取值保持不变。Pos0和X0不参与Bundle Adjustment。

因此，参与优化的是下图中红色椭圆圈出的部分，其中红色代表取值会被优化，灰色代表取值保持不变。(u,v)是X在Pos下的二维投影点，即X在Pos下的测量（measurement）。优化的目标是让投影误差最小。

2.3 全局优化

在全局优化中，所有的关键帧（除了第一帧）和三维点都参与优化。

2.4 闭环处的Sim3位姿优化

当检测到闭环时，闭环连接的两个关键帧的位姿需要通过Sim3优化（以使得其尺度一致）。优化求解两帧之间的相似变换矩阵，使得二维对应点（feature）的投影误差最小。

如下图所示，Pos6和Pos2为一个可能的闭环。通过(u4,2,v4,2)(u4,2,v4,2)和(u4,6,v4,6)(u4,6,v4,6)之间的投影误差来优化S6,2S6,2。

2.5 Sim3上的位姿优化

单目SLAM一般都会发生尺度（scale）漂移，因此Sim3上的优化是必要的。相对于SE3，Sim3的自由度要多一个，而且优化的目标是矫正尺度因子，因此优化并没有加入更多的变量（如三维点）。

作者在检测到闭环时在Sim3上对所有的位姿进行一次优化。定义Sim3上的残差如下：

ei,j=logSim3(SijSjwS−1iw)ei,j=logSim3(SijSjwSiw−1)

其中SiwSiw的初值是尺度为1的Pos i相对于世界坐标系的变换矩阵。Si,jSi,j为Pos i和Pos j之间的（Sim3优化之前的）相对位姿矩阵，表示SiwSiw和SjwSjw之间的测量（measurement）。此处相当于认为局部的相对位姿是准确的，而全局位姿有累计误差，是不准确的。

 三、小结

个人理解，单目SLAM中的优化需要更多技巧，要有明确的优化目标，要仔细权衡其中的参数选择、自由度、速度和稳定性。

六、ORB-SLAM 回环检测

无论在单目、双目还是RGBD中，追踪得到的位姿都是有误差的。随着路径的不断延伸，前面帧的误差会一直传递到后面去，导致最后一帧的位姿在世界坐标系里的误差有可能非常大。除了利用优化方法在局部和全局调整位姿，也可以利用回环检测（loop closure）来优化位姿。

这件事情就好比一个人走在陌生的城市里，一开始还能分清东南西北，但随着在小街小巷转来转去，已经不知道自己在什么地方了。通过认真辨识周边环境，他可以建立起局部的地图信息（局部优化）。再回忆以前走过的路径，他可以纠正一些以前的地图信息（全局优化）。然而他还是不敢确定自己在城市的精确方位。直到他看到了一个之前路过的地方，就会恍然大悟，“噢！原来我回到了这个地方。”此时，将这个信息传递回整个地图，就可以得到相当准确的地图信息。这就是回环检测。

因此，回环检测在大尺度地图构建上是一个非常有用的方法。回环检测可以从二维图像出发，也可以从三维点云出发。目前大家更推荐基于二维图像的方法。

DBoW2

基于二维图像的方法本质上是一个场景识别的问题。我没有深入研究过，因此直接介绍一下ORB－SLAM中用到的DBoW2的方法。

BoW（bag of words，词袋模型），可以理解为一个以特征描述作为元素的词典。如果是ORB特征，那就是ORB词典；如果是SIFT特征，那就是SIFT词典。词典可以从图像数据集中训练出来。下面举一个简单的例子。假如我们有一个一万幅图像的数据集，并认为它基本上涵盖了我们所面临的场景。

1. 从每幅图像中提取特征点和特征描述；特征描述一般是一个多维向量，因此可以计算两个特征描述之间的距离；

2. 将这些特征描述进行聚类（比如k－means），类别的个数就是词典的单词数，比如1000；也可以用Beyes、SVM等；

3. DBoW2将这个词典组织成树的形式，方便搜索。

在实际应用中，每一幅图像都在词典中搜索其最近邻的单词，并在该单词下留下标记。如果A、B两幅图像定位到同一个单词时，说明这两幅图像有可能有相似的特征点。当A、B有一定量的相似点时，可以认为这两幅图像之间存在着一定的相似性。

ORB－SLAM的作者修改了DBoW2，输出一系列候选图像（candidate）而不是一幅最相似的图像。

基于BoW的方法有一些非常好的优势：

1. 词典可以离线训练。在实时应用中能离线的东西越多越好。作者提供了通过大量数据训练出来的BRIEF和SIFT的词典。

2. 搜索速度飞快。小尺寸的图像可以在毫秒级别完成。作者提供了正向（direct index）和反向（inverse index）两种辅助指标。反向指标在节点（单词）上储存到达这个节点的图像特征的权重信息和图像编号，因此可用于快速寻找相似图像。正向指标则储存每幅图像上的特征以及其对应的节点在词典树上的某一层父节点的位置，因此可用于快速特征点匹配（只需要匹配该父节点下面的单词）。

3. 很多slam应用本身就需要计算特征点和描述，因此可以用特征来搜索。

4. ORB－SLAM的作者还用词典的特性做快速的特征筛选，减少特征匹配需要的时间（特别是在大尺度上搜索特征时）。​

当然它也有自己的劣势：

1. 如果应用的场景比较特殊，请训练自己的词典。一般的词典会不太好用。

2. BoW一般不太考虑特征之间的几何关系（有人在做，但不清楚效果和计算量如何）

还有几个comments：

1. 如果应用本身不需要计算特征，那要考虑额外的计算时间。

2. 推荐像ORB－SLAM一样，BoW只用来快速筛选图像，后续需要通过其它方法一一验证、严格验证。如果回环选错了，那，就跪了。

3. 如果场景特征很少，或者重复的特征太多，比较难办。

4. 在稠密点云重建中，如果场景本身有丰富的几何纹理，那么可以利用两帧之间（包括相邻位置）的三维点云匹配去验证回环。如果匹配的误差足够小，那么回环是比较准确的。

回环验证和Sim3优化

对每个候选的回环帧，作者先匹配其和当前帧上的特征点，然后用特征点对应的三维点去求解一个相似变换矩阵（RANSAC框架下）。如果某个回环帧对应的矩阵有足够多的内点，那去做Sim3优化。利用优化结果再去寻找更多的特征匹配，再做一遍优化。如果内点足够多，那么接受这个回环。

Sim3优化详见上一篇。

回环融合（fusion）

这里有一个隐含假设，即误差随着时间不断累积，相对而言，我们更信任之前的信息而不是当前的信息。这部分主要是把回环帧的信息融合到当前帧里面，包括匹配的特征点对应的三维信息（深度、尺度等），世界坐标系下的位姿（通过Sim3的结果转化过去）等等。融合也包括回环帧的邻域和当前帧的邻域。

全局优化

全局优化时，固定回环帧及其邻域，当前帧及其邻域，优化剩余帧在世界坐标系的位姿。详见上一篇。

[1] ORB-SLAM: A Versatile and Accurate Monocular SLAM System

[2] https://github.com/raulmur/ORB_SLAM2

[3] http://www.robots.ox.ac.uk/NewCollegeData/

转载说明：

 本文转载自博客园(http://www.cnblogs.com/luyb)，未经允许请勿用于商业用途。 COPYRIGHT@CNBLOGS.COM/LUYB 联系方式：luyanbin7 at gmail.com