单目相机具有成本低，设置简单的优点，但是同样存在尺度不确定、初始化需要足够视差，最终造成尺度漂移、无法处理纯旋转等问题。
ORB-SLAM2为SLAM的发展作出了以下几个贡献：
1.这是第一个同时提供单目，双目和RGB-D接口的SLAM开源系统，并且包含回环检测，重定位和地图重用。
2.通过BA对RBG-D进行优化，效果优于state-of-the-art的ICP或photometric and depth error minimization
3.通过使用近距离和远距离的双目点以及单目观测，使得其双目的精确度要高于state-of-the-art的直接使用双目的SLAM系统
4.通过禁用建图来实现利用已有地图，进行轻量级的定位。
ORB-SLAM2的框架和ORB-SLAM的框架几乎是一模一样的，包含了三个线程：Tracking，Local Mapping和Loop Closing。主要的区别还是在对于双目相机和RBG-D相机前端，是如何进行追踪并构建后端优化问题的。因此与ORB-SLAM相同的部分就不再赘述，详细可参考上一篇关于ORB-SLAM论文的博客。
A.Monocular, Close Stereo and Far Stereo Keypoints
ORB-SLAM2是基于特征的SLAM系统，因此当从输入的图像中提取特征之后，图像不需要被保存而是直接丢弃，因此可以说ORB-SLAM2与传感器之间是相互独立的，重要的还是特征提取的过程，如图所示。
Steroe keypoints
Steroe Keypoints用三维坐标Xs=(uL,vL,vR)来定义。其中(uL,vL)表示左边图像的像素点坐标，vR表示右边图像的横坐标。
对于双目相机，首先从左边图像中提取ORB特征，然后在右边图像中寻找匹配，通过这种方式可以快速的找到一系列的Steroe keypoints。
对于RGB-D相机，从RGB图像中提取ORB特征，此时提取的ORB特征的像素坐标作为左边图像的像素坐标，然后根据每个特征对应的深度信息恢复出该ORB特征在右边图像的像素坐标，从而得到Steroe Keypoints。基线设置为8cm。
一个Keypoint会被分为far和close两种。如果Steroe Keypoints对应的深度小于基线的40倍则为close，反之则为far。close的关键点可以从一帧图像中直接三角化得到准确的scale，translation和rotation信息，因为其深度信息是可靠的。而far的关键点可以提供准确的rotation信息，而scale和translation信息并不可靠，因此需要通过多视图方式来对远距离的关键点进行三角化。
Monocular keypoints
Monocular keypoints用二维坐标Xm=(vL,vL)来定义。对于无法提供深度信息的关键点，只能通过多视图的方式进行三角化，但无法恢复scale信息，只能恢复出rotation和translation信息。
B.System Bootstrapping
使用双目或者RGB-D相机不需要像单目相机那样进行复杂的初始化，而只需要把第一帧作为关键帧，得到初始地图。
C.Bundle Adjustment with Monocular and Stereo Constraints
论文中提到了三种BA方式：motion-only BA，local BA和full BA。与ORB-SLAM中提到的BA方式也是一一对应的。
ORB-SLAM2中的BA增加了从地图点到Steroe keypoint的投影方式，从而使Steroe keypoint可以直接作为误差计算对象出现在BA中。
D. Loop Closing and Full BA
使用双目或者RGB-D相机不会出现尺度漂移的问题，因此在对回环候选帧进行位姿优化时，不再需要使用sim3相似变换，使用so3刚体变换就可以了。而进行full BA的闭环优化时，鉴于其计算量可能会比较大，因此在这里会新开一个线程专门处理full BA，这里会涉及到full BA过程中如果有新的回环被检测到该怎么办的问题。论文阐述的做法是直接停止当前正在进行的full BA，闭合最新的回环和full BA。新开线程处理full BA还会存在full BA完成时，如何把full BA的结果与full BA过程中信添加的关键帧和地图点融合，论文提到的做法是full BA执行过程中，先不把新的关键帧和地图点加进来，而是等full BA完成时，根据优化结果对新的关键帧和地图点做一次矫正，然后再进行关键帧和地图点的添加。
E. Keyframe Insertion
ORB-SLAM2的关键帧添加策略与ORB-SLAM基本保持一致。唯一的区别就是基于Steroe Keypoint添加了一个条件。在大尺寸的场景中，只有存在足够多的close Steroe Keypoint才能保证位姿评估的准确性，因此在Tracking过程中，两帧之间匹配的close Steroe Keypoint少于100时，并且当前帧可以提取多余70个close Steroe Keypoint时，当前帧就可以作为关键帧进行添加。这种策略对于大场景下的定位非常关键。
F. Localization Mode
ORB-SLAM2在ORB-SLAM的基础上添加了定位模式，使其可以利用已有的地图进行高效的定位。在这种模式下Local Mapping和Loop Closing线程被停止，只通过Tracking线程对相机位姿进行追踪。用到的就是用两帧之间的特征匹配做motion-only BA，以及根据已有地图做local BA。
总结
可以说ORB-SLAM2是非常优秀并且易于学习的SLAM系统，它涉及了基于视觉特征的SLAM的方方面面。作者最后也提到了基于ORB-SLAM2的未来工作，包括多相机融合，鱼眼相机的支持，生成大尺度的稠密地图以及提高系统鲁棒性等。最后我希望自己未来能够在这些方面作出贡献。
ORB-SLAM 论文笔记
论文是从Bundle Adjustment(BA)对于SLAM的作用展开的。通过BA可以为相机定位和地图构建提供更精确的评估。关于BA在博客3D-2D的运动估计问题是指，对于空间中的某一点，我们知道这个点在两个相机坐标系中的三维坐标，如何利用这两个三维坐标来求解这两个相机坐标系的运动就是3D-3D的位姿评估问题。里做过一次简单的介绍。以前BA由于计算量比较大，因此被认为无法用于实时SLAM，特别是视觉SLAM，但是随着移动计算能力的快速发展，BA已经可以在视觉SLAM中达到实时的效果。要运用BA，相应的视觉SLAM算法必须满足以下几个方面：
1.要对关键帧之间的特征进行匹配以及地图点和特征之间进行关联
2.为了防止计算量过大，要去除冗余的关键帧
3.关键帧和地图点之间的匹配和关联要尽可能的稠密，也就是说关键帧中的观察到的地图点能够提供足够多的视差以及足够的回环
4.要提供一个尽可能准确的初始值用于BA的非线性优化
5.对于局部地图，优化只在局部进行，与全局尺寸无关，从而具备更好的可扩展性
6.对于全局地图，要有回环检测和全局优化
其实从ORB-SLAM的很多设计方式上看，都是奔着满足上面这几个方面去的，也算BA运用到视觉SLAM中的一个简单的指导。
ORB-SLAM的成功离不开第一个基于特征的视觉SLAM：PTAM。ORB-SLAM基本延续了PTAM的基本思路，在PTAM的基础上做了很多方面的改进。
ORB-SLAM主要有以下几点优点：
1.在所有的模块中都采用相同的图像特征。这些模块包括追踪，建图，重定位，回环，因此使得我们的系统更加的简单，高效和可靠。通过使用ORB特征，使得可以在不采用GPU的情况下实时运行，并在不同的视角和光照下都具有良好的不变性。
2.能够实时运行在大场景的环境中。通过使用Covisibility Graph，追踪和局部地图构建都在Covisibility Graph的一部分中处理，因此与全局的地图尺寸无关。
3.实时的回环检测的优化是基于Essential Graph进行的，Essential Graph是通过生成树进行维护，是基于Covisibility Graph强关联边和回环边构建的。
4.实时的重定位，能够从追踪丢失中恢复追踪以及在已有地图中进行定位。
5.地图初始化时可以根据是否为平面自动选择相应的模型。
6.对于地图点和关键帧的选择采用一种survival of the fittest的方法，生成时放宽尺度但选择时提高要求。这种策略可以剔除冗余的关键帧，从而增强追踪的鲁棒性以及长时间运行的能力。
System Overview
A.Feature Choice
我们设计的主要思想之一就是用于建图和追踪的特征可以同时用于重定位和回环检测。这使得我们的系统更有效率。选择ORB特征[3]是因为其计算和匹配速度快，具备旋转不变性。ORB特征已经在位置识别中取得了很好的性能[4]。
B. Three Threads: Tracking, Local Mapping and Loop Closing
如图1所示，系统共有Tracking，Local Mapping和Loop Closing三个线程在同时运行。
Tracking：基于每一帧来对地图进行定位，并会决定什么时候插入新的关键帧。通过与前一帧进行特征匹配来对当前帧的位姿进行初始化估计，然后用motion-only BA对位姿作进一步优化。当Tracking丢失时通过位置识别来进行全局的定位。完成了与前一帧的匹配和位姿估计之后，可以通过得到的位姿将与其他关键帧进行特征匹配，也就是从一个局部地图里面进行搜索匹配使相机位置得到进一步优化。最后Tracking线程来判断当前帧是不是关键帧。
Local Mapping：通过处理关键帧和执行local BA，从而得到一个较优的地图。对于新的关键帧，会Covisibility Graph寻找更多的特征匹配，并三角化得到新的地图点。对于新的地图点会进行严格筛选，Local Mapping也同样负责删除一些冗余帧。
Loop Closing：对每个新的关键帧都要进行闭环检测，以确认是否有闭环。如果闭环被检测到，通过计算相似变换来得到闭环的累积误差。然后对齐闭环处的两帧并融合重复的地图点。最后，执行有相似约束的位姿图优化[5]，消除回环误差，确保全局地图的一致性。这里的优化是基于Essential Graph进行，它是Covisibility Graph的子图。
所有的优化都是基于g2o实现的 Levenberg-Marquardt 算法。
C.Map Points, KeyFrames and their Selection
每个地图点pi包含：
1.世界坐标系下的3D坐标Xw,i
2.视图方向ni，对于每个能观察到该地图点的关键帧的中心到地图点有一个视图方向，ni是所有能观察到该地图点的关键帧的视图方向的平均值，用单位向量表示
3.ORB特征描述子Di，对于所有能够观察到该地图点的关键帧都计算一个ORB特征描述子，Di是其中汉明距离最小的。
4.根据ORB特征尺度不变性的约束，计算出能观测到该点的最大距离dmax和最小距离dmin
每个关键帧Ki包含：
1.相机的位姿Ti,w，也是从世界坐标系转换到相机的坐标系的刚体变换矩阵
2.相机内参，包括焦距和相机主点
3.所有从图像帧提取的ORB特征，不管是否已经关联了地图点，都通过畸变模型进行矫正
地图点和关键帧通过宽松策略创建，而剔除机制会非常严格，剔除冗余的关键帧和错误匹配或不可追踪的地图点。从而使运行期间地图具备扩展性，增强在极端环境下追踪的鲁棒性。
D.Covisibility Graph and Essential Graph
关键帧之间的视图内容关联信息(Covisibility information)在系统的几个模块上都非常有用,关联信息用权重图像间接地表示。每个节点都是一个关键帧，如果两个关键帧可以观察到相同的地图云点，根据地图云点数目赋予边权重值θ。
我们通过位姿图优化来纠正回环。为了防止Covisibility Graph非常稠密，我们通过建立Essential Graph来保留所有的关键帧节点，但是只保留少量的边，从而保留足够的信息来产生精确的优化结果。系统从第一个关键帧开始增量式地构建一个生成树，它是Covisibility Graph中边数最少的连通子图。当新的关键帧插入时，它加入树中，并连接到能够观察到相同地图点最多的关键帧上。当一个关键帧通过筛选策略被删除时，系统会根据关键帧所在的位置更新生成树。Essential Graph包含了生成树，是保留了Covisibility Graph中高度关联(θmin = 100)的边和回环边的子图，构成强连通的关键帧连接图。
E.Bags of Words Place Recognition
基于DBoW2[7]，系统嵌入了基于图像词袋的位置识别模块，完成闭环检测和重定位功能。视觉单词是离散化的描述子空间，视觉单词组成视觉字典。视觉字典是离线创建的，创建视觉字典的ORB特征描述子是从大量图像中提取的。如果图像足够多，同一个视觉字典可以用于多个不同的环境。系统增量式的构建一个包含倒序索引的数据库，用来存储视觉单词，从而可以高效地检索关键帧。当关键帧通过筛选机制被删除时，数据库也会更新。
关键帧在视图上可能会存在重叠，检索数据库时，可能不止一个高分值的关键帧。DBoW2认为是图像重叠的问题，就将时间上接近的所有图像的分值相加。但这样并没有考虑同一个地方的关键帧在不同时刻被观察到的情况。因此我们依据关键帧是否是视图关联的进行分类。另外，我们的数据库返回的是分值高于最好分值75%的所有关键帧。
利用词袋模型还可以加速特征匹配。详见[4,7]。
Automatic Map Initialization
由于单目相机无法直接得到景深信息，因此需要通过多张图像来恢复环境的三维结构，这个过程通常被称为地图初始化。地图初始化需要两帧之间有足够的视差，基于视差计算出两帧之间的位姿变换，并通过三角化得到初始的地图点。依据观察到的场景是否是平面的，单目相机的初始化通常分为两种方法：基于单应矩阵的平面初始化和基于基础矩阵的非平面初始化。ORB-SLAM的自动初始化就是同时计算这两种模型，然后用启发式的方法选择最合适的模型来完成初始化。主要的计算流程如下。
1.Find initial correspondences
从当前帧Fc中提取ORB特征，并与参考帧Fr做特征匹配xc↔xr。如果没有找到足够多的匹配特征，就重设参考帧。
2.Parallel computation of the two models
在两个线程中并行的计算单应矩阵Hcr和基础矩阵Fcr:
xc=HcrxrxTcFcrxr=0
单应矩阵的计算方法是normalized DLT，基础矩阵的计算方法是8点法，具体计算方法见神书多视图几何[8]。
为了公平比较单应矩阵和基础矩阵哪个更好，论文设计了一些技巧，比如求解时设定相同的迭代次数，并且用于且解的特征匹配点也是相同的。
对于每一次迭代，都计算一次symmetric transfer errors，基于这个误差求得单应矩阵和基础矩阵的最终得分SH和SF。具体计算过程可以参见原文。
3.Model selection
如果在平面场景选择了基础矩阵进行初始化，得到初始化的地图往往是错误。为了能够在平面场景时选择单应矩阵，在非平面场景时选择基础矩阵，通过计算RH=SHSH+SF,当RH>0.45时,选择单应矩阵，反之选择基础矩阵。这是一种启发式的方式。
4.Motion and Structure from Motion recovery
当模型被选定后，就可以通过得到的矩阵计算相关的运动假设。对于单应矩阵可以恢复出8种假设。对于每种假设匹配好的特征点进行验证。只有当三角化得到的地图点都在相机的前方并且重投影误差足够小，才认为本次的地图初始化是成功的。对于基础矩阵也是如此。
5.Bundle adjustment
执行一次full BA。除了第一帧不变之外，所有关键帧和地图点都会称为BA优化的对象。
Tracking
Tracking线程用来处理从相机中得到的每一帧图像。对于每一帧图像，Tracking线程会通过如下几个步骤进行处理。
1.ORB Extraction
提取ORB特征涉及到几个参数。第一个参数是scale levels，第二个参数是scale factor。这两个参数应该是用来建立图像金字塔，提高ORB特征的尺度不变性。第三个参数是提取ORB的特征数。对于512 × 384 到 752 × 480分辨率，提取1000个。对于更大的分辨率，提取2000个。为了保证提取的ORB特征能够均匀分布，通过将每个scale level的图像进行网格划分，并在每个网格中提取至少5个特征点作为阈值。根据每个网格中实际提取的特征点数，会对阈值进行相应的调整。最后会计算ORB描述子。
2.Initial Pose Estimation from Previous Frame
如果上一帧的追踪成功，就用相同的速率运动模型进行位姿的估计，然后从前一帧看到的地图点云与当前帧做匹配，根据匹配结果进行位姿优化。这里被称为motion BA。如果匹配失败，就加大搜索范围。
3.Initial Pose Estimation via Global Relocalization
如果追踪失败，则将当前帧转化为词袋，然后检索词袋数据库，在所有关键帧中查找ORB特征的匹配。找到对应的匹配之后，进行PnP算法求解当前的位姿。得到一个位姿估计值之后，可以通过搜索当前帧与其他关键帧之间的匹配，从而进一步优化位姿。
4.Track Local Map
当有了对当前相机位姿的估计之后，可以通过建立局部地图的方式来提高位姿评估的精确度。局部地图K1包含一系列关键帧，这些关键帧可以和当前帧观察到相同的地图点，局部地图K2也是一系列关键帧，这些关键帧与K1中的关键帧是相邻的关联关系。K1中和当前帧观察到相同的地图点数目最多的关键帧称为kref。所有在K1和K2可以看到的地图点都会在当前帧中进行一次搜索匹配：
1）直接计算地图点到当前帧的投影x，如果投影在图像边界之外，则直接剔除。
2）计算当前帧的方向v和地图点的视图方向n的乘积，如果v⋅n<cos(60∘)，则剔除。
3）计算地图点到相机中心的距离d，如果d∉[dmin,dmax],则剔除。
4）计算当前帧的尺度d/dmin。
5）比较地图点ORB描述子与当前帧中还未匹配的ORB特征，如果匹配到的ORB特征在尺度和位置上都接近，则找到了地图点和当前帧的一组匹配。
相机位姿会根据匹配的地图点进行进一步优化。
PS:步骤2中提到的motion BA和步骤3,4中提到的位姿优化都是指固定当前帧中能看到的地图点，然后优化当前帧的位姿。
5.New Keyframe Decision
最后Tracking线程会判断当前帧是不是满足关键帧的条件，只有满足以下所有条件才能称为关键帧：
1）距离上一次全局重定位已经超过了20帧图像。（保证好的重定位）
2）Local Mapping处于空闲状态，或者距离上一次插入关键帧已经超过了20帧图像
3）当前帧可以观察到超过50个地图点。（保证好的追踪）
4）当前帧与参考帧同时可以观察到的地图点少于90%。参考帧是K1中与当前帧同时可以观察到的地图点最多的关键帧。（增加视差变化）
Local Mapping
Local Mapping处理每一个被Tracking线程判断为关键帧的新关键帧ki。主要有以下几个处理步骤：
1.KeyFrame Insertion
首先更新Covisibility Graph，图中插入节点ki以及更新与ki相关联的其他关键帧之间的边。
其次更新生成树，将关键帧连接到参考帧上。
最后计算关键帧的词袋表达，有利于为三角化新的地图点提供数据关联。
2.Recent Map Points Culling
地图点能够保留下来必须经过严格的筛选。在创建了三个关键帧之后，所有新创建的地图点都必须满足以下两个条件：
1）能观察到该点的关键帧不应该少于理论上能观测到该点的关键帧数目的1/4。
2）地图点被创建之后，接下来应该至少有3个关键帧可以观察到这个地图点
在任何时刻，如果观察到地图点的关键帧少于3个，那么该地图点会被剔除。这种情况会发生在进行局部BA时，一些异常的关键帧被剔除了。
3.New Map Point Creation
对于ki与在Covisibility Graph中其他相连的关键帧集合Kc，搜索ki中和Kc未与地图点关联的ORB特征之间的匹配，然后三角化出新的地图点。新地图点的创建要满足地图点位置在相机的前方，重投影误差小以及进行尺度的检查。新的地图点是通过两个关键帧创建的，但是可以通过Tracking线程中的Track Local Map，实现地图点和其他关键帧中未匹配地图点的ORB特征进行匹配。
确定新地图点的相关属性。
4.Local Bundle Adjustment
局部BA针对ki,相关联的Kc和所有能被观察的地图点进行优化的。其他可以看到相同的地图点但是不与ki相关联也会加入到BA中，但是只作为约束条件，是固定不变的。优化过程中或者优化结束后，异常点会被剔除。
5.Local Keyframe Culling
Local Mapping会剔除冗余的关键帧，从而有利于BA优化和长时间运行时关键帧的维护开销。
如果一个关键帧中观察到的地图点超过90%能被其他三个关键帧观察到，则剔除该关键帧。这个策略是[9]的启发。
Loop Closing
当Local Mapping线程处理完一个关键帧ki之后，Loop Closing线程会尝试利用ki做回环检测。主要有以下几个步骤：
1.Loop Candidates Detection
计算ki与在Covisibility Graph中相关联的关键帧(θmin=30)的词袋向量的相似度,保留最小值smin。然后检索位置识别数据库并剔除所有小于smin的关键帧。另外，所有与当前帧相连的关键帧都会被剔除。如果连续三个候选关键帧的检索结果是一致的，则认为ki确实为闭环候选关键帧。
2.Compute the Similarity Transformation
单目SLAM系统有7个自由度，3个平移，3个旋转，1个尺度因子，这七个自由度上，地图都有可能发生漂移[5]。因此要闭合某个回环，我们需要计算从当前关键帧Ki到回环关键帧Kl的相似变换矩阵，以获得回环的累积误差。计算相似变换也可以作为回环的几何验证。
我们首先计算与ki中ORB特征关联的地图点和回环候选关键帧的对应关系。此时，对每个候选回环，我们有了一个3D到3D的对应关系。我们对每个候选回环执行RANSAC迭代，通过Horn方法[10]找到相似变换。如果我们用足够的有效数据找到相似变换Sil，我们就可以优化它，并搜索更多的对应关系。如果Sil有足够的有效数据，我们再优化它。当支持Sil的有效数据足够多时，就可以认为kl回环被接受。这里的优化，所有的地图点都是固定的，只优化当前帧的位置。
3.Loop Fusion
回环矫正的第一步是融合重复的地图点，插入与回环闭合相关的相似视图的新边缘。在步骤2中通过相似变换Sil已经矫正了当前关键帧的位姿Tiw，现在可以将矫正扩散到与当前帧相关联的关键帧上，这样回环两端就可以对齐。所有回环关键帧和相关联的关键帧可以观察到的地图点都会被重投影到ki和与ki相关联的关键帧，类似于Tracking线程中的Track Local Map。所有匹配的地图点和用于计算Sil会被融合。融合过程中所有的关键帧将会更新它们的边，这些新更新的边将非常有效的用于闭合回环。
4.Essential Graph Optimization
为了有效的闭合回环，基于Essential Graph做一次位姿优化，此时的优化只固定初始位姿，其他所有的位姿进行尺度漂移的优化，即Sim3的位姿优化。
总结
实验部分就暂不分析了。至此，ORB-SLAM的总体流程和框架应该都非常明了了，对于其中的具体实现细节还需要结合作者的开源代码来看。
对于回环检测的部分，特别是Sim3优化的部分的理解还比较羞涩，需要依据[5,11,12]做进一步学习。
来源：CSDN
作者：春秋深夜
链接：https://blog.csdn.net/qq_21950671/article/details/103908575