转载自我的csdn博客：https://blog.csdn.net/qinqinxiansheng/article/details/115323912

一、系统概述

  ORB-SLAM2支持单目、双目、RGB-D相机的输入，整个系统包含三个线程跟踪线程、局部建图线程、回环检测线程（当检测到回环时，回环融合后还会触发Full BA线程）各线程之间通过关键帧链接。
  跟踪线程： 首先执行当前帧与上一帧地图点的2D-3D匹配，并使用仅motion-onlyBA来优化姿态。如果跟踪丢失则执行全局重新定位。接着将局部地图点投影进来寻找更多匹配以计算更准确的位姿，最后筛选关键帧以并传给局部建图线程。
  局部建图线程： 此线程处理新的关键帧以优化局部地图点和关键帧的位姿，在此过程中需要对地图点进行筛选和添加，最后删除冗余的关键帧以减轻后续线程的压力。
  回环检测线程： 回环检测线程用于检测相机是否移动到了之前的位置，通过回环的修正来消除（或者说分摊）累积误差。回环检测成功后先进行回环修正，最后触发BA线程以进行全局地图点和轨迹的优化。

二、加速特征点匹配策略

  在SLAM系统中，特征点描述子的计算和匹配是非常耗时的，ORB-SLAM2总体来说有两种加速特征点匹配的策略：词袋模型加速匹配、恒速运动模型加速匹配。

2.1词袋模型加速匹配

  词袋模型（BoW）就是用“这幅图里有什么”来描述一副图像，即关心每副图里面有什么“单词”，而不关心其位置。所有单词的集合组成字典。字典通常被组织成树形结构，其中每个圆圈为节点node，最底层的叶子节点为word（单词）。在对特征点进行查询时，通过每一层的分类最终找到其对应的word。
  在这里字典的数据结构除了节点外，还一个前项索引（direct index image）和一个逆向索引（inverse index word）。前项索引记录了每张图片上每个特征点所对应的node（如果是最后一层的话就是word），而每个逆向索引记录了每个word里面都存储了那张图片的那个特征点。

  加速特征点匹配： 在匹配的图像中，对于其每个特征点，查找其属于那个node，在匹配过程中只匹配属于同一node的特征点，即可大大缩小匹配范围。这里查询node的原因是我们一般不会直接查询其所在的最低层的node，即word，因为其可能分的过细，一对匹配的特征点会有可能被分到了两个word里面。
  重定位与回环检测： 通过后向索引，找到与当前帧具有相同单词（word）的关键帧作为候选帧，进一步筛选即可找到与当前帧相似度极高的图像，这便可以用于重定位和回环检测。

2.2恒速运动模型加速匹配

  ORB-SLAM中在跟踪线程中用恒速运动模型预估计相机的位置，即假设相机运动是匀速的，相邻两帧的移动是相等的。跟踪的过程需要将当前帧Fc与上一帧的地图点进行匹配计算位姿，由恒速运动模型得当前帧运动位姿Tbc=Tab，预估出其位置，对于Fb中为每个特征点，由空间位置投影到当前帧Fc上，在投影位置半径r的邻域内寻找其在Fc中的匹配特征点。这样便可大大缩小匹配范围和提高匹配精度。

  值得注意的是，ORB-SLAM中搜索邻域特征点的过程中也有些技巧。每张图像会被分为许多网格，特征点根据位置分配到每一个网格之中。在搜索邻域特征点时：

  1. 首先搜索邻域搜索的网格
  2. 然后对于筛选出来的网格，遍历其特征点，计算距圆心的距离来判断是否属于邻域范围。

三、系统原理详解

3.1初始化

  为什么要初始化:在跟踪的过程需要将当前帧与上一帧所对应的地图点进行3D-2D的匹配，以此来计算位姿，初始化的目的就是生成一些空间地图点供以后匹配。

单目初始化

  单目初始化时需要先从两帧图像恢计算出位姿，然后三角化恢复出地图点。其中两帧图像计算位姿时可以通过计算单应矩阵H或者基础矩阵E两种方法，由H后者E恢复旋转R和平移t，其中计算H矩阵的方法适合地图点分布在同一平面或者相机发生纯旋转运动的情况，计算E矩阵的方法适合空间点杂乱的情况，ORB-SLAM则分别用两种方法计算选取较优者。具体过程为：
  1.特征并匹配： 在特征点匹配的过程中使用类似于恒速模型加速的方法，直接将帧Fa中的特征点投影至Fb中相同的位置，在邻域r中搜寻匹配点。
  2.并行计算单应矩阵H和基础矩阵E（ransac+DLT，ransac+八点法）
  3.评估两种方法的得分，并选择较优的一方。评估得分的方法为双边投影误差，并用核函数进行处理。之后由二者的评价得分SH和SF计算RH，由RH的大小判断使用哪种方法。
  4.三角化恢复地图点 ，由E会恢复出4组R和t，由H会恢复出8组R和t，具体使用那一组解，需要先通过地图点与光心的位置排除一部分解，然后通过三角化恢复出地图点内点个数进行判断。在通过内点个数进行判断时，最优解要远高于次优解才被认为成功，否则初始化失败，重新回到过程1。
  5.Full BA BA优化，优化位姿与地图点。

双目/RGBD初始化

双目初始化的过程要简单的多，由双目的特征匹配及相机内参计算位姿并和地图点。具体过程为
1. 对于匹配左目的特征点，在右目对应行的带状区域进行搜索特征点进行匹配
2. 在右目匹配好的特征点周围进行SAD滑窗搜索，进行像素级别的搜索
3. 对于SAD滑窗的结果（a,b），对（a-1,b）（a,b）（a+1,b）三个像素点，由其第一个坐标和灰度值，进行抛物线拟合，由抛物线的最低点的横坐标作为亚像素级别的结果。（这里抛物线的最低点可由表达式直接表示，不用特别复杂的计算）

  RGBD的直接提取特征点并查找图像深度即可恢复出地图点。RGB-D还会根据像素深度反推出特征点在假想右目中的横坐标uR，这样的话RGBD相机便可以像双目一样使用了。值得注意的是，在ORBSLAM中，单目点被描述为（uL,vL），双目和RGBD点会被描述成（uL,vL,uR），这种描述在BA优化时会有体现。

3.2跟踪线程

  跟踪线程中首先需要对图像进行预处理，也就是特征提取与匹配，ORB在提取特征点的中有均匀化的过程。

  这个线程将当前帧特征点与上一帧的地图点匹配计算位姿。可以细分为三种模式：运动模式、关键帧模式和重定位模式。遵循以下规则：
  1. 正常情况下使用运动模式，用当前帧的特征点与上一帧的地图点进行匹配，使用恒速运动模型加速特征匹配。
  2. 当运动模式跟丢时，将当前帧与上一个关键帧进行匹配计算位姿，使用词袋模型加速匹配。
  3. 当关键帧也跟丢时，则启用重定位模式，用词袋模型寻找候选关键帧，然后再用词袋模型加速当前帧与候选关键帧的匹配，经过筛选后得到最优的匹配结果。

  在这里阐述到共视图和局部地图的概念：
  共视图：以关键帧为顶点，若两个关键帧能够一起看到的地图点达到一定阈值，则这两个顶点之间建立一条边，边的权重为共视地图点的个数。
  局部地图点：1.令与当前帧具有共视关系的关键帧记为共视关键帧K1 2.共视图中与共视关键帧相邻的关键帧K2 3.K1与K2所看到的地图点记为局部地图
  跟踪局部地图的目的是将更多地图点的投影到当前帧中，以寻找更多的3D-2D 的匹配（PS. ORB-SLAM2提出了定位模式，也就是可以只运行跟踪线程，试想如果这里不插入些地图点进行匹配的话，跟踪到的地图点岂不是会越来越少）。在这里便是将当前帧的局部地图点中满足约束的点投影至当前帧进行匹配，约束包括：1 该地图点投影至当前帧后应在图像里面 2该点的平均观测方向与当前帧对该点的观测方向的夹角小于60度 3该点到相机光心的距离满足一定阈值。
在投影并匹配成功后，优化并更新相机的位姿

  筛选并插入关键帧要满足一下条件：
  1. 距离重定位或者上一帧至少经过了20帧，即关键帧不能插入的过密
  2. 当前帧至少跟踪到了50个点，即当前帧的插入要有价值，具有很好的跟踪状态
  3. 当前帧中观测到的地图点中，参考关键帧（与当前帧共视地图点最多的帧）能观测到的点的比例小于90%，即当前帧观测到了足够多的新环境，避免冗余

3.3局部建图线程

  插入关键帧： 更新共视图和生成树（共视图的子集，要求更严格）；计算词袋BoW（之前只有在“跟踪关键帧模式”下才会计算当前帧的词袋）
  剔除冗余的地图点： 这是为了避免误匹配和错误的三角化，保证地图点的质量。地图点要满足两个条件：1被观测到的次数/应该被观测到的次数<25%； 2 如果之后经过了很多帧，那么当前地图点应至少被以后的三个关键帧观测到

  添加新的地图点： 这一过程针对新插入帧中没有匹配到的特征点进行，将这些特征点与局部地图点进行匹配，只有匹配成功并满足了极线约束、正深度检验、视差检验、重投影误差检验，才能被添加到地图中。之后还要根据新生成的地图点更新共视图，并向其他相关的关键帧做投影。

  局部BA： 对地图点和位姿进行BA优化。优化的内容包括新处理的关键帧、其具有共视关系的关键帧，以及他们观测到的地图点，除了这些关键帧，其他观测到待优化地图点的关键帧只提供约束而不会被优化。
  局部关键帧剔除： 剔除关键帧的目的是减少结构冗余，为回环检测线程减轻计算压力，不然相机即使在相同的环境中运动，关键帧规模也会很快地增长。剔除的主要条件为：该关键帧90% 的关键帧被其共视关键帧观察过三次。

3.4回环检测线程

  回环检测： 根据之前词袋模型的讲解，通过词袋模型查找候选匹配帧，然后进行复杂的筛选，约束包括BoW向量的相似度、共有单词数目、连续性。具体的逻辑非常复杂，推荐看b站上的相关视频。
  计算Sim3： Sim3是一种相似变换，有7个自由度，除了R和t外，还有一个尺度s。单目长时间运行过程中会出现尺度漂移，可以用sim3表示两帧之间的关系，而双目和RGBD不存在这样的问题，使用时会令s=1。这样便可以描绘出当前帧会回环匹配帧之间的位置关系了。
  回环修正： 回环修正包括回环融合和本征图优化两个部分。在回环融合过程中，将当前帧的共视图和匹配帧的共视图的地图点进行匹配，建立并更新共视关系，然后通过本征图优化优化整个轨迹的位置。
  Full BA： 针对所有地图点和关键帧进行BA优化。

四、一些总结

4.1单目、双目、RGBD的差别

  之前提到双目和RGBD相机初始化完成后用相同的信息描述一个点，可以等价使用。双目/RGBD和单目在使用的时候有一下几点区别：
  1. 初始化过程，单目初始化是一个非常复杂的过程，双目和RGBD相机可以很容易的恢复出地图点。
  2. 在跟踪线程中用恒速运动模型计算两帧位姿时，双目和RGBD根据上一帧未成功匹配的特征点恢复出一些临时的地图点，用于计算当前帧的位姿，计算位姿后这些地图点会被立即删除。
  3. 双目会有近点和远点的概念，文章说近点可以提供可靠的平移和旋转约束，而原点只能提供旋转约束，但是这点在程序中并未得到体现，近点远点只在插入关键帧时贡献了一些条件。
  4. BA优化时的区别。之前我们提到，在对点的描述上，双目和RGBD比单目多了一个右目的横坐标uR，所以在计算重投影误差时双目RGBD会多一个维度。

4.2系统所用到的优化

  BA优化有两种形式，只优化位姿（位姿图优化）、同时优化位姿与地图点位置，上图中有公式。ORB-SLAM系统中多处进行了位姿优化：
  1. 单目初始化：同时优化位姿与地图点位置
  2. 跟踪线程：只优化位姿（也就是论文中提到了的motion-only BA），应该是位姿图优化
  3. 局部建图线程：同时优化位姿与地图点位置
  4. 回环修正部分有本征图优化：只优化位姿，应该是位姿图优化
  5. FULL BA：同时优化位姿与地图点位置