ORB-SLAM2原理分析
原理分析
ORB-SLAM2是一种基于单目、双目和RGB-D相机的实时视觉SLAM系统，用于在无GPS信号或有限的传感器信息情况下，构建三维地图并定位相机的位置和姿态。ORB-SLAM2采用了ORB特征点提取和描述符匹配技术，以及图优化和闭环检测算法，从而实现了高精度的地图构建和相机定位。ORB-SLAM2可以应用于室内外的多种场景，如机器人导航、增强现实和虚拟现实等领域。
ORB-SLAM2系统的主要特点和优点包括：
多种相机支持：ORB-SLAM2支持单目、双目和RGB-D相机，可以适应不同的应用场景和传感器限制。
实时性能：ORB-SLAM2采用了高效的ORB特征点提取和描述符匹配技术，以及图优化和闭环检测算法，从而实现了高速、实时的地图构建和相机定位。
鲁棒性：ORB-SLAM2在处理模糊、遮挡、光照变化和动态场景等复杂情况下，具有较强的鲁棒性和稳定性。
易于使用和扩展：ORB-SLAM2提供了易于使用和扩展的接口和工具，允许用户自定义算法和参数，以满足不同的应用需求。
开源：ORB-SLAM2是一个开源软件，可从github上获取源代码和文档，方便用户学习和使用。
ORB-SLAM2已经在学术界和工业界得到了广泛的应用和验证，在机器人导航、自动驾驶、增强现实、虚拟现实和无人机等领域具有重要的应用价值和研究意义。
- ORB-SLAM2是由西班牙Zaragoza大学的Raul Mur-Artal教授和Juan D.
Tardos教授等人开发的，于2016年首次发布，是ORB-SLAM的改进版本。 - ORB-SLAM2的主要算法包括：ORB特征点提取和描述符匹配、初始化、位姿估计、地图构建、图优化和闭环检测等。
- ORB-SLAM2的ORB特征点提取和描述符匹配采用了FAST角点检测和ORB描述符生成算法，可以在保证高效率的同时，保持较高的特征匹配精度。
- ORB-SLAM2的初始化算法使用了三个步骤：单目相机的尺度恢复、基础矩阵估计和三角测量。这些步骤可以实现对相机位姿的初始估计，为后续的跟踪和地图构建提供基础。
- ORB-SLAM2使用了基于视觉里程计（Visual
Odometry）的位姿估计方法，通过对相邻帧之间的特征点匹配来计算相机的位姿变化。同时，ORB-SLAM2还采用了IMU（惯性测量单元）数据来提高位姿估计的精度和鲁棒性。 - ORB-SLAM2的地图构建算法使用了基于特征点的稀疏地图表示方法，可以在相机运动过程中实时生成和更新地图。同时，ORB-SLAM2还支持地图点云的稠密表示方式，可以提高地图的精度和可视化效果。
- ORB-SLAM2的图优化算法采用了基于因子图（Factor
Graph）的非线性优化方法，可以将相机位姿和地图点位置的误差最小化，从而提高地图精度和位姿估计的准确性。 - ORB-SLAM2的闭环检测算法使用了词袋模型（Bag-of-Words）和BoW树等技术，可以识别和处理闭环（LoopClosure）情况，从而避免地图漂移和重复建立。
总的来说，ORB-SLAM2是一种功能强大、高效、鲁棒性强的视觉SLAM系统，可以满足多种应用场景和实际需求。
ORB-SLAM2的代码和论文可以在以下网址上找到：
代码：https://github.com/raulmur/ORB_SLAM2
论文：Raul Mur-Artal, Juan D. Tardos, and J. M. M. Montiel, “ORB-SLAM2: An Open-Source SLAM System for Monocular, Stereo, and RGB-D Cameras,” IEEE Transactions on Robotics, vol. 33, no. 5, pp. 1255-1262, Oct. 2017.
在代码仓库中，你可以找到ORB-SLAM2的完整源代码、文档和示例数据集。ORB-SLAM2的代码采用C++语言编写，可以在Linux和Windows系统上编译和运行。同时，ORB-SLAM2还提供了Python接口和ROS（机器人操作系统）节点，方便用户进行二次开发和集成。
在论文中，作者详细介绍了ORB-SLAM2的算法原理、实现细节和实验结果，并与其他SLAM系统进行了比较和评估。论文中还提供了大量的实验和分析数据，可以帮助用户更好地理解和应用ORB-SLAM2系统。
特征点法
特征点法（Feature-Based Method）是一种计算机视觉中常用的方法，通常用于图像配准（Image Registration）、目标跟踪（Object Tracking）、三维重建（3D Reconstruction）等领域。
它的基本思想是在图像中找到一些具有良好区分度和可重复性的特征点，比如角点、边缘点等，然后将这些特征点进行描述，并与其他图像中的特征点进行匹配，从而实现图像的配准、目标跟踪或三维重建等功能。
特征点法的优点是对图像的光照、旋转、尺度等变化具有一定的鲁棒性，可以在相对复杂的场景中实现准确的匹配和定位。常用的特征点算法包括SIFT（尺度不变特征变换）、SURF（加速稳健特征）、ORB（Oriented FAST and Rotated BRIEF）等。
特征点法
ORB-SLAM2 是特征点法的典型代表。
图像中采集到的单个像素点往往受很多因素的干扰，存在噪声，不够稳定
纯单目系统框架
纯单目视觉
纯单目视觉（Monocular Vision）是指利用单目相机获取图像信息，并通过计算机视觉算法实现目标检测、跟踪、定位、三维重建等任务的技术。
与双目视觉、多目视觉等技术相比，纯单目视觉具有成本低、便于实现等优点，因此在许多应用场景中得到了广泛应用，比如无人驾驶、机器人导航、增强现实、虚拟现实等领域。
纯单目视觉的实现需要利用计算机视觉算法对单目图像进行处理和分析，常用的算法包括特征点法、深度学习等。在目标检测和跟踪方面，可以通过利用深度学习的方法，训练出适合单目图像的目标检测和跟踪模型，实现准确的目标检测和跟踪。在三维重建方面，可以通过利用多视角几何方法，将多个单目图像的信息融合起来，实现更精确的三维重建。
三维空间运动
三维空间运动是指物体在三维空间中的运动，包括平移运动和旋转运动。
在三维空间中，物体的平移运动可以用三个方向的位移来描述，通常用位移向量表示。旋转运动则可以用旋转角度和旋转轴来描述，通常用旋转矩阵或四元数表示。
对于三维空间运动，通常需要进行运动估计和运动恢复等任务。运动估计是指从连续的图像序列中估计出物体的运动状态，包括平移向量和旋转矩阵或四元数。运动恢复则是指从物体的运动状态恢复出物体的三维形状和运动轨迹等信息。
常用的三维空间运动估计算法包括基于特征点的方法、基于直接法的方法、基于深度学习的方法等。在三维重建和物体跟踪等领域中，三维空间运动估计是一个重要的基础任务，对于实现准确的三维重建和物体跟踪等应用具有重要意义。
总之，三维空间运动是指物体在三维空间中的平移和旋转运动，对于许多计算机视觉任务具有重要意义，如三维重建、物体跟踪等。
多视图几何2D-2D
多视图几何2D-2D是指在多个视角下，通过对图像中的2D点进行分析和计算，推导出相机之间的运动和三维点的位置。
在2D-2D的多视图几何中，通常需要使用多个视角下的图像，通过对图像中的2D点进行匹配，推导出相机之间的运动和三维点的位置。这个过程通常包括以下几个步骤：
选取特征点：在每个视角下，选取一些具有良好区分性的特征点，比如角点、边缘点等。
特征点匹配：将在不同视角下选取的特征点进行匹配，建立起2D点之间的对应关系。
估计相机运动：通过对2D点之间的对应关系进行分析和计算，可以推导出相机之间的运动，包括相机的平移向量和旋转矩阵。
三维重建：利用相机运动和2D点之间的对应关系，可以恢复出三维点的位置。
在多视图几何2D-2D中，常用的算法包括基于特征点的方法、基于直接法的方法、基于深度学习的方法等。这些算法可以用于三维重建、立体匹配、视觉SLAM等领域中。
总之，多视图几何2D-2D是指通过对多个视角下的图像中的2D点进行匹配和计算，推导出相机之间的运动和三维点的位置的技术，具有广泛的应用价值。
2D-2D 模型
- 单目初始化用到
这个模型中，给定的已知条件是两张 2D 图像的匹配信息，所以称为 2D-2D 模型。在单目 SLAM 初始化新地图点时就需要用到这种模型。
多视图几何3D-2D
多视图几何3D-2D是指通过在三维空间中重建物体的三维结构，并将其投影到多个视角下的图像中，推导出相机之间的运动和三维点的位置的技术。
在3D-2D的多视图几何中，通常需要使用多个视角下的图像和物体的三维结构，通过将物体的三维结构投影到2D图像中，建立起3D点和2D点之间的对应关系，推导出相机之间的运动和三维点的位置。这个过程通常包括以下几个步骤：
- 三维重建：通过利用多个视角下的图像，利用多视图几何算法重建出物体的三维结构。
- 三维点投影：将物体的三维结构投影到多个视角下的2D图像中，得到3D点和2D点之间的对应关系。
- 相机运动估计：通过对3D点和2D点之间的对应关系进行分析和计算，可以推导出相机之间的运动，包括相机的平移向量和旋转矩阵。
在多视图几何3D-2D中，常用的算法包括基于特征点的方法、基于直接法的方法、基于深度学习的方法等。这些算法可以用于三维重建、视觉SLAM、虚拟现实等领域中。
总之，多视图几何3D-2D是指通过在三维空间中重建物体的三维结构，并将其投影到多个视角下的图像中，推导出相机之间的运动和三维点的位置的技术，具有广泛的应用价值。
多视图几何3D-3D
多视图几何3D-3D是指通过在多个视角下重建物体的三维结构，并将其转化为不同视角下的三维点云，推导出相机之间的运动和三维点的位置的技术。
在3D-3D的多视图几何中，通常需要使用多个视角下的三维点云，通过对三维点云之间的对应关系进行分析和计算，推导出相机之间的运动和三维点的位置。这个过程通常包括以下几个步骤：
- 三维重建：通过利用多个视角下的图像，利用多视图几何算法重建出物体的三维结构。
- 三维点云配准：将不同视角下的三维点云进行配准，建立起三维点之间的对应关系。
- 相机运动估计：通过对三维点之间的对应关系进行分析和计算，可以推导出相机之间的运动，包括相机的平移向量和旋转矩阵。
- 在多视图几何3D-3D中，常用的算法包括基于点云配准的方法、基于深度学习的方法等。这些算法可以用于三维重建、机器人导航、虚拟现实等领域中。
总之，多视图几何3D-3D是指通过在多个视角下重建物体的三维结构，并将其转化为不同视角下的三维点云，推导出相机之间的运动和三维点的位置的技术，具有广泛的应用价值。
ORB-SLAM2系统框架
ORB-SLAM2是一种基于单目/双目/RGB-D相机的、实时的、鲁棒的、开源的视觉SLAM系统。其系统框架主要包括以下模块：
- 前端模块：包括特征提取、特征描述和特征匹配等子模块。ORB-SLAM2使用Oriented FAST and Rotated BRIEF
(ORB)算法进行特征提取和描述，采用基于BoW（Bag of Words）的方法进行特征匹配。 - 后端模块：包括关键帧选择、位姿优化、地图点优化等子模块。ORB-SLAM2使用基于信息矩阵的位姿优化方法和基于重投影误差的地图点优化方法，同时采用BA（Bundle Adjustment）进行全局优化。
- 地图管理模块：包括关键帧管理、地图点管理、回环检测等子模块。ORB-SLAM2使用DBow2库进行关键帧和地图点管理，并采用基于词袋模型的回环检测算法进行回环检测。
- 传感器驱动模块：包括相机驱动、IMU驱动等子模块。ORB-SLAM2可以接收单目、双目、RGB-D相机和IMU传感器的输入，支持多种传感器的数据融合。
- 可视化模块：包括3D地图可视化、相机轨迹可视化等子模块。ORB-SLAM2使用可视化工具进行实时的地图和轨迹可视化。
总之，ORB-SLAM2的系统框架包括前端、后端、地图管理、传感器驱动和可视化等多个模块，通过这些模块的协同工作，实现了基于单目/双目/RGB-D相机的实时鲁棒视觉SLAM。
ORB-SLAM2 系统框架要分两部分来看：
- 纯单目系统架构
- 双目和 RGB-D 系统架构
由于双目和RGB-D系统架构是从纯单目系统架构发展而来的，并且双目和RGB-D系统架构实现起来比纯单目更容易。
掌握了纯单目系统的原理，理解双目和 RGB-D 系统非常容易。
纯单目系统架构
- 纯单目系统架构由追踪、局部建图和闭环3个主要线程构成，除此之外还包括地图初始化、位置识别、地图结构等模块。
纯单目系统的架构通常由追踪、局部建图和闭环检测三个主要线程构成，同时还包括地图初始化、位置识别、地图结构等模块。具体来说，各模块的功能如下：
追踪线程：负责实时估计相机的运动，并根据运动估计来跟踪场景中的特征点。当相机运动较小时，追踪线程可以高效地跟踪特征点，从而得到连续的相机位姿估计。
局部建图线程：负责生成局部的地图，并根据追踪线程跟踪到的特征点进行更新。局部建图线程通常使用基于关键帧的方式来组织地图，即将相邻关键帧之间的共同特征点抽象为地图点，从而形成一个稠密的局部地图。
闭环检测线程：负责检测场景中的闭环，并将闭环信息加入到地图中，以提高地图的精度和鲁棒性。闭环检测通常通过比较不同关键帧之间的特征点相互匹配来实现。
此外，地图初始化模块负责在系统启动时生成初始地图，位置识别模块负责在场景中识别相对位置，地图结构模块负责管理地图结构和地图点信息等。
综上所述，纯单目系统的架构通常由追踪、局部建图和闭环检测三个主要线程构成，同时还包括地图初始化、位置识别、地图结构等模块，通过这些模块的协同工作，实现了对场景的感知和理解。
纯单目系统框架
双目和 RGB-D 系统架构
-
相比于纯单目系统，只是增加了输入预处理（Pre-process Input）模块用于专门处理双目或 RGB-D 数据
输入预处理
-
闭环检测从计算 Sim3 换成了计算 SE3（因为双目或RGB-D系统的尺度不确定性消失了）
-
增加了第4个线程——全局BA优化，系统其它部分大体上与纯单目系统保持了一致
纯单目系统框架
地图结构
地图结构的组成：
- 关键帧
- 地图点（MapPoints）
- 共视关系图（covisibility graph）
- 生成树（spanning tree）
关键帧
系统中处理的图像分为两类：
- 普通帧
- 关键帧
普通帧
- 概念：指从相机之诶输入系统追踪线程的图像，仅用于定位追踪。
- 问题：以工作帧率为 30Hz 的相机为例，系统每秒将要处理 30 个普通帧的图像数据，显然这样数据量很大，且帧与帧之间的数据重复率较大，每帧图像内所包含的特征点质量也参差不齐
关键帧
-
从普通帧中挑选出一些有代表性的帧，比如：
- 前后帧之间差别足够大
- 帧图像特征点足够丰富
- 与周围帧共视关系足够多
这些有代表性的帧就组成了关键帧。
-
优点
- 数据量明显下降，计算量小
- 关键帧的图像都具有代表性
- 鲁棒性高
-
关键帧中的每个帧都包含丰富的属性：
- 拍摄该帧时相机在世界坐标系下的位姿坐标
- 提取出来的ORB特征点
- 各种约束关系
关键帧的约束关系
-
以上图的关键帧KF(1)、KF(2)为例：
- KF(1) 的ORB特征点：ORB(1)、ORB(2)、ORB(3)、ORB(4)、ORB(5)
- KF(2) 的ORB特征点：ORB(1)、ORB(2)、ORB(3)
通过图像特征匹配，我们知道下面两个点对匹配：
- KF(1) 中的 ORB(4) 与 KF(2) 中的 ORB(1)
- KF(1) 中的 ORB(5) 与 KF(2) 中的 ORB(2)
利用多视图几何 2D-2D 模型三角化重建，这两对匹配点就能重建出地图点 P(2)、P(3)
总结
整个算法的运行过程就是在动态的维护这个地图结构。地图中的关键帧不是一成不变的：
- 有一套机制负责增加和删减关键帧中的帧数据
- 有一套机制负责增加和删减地图点中的云点数据
BA与图优化
BA（Bundle Adjustment）和图优化（Graph Optimization）都是常用的非线性优化方法，常用于视觉SLAM等领域。
BA是一种基于优化的方法，用于优化相机的位姿和场景中的三维点位置，使得重投影误差最小化。具体来说，BA将所有观测到的特征点和相机位姿作为优化变量，通过最小化重投影误差来优化相机的运动和场景中物体的三维结构。BA通常采用高斯-牛顿法或Levenberg-Marquardt算法等迭代方法进行求解。
图优化是一种基于图论的方法，用于优化图中的节点和边的状态，使得全局误差最小化。在图优化中，节点表示状态变量，边表示约束条件，节点和边的状态可以通过最小化全局误差来优化。在视觉SLAM中，图优化通常用于优化相邻关键帧之间的相机位姿和地图点的位置。图优化的求解方法包括高斯-牛顿法、LM法、增量式方法等。
BA和图优化都是非线性优化方法，它们可以通过最小化重投影误差或全局误差来优化相机位姿和场景中物体的三维结构。BA通常用于优化相机的位姿和三维点的位置，而图优化通常用于优化相邻关键帧之间的相机位姿和地图点的位置。两种方法都可以提高SLAM系统的精度和鲁棒性，但是求解速度较慢，需要进行优化和加速。
前面讲到的约束关系是构建 BA 优化的关键：
-
首先是关键帧之间的特征匹配关系能直接三角化重建出新地图点，即多视图几何
2D-2D模型 -
其次是
关键帧位姿、关键帧中的图像特征点和地图点构成的投影关系，通过最小化重投影误差来 BA 优化关键帧位姿和地图点，即多视图几何3D-2D模型 -
最后，两个关键帧能同时观测到一些共同的地图点，这就构成了共视关系，即多视图几何
3D-3D模型
地图初始化
地图初始化是指在SLAM系统启动时，生成初始地图的过程。地图初始化通常包括以下几个步骤：
特征提取：从相机图像中提取特征点，例如SIFT、ORB等算法。
特征匹配：通过特征描述子将相邻帧之间的特征点进行匹配。
运动估计：利用匹配的特征点，估计相邻帧之间的相对运动。
三角化：通过已知的相机位姿和匹配的特征点，计算场景中的三维点坐标。
地图点筛选：根据三角化结果，筛选出质量较好的地图点。
关键帧选择：根据场景中的运动状态和三角化结果，选择一些关键帧作为地图的基础，通常选择具有较多特征点和较大视角差异的帧作为关键帧。
地图点初始化：根据选定的关键帧和三角化结果，初始化地图中的地图点。
在地图初始化过程中，需要注意的是，由于初始位姿和地图点的估计误差较大，因此需要进行后续的优化和纠正。一般来说，可以通过BA（Bundle Adjustment）等非线性优化方法来优化相机位姿和地图点位置，以提高地图的精度和鲁棒性。
地图初始化是SLAM系统中非常重要的一环，它直接影响到后续的运动估计和地图更新，因此需要对其进行仔细和准确的处理。
4.2 地图初始化
1 为什么需要地图初始化
- 系统启动后，必须要现有一些地图初始云点，在这些初始云点的基础上才能往下不断增量建图
- 如果系统启动后，已经有地图，并且可以载入进行单纯的重定位，这种情形另外讨论
2 单目SLAM系统初始化
-
通过计算选取的两帧图像之间的相机位姿变换关系，以三角化重建地图初始云点，有以下方法：
-
方法1：追踪环境中某个已知物体（例如贴在墙上的标定板）
-
方法2：追踪环境中某个平面上的一些点
-
方法3：追踪环境中非共面的一些点
方法1需要依赖外界已知信息，灵活性较差，ORB-SLAM2 结合了方法2、3，并且采用了一种基于统计的模型选择策略，智能地选择更好的一个作为最终结果
-
3 地图初始化具体流程
地图初始化流程
4 地图初始化的意义
地图初始化过程的效果直接决定了整个 SLAM 系统（特别是由尺度不确定性的单目 SLAM 系统）后续的建图质量，因此地图初始化过程很重要。
ORB-SLAM2 在地图初始化过程有大量严苛的评判条件：
- 参考帧 Fr 与当前帧 Fc 之间特征点匹配度不够就直接重置初始化过程
- 根据不同初始化场景选择更优的模型
- 最后重建出的地图云点重投影误差过大就直接重置初始化过程
因为有这些严苛的评判条件，所以运行 ORB-SLAM2 的时候，第一步初始化操作就花费了很多时间
位置识别
位置识别是指在SLAM系统中，通过已知地图和传感器数据，确定相机在地图中的位置的过程。位置识别通常包括以下几个步骤：
特征提取：从相机图像中提取特征点，例如SIFT、ORB等算法。
特征匹配：将当前帧的特征点与地图中的特征点进行匹配。
运动估计：利用匹配的特征点，估计相机的运动。
位置估计：根据地图中已知的相机位姿和当前帧与地图中的匹配特征点，估计当前帧在地图中的位置。
在位置识别过程中，需要注意的是，由于传感器误差和地图不完整等原因，估计的位置可能存在一定的误差。因此，需要通过优化和纠正来提高位置识别的精度和鲁棒性。一般来说，可以利用图优化等非线性优化方法对估计的相机位姿进行优化。
位置识别是SLAM系统中的重要环节之一，它可以用于实现基于地图的导航和目标跟踪等应用。
位置识别
1 为什么要用到位置识别
重定位：如果 SLAM 建图过程中追踪线程突然跟丢了，就要启动重定位尽快找回跟丢的位置信息闭环检测：或者 SLAM 在构建了一个很大的地图后，要判断当前路过的位置是否之前已经来过
2 图像的位置识别方法
基于图像的位置识别方法，主要有以下3种：
图像到图像匹配（Image-to-Image matching）：将地图看成由一系列图像组成的序列，那么用当前观测图像与地图中的每幅图像匹配，就是图像到图像匹配（2D-2D）图像到地图的匹配（Image-to-map matching）：将地图看成一系列地图点组成的点云，那么用当前观测图像与地图点云匹配，就是图像到地图匹配（3D-2D）地图到地图的匹配（Map-to-map matching）：如果用当前几帧图像构建出来的局部小地图与全局大地图匹配，就是地图到地图的匹配
图像到图像匹配（Image-to-Image matching），在大环境下比地图到地图（Map-to-map matching）或图像到地图（Image-to-map matching）的方法的尺度特性更好。
而为了提到图像到图像的匹配效率，通常会用到 词袋模型（Bag-of-Words, BoW）。
3 词袋模型
词袋模型最早用于自然语言处理（Natural Language Processing, NLP）和信息检索领域，比如：
- 微信收录了很多公众号文章，你输入一些关键词查询出你想看的文章
- 写毕业论文的时候，查询是否有文章雷同
例子
-
假如现在两句话
Doc1: I like programming, you like too. Doc2: I alson like robots.在词袋模型中，把 Doc1 和 Doc2 的单词提取出来，并给每个单词一个编号，就构成了一个词典：
{ 1: "I", 2: "like", 3: "programming", 4: "you", 5: "too", 6: "also", 7: "robots" }按照单词在句子中出现的次数我们可以将两个句子表示为一个 7 维向量（表征向量）：
Doc1: v1=[1,2,1,1,1,0,0] Doc2: v2=[1,1,0,0,0,1,1]利用这两个向量 v 1 \pmb{v}_1 v1 和 v 2 \pmb{v}_2 v2 的相似性，就可以判断两篇文章的相似度。
4 视觉词袋模型
借鉴词袋模型的思想，把图像中的特征点看成单词，一幅图像就是由许多单词组成的文章，多张图像就构成了一个由多篇文章组成的语料库。也就是说可以将词袋模型应用于计算机视觉领域，即视觉词袋模型。
从图像中可以提取出多种特征点（比如 SIFT、SURF、ORB等），每种特征点都对应一种视觉词袋模型。由于 ORB 是具有旋转和尺度不变性的二进制特征，并且提取速度特别快，ORB-SLAM 选了这种特征用于整个系统的图像处理
具体实现库：DBoW2
构建视觉单词的离线字典
构建视觉单词的离线字典是视觉SLAM中的一个重要步骤，它可以有效地减少实时计算量，提高系统的运行效率。下面是一个基本的离线字典构建流程：
特征提取：对数据集中的所有图像进行特征提取，例如SIFT、ORB等算法。
特征聚类：将所有提取的特征点聚类成一个视觉单词集合。常用的聚类方法包括K-Means、GMM等。
生成离线字典：将聚类后的视觉单词集合作为字典保存下来，供后续使用。
在构建离线字典时，需要注意的是，字典的大小和特征点的数量应该平衡。如果字典过大，可能会导致运行时间增加；如果字典过小，可能会导致匹配精度下降。因此，在构建离线字典时，需要进行合理的参数选择和实验验证。
离线字典构建完成后，可以将其用于实时的视觉单词匹配，以减少计算量和提高系统的实时性。
1 字典
使用词袋模型时，需要先训练得到一个字典。
ORB 特征描述子为一个 256 b i t 256bit 256bit 的二进制向量，也就是说 ORB 特征在向量空间的取值有 2 256 2^{256} 2256 种可能。
训练过程：
- 选取一个庞大的数据集（例如 10 万张图像）
- 将数据集中提取出的所有
ORB 特征点放入向量空间 - 采用
海明距离度量向量空间中特征点之间的距离 - 对特征点进行聚类
- 将向量中距离相近的 k 个特征点归结为同一类
- 并用一个平均特征来表示整体，这个平均特征就是一个单词。
- K-Means
- K-Means++
- SVM 等
2 字典的树结构
假设每张图形提取 200 个特征点，整个数据集一共可提取 M = 1 0 5 × 200 M=10^5\times 200 M=105×200 个特征点，经过 k-means 聚类后，可以得到 M k \frac{M}{k} kM 个不同的单词，这个单词量很庞大，如果要查 某个特征点 属于哪个 单词，搜索复杂度将非常高。
为了提高搜索效率，我们将 单词 再一次进行聚类，经过 d 次聚类后，就得到了一个具有 d 层的 k 叉树，树的分支数 k 和层数可以根据需要选择。
- 三角形：特征点
- 正方形：单词
- 五角形：的第二次聚类的结果
- …
聚类
3 单词的权重（IDF）
在训练该 k 叉树字典时，还要给字典中的每一个单词赋予一个权重，通常为IDF（Inverse Document Frequency，逆文本频率指数）。
例如我们语言中有大量的连接词和助词，这些词没有实质性的意义并且出现频次还很高，即一个词越常见其独特性越低，也就是说其重要性越低。
同样在视觉中，哪些在图像训练集中越常见的单词，重要性越低，单词重要性权重 IDF 公式如下：
I D F i = l o g n n i IDF_i = log\frac{n}{n_i} IDFi=lognin
利用大量的图像训练数据，经过漫长的训练就构件好了一个视觉单词的离线字典。这个离线字典包含：
- 用于组织视觉单词（也就是叶子节点）的 k 叉树
- 每个单词所对应的重要性权重（即IDF值）
构建图像序列的在线数据库
构建图像序列的在线数据库是视觉SLAM中的一个重要步骤，它可以提供数据管理和快速检索的功能，帮助SLAM系统更高效地进行定位和地图构建。下面是一个基本的在线数据库构建流程：
图像序列采集：使用相机或其他传感器采集图像序列，保存为图像文件。
图像预处理：对采集的图像进行预处理，例如去除畸变、裁剪等操作。
特征提取：对预处理后的图像进行特征提取，例如SIFT、ORB等算法。
特征匹配：将相邻帧之间的特征点进行匹配，并保存匹配结果。
数据库构建：将匹配结果和图像序列等信息保存到数据库中，以便后续检索和使用。
在构建在线数据库时，需要考虑数据库的数据结构、索引和查询等方面。常用的数据库包括MySQL、PostgreSQL、MongoDB等，可以根据应用场景和需求进行选择。在查询时，可以利用哈希表、R树等数据结构快速定位和检索相关数据。
在线数据库的构建可以帮助SLAM系统更高效地管理和检索数据，提高系统的实时性和鲁棒性。同时，也可以为后续的数据处理和应用提供便利。
应用词袋模型
词袋模型是一种常用的文本表示方法，它将文本看作是一组词的集合，忽略词与词之间的顺序和语法关系，仅仅关注每个词在文本中出现的频率。在视觉SLAM中，词袋模型也被广泛应用于图像表示和场景理解等方面。下面介绍几个应用词袋模型的例子：
视觉单词表示：将图像中的特征点表示为视觉单词，利用词袋模型将视觉单词进行聚类，生成视觉单词字典。对于新的图像，可以利用视觉单词字典将其表示为一个词频向量，用于图像检索和匹配等任务。
场景分类：将场景中的图像表示为词频向量，利用机器学习算法训练分类器，实现场景分类和识别等任务。
目标检测：将图像中的区域表示为词频向量，利用机器学习算法训练目标检测器，实现目标检测和分割等任务。
可视化定位：通过对图像进行词袋特征提取和匹配，实现图像与地图的匹配和定位，用于自主导航和机器人定位等应用。
在应用词袋模型时，需要注意选择合适的特征和算法，并进行参数调优和实验验证。同时，也需要考虑模型的复杂度和计算效率等因素，以保证系统的实时性和鲁棒性。
追踪线程
在视觉SLAM系统中，追踪线程是一个重要的模块，它主要负责实时地跟踪相机的运动和更新位姿估计。具体来说，追踪线程通常包括以下几个步骤：
特征提取：从当前帧中提取特征点，例如SIFT、ORB等算法。
特征匹配：将当前帧的特征点与地图中的特征点进行匹配，并估计相机的位姿变化。
姿态更新：利用位姿变化信息，更新相机的位姿估计，并将当前帧加入地图中。
位姿优化：利用非线性优化算法对相机的位姿进行优化，以提高位姿估计的精度和稳定性。
追踪线程的目标是实现实时的位姿估计和跟踪，因此需要考虑计算效率和鲁棒性等因素。为了提高系统的鲁棒性，追踪线程通常会采用多种技术，例如光流、匀速模型、图像边缘信息等，来应对光照变化、运动模糊等复杂情况。
在实现追踪线程时，还需要考虑线程同步、数据共享等问题。通常情况下，追踪线程和地图构建线程是并行运行的，需要进行有效的线程同步和数据传递，以确保系统的正确性和稳定性。
追踪线程
追踪线程主要任务
- 追踪线程从相机获取输入图像，进行ORB特征提取
- 单目相机需要先完成地图初始化，在已有地图云点的基础上才能执行追踪任务
- 输出每帧图像的相机位姿信息用于定位
- 从众多的输入的图像中挑选具有代表性的关键帧（给接下来的局部建图线程）
1 特征提取
每个从相机输入到追踪线程的图像都要先提取 ORB 特征，特征提取步骤包括：
- 构建图像金字塔
- 提取 FAST 及欧典
- 计算特征描述子
ORB 为了让特征点在图像中分布尽量均匀，这里的提取过程加入了一个格外要求，就是将图像划分成网格，每个网格内必须要提取出一定数量的角点
2 定位
追踪的目的就是为了实时确定相机在世界坐标系中的位姿，也就是定位。而定位常常分为：
- 粗定位：快速获得一个不太精确的初始位姿估计
- 精定位：在初始位姿估计（粗定位）的基础上通过复杂运算得到更精确的位姿估计
2.1 初始位姿估计（粗定位）
初始位姿估计涉及 3 个计算模型，它们的关系就是不断扩大追踪搜索范围。每个计算模型的追踪实现基本都一样：
- 先让当前帧与追踪帧匹配
- 接着构建多视图几何中的 3 D − 2 D 3D-2D 3D−2D 模型，用 P n P PnP PnP 算法求解
- 对解出来的 T \pmb{T} T 进一步优化
- 最后判断追踪成功与否
（1）追踪上一帧 TODO
-
如果上一帧输入的图像 F k − 1 F_{k-1} Fk−1 被追踪成功了，那么假设相机以 F k − 1 F_{k-1} Fk−1 时刻的速度匀速运动，得到了当前帧图像 F k F_k Fk。
-
其实就是匀速模型，过程如下
- 利用 F k − 1 F_{k-1} Fk−1 时刻的角速度和线速度乘以时间间隔，就可以估算出 F k − 1 F_{k-1} Fk−1 到 F k F_k Fk 相机的 T \pmb{T} T
- 将图像 F k − 1 F_{k-1} Fk−1 中观测到的地图云点，通过 T \pmb{T} T 变换到图像 F k F_k Fk 坐标系
- 利用地图云点投影到当前图像 F k F_k Fk 的关系
- 最小化重投影误差来进一步解算 T \pmb{T} T 更精确的值
-
这就是 3 D − 2 D 3D-2D 3D−2D 模型，可以用 P n P PnP PnP 问题的各种算法进行求解。
（2）追踪参考关键帧
-
如果相机运动太快或者相机运动过程中有抖动等因素，导致上面用匀速模型计算出的 T \pmb{T} T 不正确（匀速模型追踪失败）。
-
这时要扩大追踪的搜索范围，及局部地图上的一系列关键帧。
-
利用词袋模型：
- 首先将当前帧图像 F k F_k Fk 转换成表征向量
- 然后利用表征向量将当前帧图像 F k F_{k} Fk 与局部地图中的一些列关键帧图像进行快速匹配
- 局部地图中与当前帧匹配度最高的帧被称为
参考关键帧。
-
如果参考关键帧与当前帧的匹配较好（匹配点对数不小于15），就可以利用待求量 T \pmb{T} T 将参考帧中观测到的地图云点变换到图像 F k 1 F_{k1} Fk1 坐标系
-
然后利用地图云点投影到当前图像 F k F_{k} Fk 的关系
-
最小化重投影误差来求 T \pmb{T} T
-
这就是 3 D − 2 D 3D-2D 3D−2D 模型，可以用 P n P PnP PnP 问题的各种算法进行求解。
（3）重定位
-
如果参考关键帧与当前帧的匹配并不好，或者求出的位姿上观测不到足够的地图点，那么参考关键帧追踪也就失败了。这时就要进行重定位
-
首先利用词袋模型将当前帧图像 F k F_{k} Fk 转换成表征向量
-
然后到全局关键帧的在线数据库中检索出相似度较高的一些关键帧——
候选关键帧 -
将 F k F_k Fk 与候选关键帧中的每个帧进行匹配，挑出匹配较好（匹配点对数 ≥ 15 \geq 15 ≥15 ）
-
P n P PnP PnP 的 R A N S A C RANSAC RANSAC 迭代（这里用的是 P 4 P P4P P4P）
P n P PnP PnP 的 R A N S A C RANSAC RANSAC
- 将每一个从候选关键帧挑出来的
匹配较好的帧所能观测到的地图云点准备好 - 用 R A N S A C RANSAC RANSAC 将上一步的地图云点，选 4 个在同一帧上的云点
- 利用 P 4 P P4P P4P 求出对应的 T \pmb{T} T
- 将每一个从候选关键帧挑出来的
-
每次 R A N S A C RANSAC RANSAC 迭代后，都要判断在求出的当前帧图像 F k F_k Fk 位姿上能否观测到足够多的地图云点（ ≥ 50 \geq50 ≥50），如果 ≥ 50 \geq50 ≥50，立刻结束 R A N S A C RANSAC RANSAC 迭代，说明重定位成功
-
如果经过所有的 R A N S A C RANSAC RANSAC 迭代后，重定位仍然不成功，就说明这是追踪处于彻底丢失（LOST）状态，只能将整个 SLAM 系统重启
总结
2.2 局部地图追踪（精定位）
在上面的初始位姿估计中
- 采用匀速模型追踪上一帧图像
- 在局部地图上追踪参考关键帧
- 全局地图上重定位
本质上都是利用当前帧与追踪帧之间的 共视地图云点 来求相机位姿。显然两帧图像之间的共视点是很有限的，即当前帧中提取出的很多特征点并没有用于构建共视关系
局部地图追踪，就是在初始位姿估计完成的基础上，利用当前帧与局部地图上的多个关键帧建立共视关系，并利用所有这些共视地图云点与当前帧的投影关系，对相机的位姿进行更精确求解。
当然要根据一些严苛的条件先将共视地图云点中的一些外点去除，然后构建多视图几何中的 3D-2D 模型，最小化重投影误差对 T \pmb{T} T 进行优化求解
3 新关键帧挑选
追踪线程最后一步就是新关键帧的挑选，按照一定的条件尽可能快速从普通帧中挑选出有代表性的帧
挑选关键帧的条件：
- 与上一个挑选的关键帧有足够的时间间隔
- 帧中要能观测到足够多的地图云点等
这里的快速挑选指初步的筛选，在局部建图线程中还要经过严苛的条件判断是否将其纳入地图中
局部建图线程
局部建图线程是视觉SLAM系统中的一个模块，其主要任务是在地图中添加新的地标点和更新地标点的位置，以提高地图的精度和稳定性。具体来说，局部建图线程通常包括以下步骤：
关键帧选择：从地图中选择一些关键帧，这些关键帧包含了重要的地标点和位姿信息。
地标点三角化：利用选定的关键帧和当前帧中的特征点，进行地标点三角化，估计新的地标点的位置。
局部地图更新：将新的地标点添加到局部地图中，并更新地标点的位置。
位姿优化：对选定的关键帧和新的地标点进行非线性优化，以提高位姿估计的精度和稳定性。
地图管理：将局部地图合并到全局地图中，并删除一些不再需要的地标点和关键帧，以减少地图的存储和计算负担。
局部建图线程的目标是实现高质量的地图建立，因此需要考虑地标点的质量、位姿估计的精度和鲁棒性等因素。为了提高地图的精度和稳定性，局部建图线程通常会采用多种技术，例如BA优化、数据关联、地标点筛选等，来应对噪声、误差等问题。
在实现局部建图线程时，还需要考虑线程同步、数据共享等问题。通常情况下，局部建图线程和追踪线程是并行运行的，需要进行有效的线程同步和数据传递，以确保系统的正确性和稳定性。同时，还需要考虑地图的存储和计算负担，以提高系统的效率和可扩展性。
局部建图线程
追踪线程挑选 关键帧 的速度较快，所以被挑选出来的关键帧存放在 缓冲区 内。局部建图线程不断从该缓冲区去取关键帧进行处理，更新当下的局部地图，并将每一个处理过的当前关键帧输出给闭环检测线程。
局部建图线程主要包括：
- 关键帧插入
- 近期地图点筛选
- 新地图云点重建
- 局部 BA 优化
- 局部关键帧筛选
1 关键帧插入
每个从缓冲区取出来的关键帧
- 首先需要在词袋模型中计算表征向量，其实就是将该关键帧更新到词袋模型的在线数据库，以便后续计算之用
- 然后将地图中的哪些有共视关系但没有与该帧建立映射的云点建立关联，这些新建立关联的云点被称为
近期地图点 - 接着计算该关键帧与地图中的已有关键帧的共视连接权重，其实就是新建连接边，将该关键帧添加到共视关系图
- 最后将该关键帧添加到地图结构之中，这个关键帧插入就完成了
2 近期地图点筛选
在关键帧的插入过程中，保留了一些近期地图点，必须将其中一些质量差的云点剔除，比如被同时观测的关键帧数少、上一个能观测到的关键帧距离当前帧太久远等，这是维护地图点鲁棒性的重要机制
3 新地图云点重建
对于每一个新插入的关键帧，借助共视关系图与邻近的关键帧进行匹配，将该关键帧中还未映射到地图云点的特征点进行三角化重建，以生成新地图云点。
这个过程其实就是多视图几何中的 2D-2D 模型。
重建出的新地图点，如果检验合格就被添加到地图结构，同时被保留为近期地图点。因为缓冲区中关键帧的插入操作需要进行多次循环，这个过程在逻辑上实际是相互包含和穿插的
4 局部 BA 优化
缓冲区的关键帧都被取出来并处理后，就可以将当前帧局部的几个关键帧以及地图云点放入局部 BA 中优化。我们把各个关键帧和地图云点看成节点，一致其初始位姿；而关键帧与关键帧之间的约束边由公式关系给出，关键帧与地图云点之间的约束由投影关系给出
5 局部关键帧筛选
当局部 BA 优化完成后，需要对局部地图中的关键帧进行一次筛选，将哪些冗余的关键帧剔除，以保证地图中关键帧的鲁棒性
6 总结
上面的5个步骤并不是简单的顺序执行，因为缓冲区中有多个关键帧需要处理，所以这5 个步骤实际上是以循环和条件判断混合的方式出现
闭环线程
闭环线程是视觉SLAM系统中的一个模块，其主要任务是检测和处理环路闭合，以提高地图的精度和鲁棒性。具体来说，闭环线程通常包括以下步骤：
关键帧选择：从历史的关键帧中选择一些候选帧，这些候选帧与当前帧可能存在闭环。
相似性度量：利用候选帧和当前帧的特征点，计算相似性度量，例如BoW、VLAD、FAB-MAP等算法，以判断是否存在闭环。
闭环检测：基于相似性度量的结果，使用RANSAC等算法进行闭环检测，以确定闭环帧和当前帧之间的变换关系。
位姿图优化：将闭环帧和与之相关的帧加入位姿图中，利用非线性优化算法（如G2O、ceres等）进行全局位姿优化，以提高地图的精度和鲁棒性。
地图更新：对地图中的位姿和地标点进行更新，以反映闭环检测的结果。
闭环线程的目标是提高地图的精度和鲁棒性，因此需要考虑相似性度量的准确性、闭环检测的鲁棒性、位姿优化的精度等因素。为了提高闭环线程的效率和鲁棒性，闭环线程通常会采用多种技术，例如分层检测、多假设检测、增量式位姿优化等。
在实现闭环线程时，还需要考虑线程同步、数据共享等问题。通常情况下，闭环线程和地图构建线程是串行运行的，需要进行有效的线程同步和数据传递，以确保系统的正确性和稳定性。同时，还需要考虑闭环帧的选择和地图更新的策略，以提高系统的效率和可扩展性。
当局部建图线程中处理后的每个当前关键帧，都要送入闭环线程，一旦回环检测通过，就对全局地图进行回环修正。闭环线程主要包括候选回环、计算相似变换、回环融合和位姿图优化
1 候选回环
利用词袋模型，将数据库中与当前关键帧相似度较高的帧挑选出来，这些帧就是候选回环帧。
2 计算相似变换
计算当前关键帧与每个候选回环帧之间的变换关系，因为单目 SLAM 存在尺度漂移问题，所以当前关键帧与候选回环帧之间的变换出来包含旋转平移 [ R ∣ t ] [\pmb{R} | \pmb{t}] [R∣t] 外，还包含一个尺度因子 s。也就是说，与计算相邻两帧间的变换 T \pmb{T} T 不同，这里计算的是相似变换，具体计算方法与多视图几何的 3D-3D 模型类似。
如果有足够多的数据能计算出相似变换 [ s R ∣ t ] [s\pmb{R} | \pmb{t}] [sR∣t]，并且该变换能保证当前关键帧与候选回环帧之间有足够多的共视点，则接纳这个候选回环帧，回环检测成功
3 回环融合
当前关键帧与被接纳的候选回环帧之间的相似变换量描述了累积误差的大小，可以利用该变换量修正当前关键帧及其邻近关联帧的累积误差，并将哪些因累积误差而不一致的地图点融合到一起
4 位姿图优化
虽然回环融合可以修正当前关键帧及其邻近关联帧的累积误差，但是地图中那些与当前关键帧无共视关系的帧还没有得到修正，因此需要用全局优化来修正。
考虑计算效率的问题，这里的全局优化只将全局地图上的关键帧位姿量当成优化变量，而地图点并不是优化变量，这种优化也称为位姿图优化
源码解析
源码解析
ORB_SLAM2是一个基于ORB特征点的实时单目、双目和RGB-D SLAM系统，是SLAM领域中一个非常流行和广泛使用的开源实现。ORB_SLAM2/Examples是ORB_SLAM2提供的一些示例程序，用于演示ORB_SLAM2的使用和功能。
ORB_SLAM2/Examples中包含了多个示例程序，包括单目、双目和RGB-D SLAM的演示程序，以及相机标定、地图保存和加载等辅助程序。这些示例程序可以帮助用户理解ORB_SLAM2的内部结构和算法原理，以及如何使用ORB_SLAM2进行SLAM任务。
ORB_SLAM2/Examples的使用非常简单，用户只需编译示例程序并运行即可。在运行示例程序时，用户可以选择不同的输入数据源，例如摄像头、图像序列、ROS话题等，以演示ORB_SLAM2在不同场景下的使用。同时，ORB_SLAM2/Examples还提供了可视化界面，方便用户观察SLAM结果和调试算法。
总的来说，ORB_SLAM2/Examples是一个非常有用的工具，可以帮助用户快速入门ORB_SLAM2，了解SLAM算法的实现和应用。同时，用户还可以基于ORB_SLAM2/Examples进行二次开发，实现更加复杂和高效的SLAM应用。
ORB_SLAM2是一个基于ORB特征点的实时单目、双目和RGB-D SLAM系统，其源码实现是一个非常复杂和庞大的系统。以下是ORB_SLAM2源码的一些主要模块和算法：
- 特征提取与描述：ORB_SLAM2使用ORB特征点作为SLAM系统的关键信息，使用OpenCV库中的ORB算法进行特征点提取和描述。在双目和RGB-D SLAM中，ORB_SLAM2还使用了立体匹配和深度信息来计算3D点的位置。
- 相机位姿估计：ORB_SLAM2使用RANSAC算法进行相机位姿估计，通过匹配关键点和地图点来计算相机的位姿。在双目和RGB-D SLAM中，ORB_SLAM2还使用了三角化算法和PnP算法来进一步提高位姿估计的精度。
- 地图构建与更新：ORB_SLAM2使用基于关键帧的SLAM方法，将关键帧和地图点组成位姿图，使用非线性优化算法进行地图构建和更新。ORB_SLAM2还使用了闭环检测和位姿图优化等技术，提高地图的精度和鲁棒性。
- 系统架构与并行计算：ORB_SLAM2的系统架构非常复杂，包括多个线程和模块，主要包括特征提取、跟踪、地图构建和闭环检测等模块。为了提高ORB_SLAM2的实时性，ORB_SLAM2还使用了多线程和并行计算等技术。
- 可视化界面和配置文件：为了方便用户使用和调试ORB_SLAM2，ORB_SLAM2还提供了可视化界面和配置文件。用户可以通过可视化界面观察SLAM结果和调试算法，通过配置文件调整系统参数和算法选项。
总的来说，ORB_SLAM2源码实现非常复杂和庞大，需要涉及多种算法和技术，包括特征点提取、相机位姿估计、地图构建和更新、系统架构和并行计算等方面。理解ORB_SLAM2源码的实现细节和算法原理，需要具备较强的计算机视觉和SLAM领域的专业知识。
intro
Intro
ORB_SLAM2/Examples 文件夹中提供了多个 main() 函数实现源文件，这些源文件可以从3个方面来分类。
- 从是否支持ROS接口方面，可以分为非ROS例程和ROS例程
- 从传感器类型方面，可以分为单目、双目和RGB-D例程
- 从数据输入方式方面，可以分为从数据集获取数据、从传感器直接获取数据和从ROS话题获取数据。
本质上讲这些例程的区别仅在于数据输入方式不同，而最终都通过调用ORB-SLAM2核心库中的System类来启动。
System类
System类
System 类是整个 ORB-SLAM2 核心库的顶层接口，Examples文件夹中所有例程的 main() 函数都会创建一个 System 类的对象。
然后System 类调用其它构造函数完成
- 载入设置参数文件
- 配置文件 .yaml
- 载入 ORB 字典文件
- ORBVoc.txt
- 创建词袋数据
- KeyFrameDatabase 类
- 创建地图数据结构
- Map 类
- 创建地图显示对象
- FrameDrawer 类
- MapDrawer 类
- 创建3个主线程
- 追踪线程：Tracking 类的 Tracking.Track()
- 局部建图线程：LocalMapping 类的 LocalMapping.Run()
- 闭环线程：LoopClosing 类的 LoopClosing.Run()
- 创建界面显示线程
- Viewer类的 Viewer.Run()
- 设置线程间指针
- 让
追踪线程、局部建图线程、闭环线程、界面显示线程能互相访问
- 让
最后 System 系统主线程在图像输入数据的驱动下运行，按照传感器数据的类型分为3种：
- 单目
- 双目
- RGB-D
他们都会调用 Tracking 类的成员函数 Track()
//注意变量命名方式，类的变量有前缀m，如果这个变量是指针类型还要多加个前缀p，
//如果是进程那么加个前缀t
// Input sensor
// 传感器类型
eSensor mSensor;
// ORB vocabulary used for place recognition and feature matching.
// 一个指针指向ORB字典
ORBVocabulary* mpVocabulary;
// KeyFrame database for place recognition (relocalization and loop detection).
// 关键帧数据库的指针，这个数据库用于重定位和回环检测
KeyFrameDatabase* mpKeyFrameDatabase;
//指向地图（数据库）的指针
// Map structure that stores the pointers to all KeyFrames and MapPoints.
Map* mpMap;
// Tracker. It receives a frame and computes the associated camera pose.
// It also decides when to insert a new keyframe, create some new MapPoints and
// performs relocalization if tracking fails.
// 追踪器，除了进行运动追踪外还要负责创建关键帧、创建新地图点和进行重定位的工作。详细信息还得看相关文件
Tracking* mpTracker;
// Local Mapper. It manages the local map and performs local bundle adjustment.
//局部建图器。局部BA由它进行。
LocalMapping* mpLocalMapper;
// Loop Closer. It searches loops with every new keyframe. If there is a loop it performs
// a pose graph optimization and full bundle adjustment (in a new thread) afterwards.
// 回环检测器，它会执行位姿图优化并且开一个新的线程进行全局BA
LoopClosing* mpLoopCloser;
// The viewer draws the map and the current camera pose. It uses Pangolin.
// 查看器，可视化 界面
Viewer* mpViewer;
//帧绘制器
FrameDrawer* mpFrameDrawer;
//地图绘制器
MapDrawer* mpMapDrawer;
// System threads: Local Mapping, Loop Closing, Viewer.
// The Tracking thread "lives" in the main execution thread that creates the System object.
//系统除了在主进程中进行运动追踪工作外，会创建局部建图线程、回环检测线程和查看器线程。
std::thread* mptLocalMapping;
std::thread* mptLoopClosing;
std::thread* mptViewer;
// Reset flag
//复位标志，注意这里目前还不清楚为什么要定义为std::mutex类型 TODO
std::mutex mMutexReset;
bool mbReset;
// Change mode flags
//模式改变标志
std::mutex mMutexMode;
bool mbActivateLocalizationMode;
bool mbDeactivateLocalizationMode;
// Tracking state
// 追踪状态标志，注意前三个的类型和上面的函数类型相互对应
int mTrackingState;
std::vector mTrackedMapPoints;
std::vector mTrackedKeyPointsUn;
std::mutex mMutexState;
构造函数
System 构造函数（.h部分）
// Input sensor
//这个枚举类型用于 表示本系统所使用的传感器类型
enum eSensor{
MONOCULAR=0,
STEREO=1,
RGBD=2
};
System(const string &strVocFile, //指定ORB字典文件的路径 voc.txt 文件
const string &strSettingsFile, //指定配置文件的路径 .yaml 文件
const eSensor sensor, //指定所使用的传感器类型 上面的枚举选项
const bool bUseViewer = true); //指定是否使用可视化界面 true或false
System 构造函数（.cc部分）
// 系统的构造函数，将会启动其他的线程
System::System(const string &strVocFile, //词典文件路径
const string &strSettingsFile, //配置文件路径
const eSensor sensor, //传感器类型
const bool bUseViewer): //是否使用可视化界面
mSensor(sensor), //初始化传感器类型
mpViewer(static_cast(NULL)), //空。。。对象指针？
mbReset(false), //无复位标志
mbActivateLocalizationMode(false), //没有这个模式转换标志
mbDeactivateLocalizationMode(false) //没有这个模式转换标志
{
// Output welcome message
cout << endl <<
"ORB-SLAM2 Copyright (C) 2014-2016 Raul Mur-Artal, University of Zaragoza." << endl <<
"This program comes with ABSOLUTELY NO WARRANTY;" << endl <<
"This is free software, and you are welcome to redistribute it" << endl <<
"under certain conditions. See LICENSE.txt." << endl << endl;
// 输出当前传感器类型
cout << "Input sensor was set to: ";
if(mSensor==MONOCULAR)
cout << "Monocular" << endl;
else if(mSensor==STEREO)
cout << "Stereo" << endl;
else if(mSensor==RGBD)
cout << "RGB-D" << endl;
// ------------------ （1）载入设置参数文件 ---------------
//Check settings file
cv::FileStorage fsSettings(strSettingsFile.c_str(), //将配置文件名转换成为字符串
cv::FileStorage::READ); //只读
//如果打开失败，就输出调试信息
if(!fsSettings.isOpened())
{
cerr << "Failed to open settings file at: " << strSettingsFile << endl;
//然后退出
exit(-1);
}
// ------------------- （2）载入ORB字典文件 -----------------
//Load ORB Vocabulary
cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl;
//建立一个新的ORB字典
mpVocabulary = new ORBVocabulary();
//获取字典加载状态
bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);
//如果加载失败，就输出调试信息
if(!bVocLoad)
{
cerr << "Wrong path to vocabulary. " << endl;
cerr << "Falied to open at: " << strVocFile << endl;
//然后退出
exit(-1);
}
//否则则说明加载成功
cout << "Vocabulary loaded!" << endl << endl;
// --------------------（3）创建词袋数据库 ---------------
//Create KeyFrame Database
//
mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
// --------------------（4）创建地图数据结构 ---------------
//Create the Map
mpMap = new Map();
// --------------------（5）创建地图显示对象 ---------------
//Create Drawers. These are used by the Viewer
//这里的帧绘制器和地图绘制器将会被可视化的Viewer所使用
mpFrameDrawer = new FrameDrawer(mpMap);
mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);
// --------------------（6）创建3个线程 ---------------
//在本主进程中初始化追踪线程
//Initialize the Tracking thread
//(it will live in the main thread of execution, the one that called this constructor)
mpTracker = new Tracking(this, //现在还不是很明白为什么这里还需要一个this指针 TODO
mpVocabulary, //字典
mpFrameDrawer, //帧绘制器
mpMapDrawer, //地图绘制器
mpMap, //地图
mpKeyFrameDatabase, //关键帧地图
strSettingsFile, //设置文件路径
mSensor); //传感器类型iomanip
//初始化局部建图线程并运行
//Initialize the Local Mapping thread and launch
mpLocalMapper = new LocalMapping(mpMap, //指定使iomanip
mSensor==MONOCULAR); // TODO 为什么这个要设置成为MONOCULAR？？？
//运行这个局部建图线程
mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run, //这个线程会调用的函数
mpLocalMapper); //这个调用函数的参数
//Initialize the Loop Closing thread and launchiomanip
mpLoopCloser = new LoopClosing(mpMap, //地图
mpKeyFrameDatabase, //关键帧数据库
mpVocabulary, //ORB字典
mSensor!=MONOCULAR); //当前的传感器是否是单目
//创建回环检测线程
mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, //线程的主函数
mpLoopCloser); //该函数的参数
// --------------------（7）创建界面显示线程 ---------------
//Initialize the Viewer thread and launch
if(bUseViewer)
{
//如果指定了，程序的运行过程中需要运行可视化部分
//新建viewer
mpViewer = new Viewer(this, //又是这个
mpFrameDrawer, //帧绘制器
mpMapDrawer, //地图绘制器
mpTracker, //追踪器
strSettingsFile); //配置文件的访问路径
//新建viewer线程
mptViewer = new thread(&Viewer::Run, mpViewer);
//给运动追踪器设置其查看器
mpTracker->SetViewer(mpViewer);
}
// --------------------（8）设置进程间指针 ---------------
//Set pointers between threads
//设置进程间的指针
mpTracker->SetLocalMapper(mpLocalMapper);
mpTracker->SetLoopClosing(mpLoopCloser);
mpLocalMapper->SetTracker(mpTracker);
mpLocalMapper->SetLoopCloser(mpLoopCloser);
mpLoopCloser->SetTracker(mpTracker);
mpLoopCloser->SetLocalMapper(mpLocalMapper);
}
FrameKeyFrameMapPointMap
Frame、KeyFrame、MapPoint、Map
- 地图数据结构的 4 个类
流程
- Frame：传感器输入的每个图像，建立一个对应的 Frame
- 追踪线程：特征提取、特征匹配、三角化重建…
- Frame 生成多个 MapPoint
- KeyFrame：通过关键帧筛选机制，将符合要求的一些 Frame 筛选出来成为 KeyFrame
- 原 Frame 包含的 MapPoint 经过严格筛选后，成为 KeyFrame 中的 MapPoint
- Map：由 KeyFrame 中的 MapPoint 经过进一步的筛选后，成为 Map 中的MapPoint
- 这些 MapPoint 和 KeyFrame 共同构成了整个 Map
Initializer类
因为双目和RGBD的地图初始化比较简单，所以没有封装成专门的类。Initialize 是专门给单目封装的。
地图初始化通过 Initialize 类的成员函数 Initialize() 实现.
地图初始化流程
- 匹配两帧图像
- 将两帧图像送到单应矩阵
H模型（FindHomography）和基础矩阵F模型（FindFundamental）中 - 两个模型计算出相应的矩阵，并给计算结果打分
- 打分高的模型继续执行
- 选择单应矩阵 H模型：调用用 ReconstructH() 函数，对矩阵 H \pmb{H} H 进行分解，求出 R \pmb{R} R 和 t \pmb{t} t，在 R \pmb{R} R 和 t \pmb{t} t 可靠的条件下（CheckRT函数）执行三角化重建
- 选择基础矩阵 F模型：调用用 ReconstructF() 函数，对矩阵 F \pmb{F} F 进行分解，求出 R \pmb{R} R 和 t \pmb{t} t，在 R \pmb{R} R 和 t \pmb{t} t 可靠的条件下（CheckRT函数）执行三角化重建
KeyFrameDatabase类
ORBextractorORBmatcher类
PnPSolver类
Sim3Solver类
Optimizer类
Tracking类
Tacking 类
- 实现追踪线程
Tracking类流程
-
载入相机和ORB设置参数
-
输入图像
- 单目图像：GrabImageMonocular
- 双目图像：
- RGBD图像：
-
输入图像经过 Frame() 提取特征
-
送入 Track() 函数
- 先判断系统是否已经完成初始化，并按情况选择初始化方法
- MonocularInitialization()
- StereoInitialization()
- 判断系统处于哪种工作模式（Mapping 或 Localization），并逐步尝试初位姿估计的3种方法：
-
TrackWithMotionModel()
-
TrackReferenceKeyFrame()
-
Relocalization()
-
- 调用 TrackLocalMap() 进行局部地图追踪
- 先判断系统是否已经完成初始化，并按情况选择初始化方法
LocalMApping类
- 实现局部建图的线程
LocalMapping 类工作流程
-
Run() 函数放在后台线程中持续运行
-
每个新关键帧都会依次进行
-
关键帧插入：ProcessNewKeyFrame()
-
近期地图点筛选：MapPointCulling()
-
新地图云点重建：CreateNewMapPoints()
-
-
当缓冲区的所有新关键帧被处理完后，就可以进行局部 BA 优化和局部关键帧筛选
LoopClosing类
- 实现闭环线程
LoopClosing
- LoopClosing 类的 LoopClosing.Run() 函数在后台持续运行
- 每个当前关键帧：
- 通过 DetectLoop() 函数进行回环检测
- 调用 ComputeSime3() 函数计算
候选回环帧与当前关键帧的相似变换
- 一旦
候选回环帧被确认为回环，就调用 CorrectLoop() 函数进行回环修正 - 回环修正
- 先进行回环融合
- 然后进行全局优化（两个实现版本）
- 精简全局 BA 优化
- 完全版全局 BA 优化（ORB-SLAM2 放在第4个线程中单独运行