ORBSLAM3整体框架

一、前言

        笔者认真学习了一年半有余的SLAM，期间学习过V-SLAM、VI-SLAM、激光SLAM和激光+IMU的SLAM，在看论文和代码的闲暇之余，会时常逛知乎和CSDN等博客膜拜大佬们的文章，回顾整个过程，似乎自己没有在相关博客留下痕迹，也深刻体会到“好记性不如烂笔头”的警示作用，因此想把自己学过的东西进行总结，也抱着向大家学习的态度，文章不足之处欢迎大家批评指正。由于ORBSLAM3的SLAM功能比较完备，因而笔者的第一篇文章想对其进行解读。
        ORBSLAM3是2020年开源的视觉惯性SLAM框架，定位精度优于其他视觉惯性SLAM系统，并且包含了单目、双目、RGB-D、单目+IMU、双目+IMU和RGB-D+IMU六种模式，多地图系统也契合目前的SLAM鲁棒感知发展时期，笔者自己用相机实测的效果也不错。ORBSLAM框架系列设计了一套命名规则，整体代码可读性很友好，包含了许多算法原理细节，而不是直接调用OpenCV，比如ORB特征、对极几何和PnP等，很值得学习。本文将对ORBSLAM3的大体框架进行总结，后续也会写文章对系统的各个部分的算法原理进行解读。

二、系统枢纽

        系统的程序入口在文件Examples和Examples_old 下，以System类充当整个系统的枢纽，在System类构造的同时，开启了跟踪、局部建图、回环检测和Pangolin可视化四个线程，各线程之间通过指针互相指向的方式进行内存共享，便于系统进行管理。如图1所示，系统的整个框架，跟踪部分(Tracking)主要负责完成位姿的初始估计，保证状态估计的实时性;局部建图(Local Mapping)线程主要负责对跟踪线程传入的关键帧位姿及对应的地图点进行优化，IMU模式下，还会以最大后验估计的方式对系统进行初始化;回环检测(Loop & Map merging)线程则用于矫正系统的累积误差，ORBSLAM3由于添加了多地图管理功能，因此回环检测线程还包括了地图合并功能;可视化线程主要通过调用基于OpenGL的Pangolin对系统的关键帧、地图点、图结构以及一些系统操作按钮进行可视化。后文将分线程对整个框架进行大体解读，由于系统的算法核心集中在跟踪、局部建图线程和回环检测线程，本文只对这三个线程内容进行解读。

图1 ORBSLAM3框架

        为了让读者对系统有初步的了解，咱们按照头文件的顺序提前小小地剧透一下各头文件所要实现的功能。

CameraModels:包含GeometricCamera.h、KannalaBrandt8.h和Pinhole.h三个头文件，第一个是相机模型的基类， 后两者分别为鱼眼相机和针孔相机模型的实现。
Atlas.h:实现多地图管理功能，系统跟踪失败不至于轨迹重建失败，增强了系统的鲁棒性 
Config.h:这个文件里面的功能貌似没见实现，可能是作者想用其实现系统配置文件功能，但还来不及具体实现 
Converter.h:全部声明为静态成员函数，Converter类无需实例化即可被外部调用，主要实现OpenCV、Eigen和G2o格式矩阵之间的转换 
Frame.h:图像帧类，实现图像信息的存储与操作 
FrameDrawer.h:实现图像帧的可视化 
G2oTypes.h:ORBSLAM3后端优化利用g2o实现的，此处定义了图优化用到的边和节点信息 GeometricTools.h 通用相机模型工具，用来计算基础矩阵和三角化。
ImuTypes.h:IMU相机参数模型及预积分类 
Initializer.h：ORBSLAM2的初始化器，ORBSLAM3并没有使用 
KeyFrame.h:关键帧类，后端与闭环模块都是基于关键帧来进行优化的，内部会包含共视图连接关系 KeyFrameDatabase.h 生成关键帧数据库，方便利用图像帧描述子生成的词袋向量和特征向量建立图像特征的匹配关系。 
LocalMapping.h:局部建图线程。
LoopClosing.h:回环检测线程。
Map.h:地图类，包含在Atlas（多地图）当中，可看作多地图的子图，内部包括属于该子图的关键帧和地图点。
MapDrawer.h:地图可视化类。
MapPoint.h: 地图点类，地图点关联了多个图像中的特征点，系统的共视图、本质图和生成树都是以对地图点的观测共视关系来建立图结构的。
MLPnPsolver.h:最大似然PnP求解器，充分考虑了观测的不确定性，或者说观测噪声，并且可不依赖于相机模型。
OptimizableTypes.h:优化所使用的一些顶点和边。
Optimizer.h:各种优化模式具体的实现流程。
ORBextractor.h:特征点提取器，对图像以四叉树的方法提取特征点，并计算特征点描述子。ORBmatcher.h:ORB特征点匹配器。
ORBVocabulay.h:DBoW2的词袋模板类。
PnPsolver.h:EPnP求解器，ORBSLAM2重定位模块就是它的。
SerializationUtils.h:序列化相关的类。
Settings.h:ORBSLAMS新搞的一个类，用于读取配置文件。
Sim3Solver.h:sim3问题的求解器。
System.h:系统枢纽类，各个线程包含在这里面，也会系统算法的入口。
Tracking.h: 跟踪线程。
TwoViewReconstruction.h:对极几何和三角化类，主要应用于单目初始化。
Viewer.h:可视化线程。

三、跟踪线程

        跟踪线程实则是系统的主线程，每来一帧数据调用一次System的跟踪成员函数，具体为以下(形参进行了省略)，注意：为了与后端g2o流形优化位姿形式统一，ORBSLAM3使用Sophus定义的位姿结构来存储载体位姿。

Sophus::SE3f TrackMonocular(...){}

Sophus::SE3f TrackStereo(...){}

Sophus::SE3f TrackRGBD(...){}

之后构造图像帧类(Frame)，对图像数据提取ORB特征，其中RGB-D和双目相机还会获取特征点的深度信息，RGB-D相机由于有深度图的加持，特征点深度则很容易获取，对于双目相机，若是立体的针孔相机，则通过基于SAD的双目立体匹配恢复深度，若是双目鱼眼相机，则使用暴力的近邻描述子匹配建立左右目的特征匹配，再结合标定的外参对特征点进行三角化获取深度信息。最后，转到跟踪(Tracking)类下的Track()函数对前端位姿进行初步的估计。根据系统阶段的不同、跟踪状态以及质量，可将跟踪阶段化分为：初始化模式、参考关键帧模式、运动模型模式和重定位模式。此外，跟踪阶段会通过NeedNewKeyFrame函数决定系统是否需要关键帧，若需要的话，会通过CreateNewKeyFrame建立新的关键帧，并插入局部建图线程进行优化。

四、局部建图线程

        PTAM首次将系统化分为跟踪和建图线程，随后慢慢流行开来，如后来的SVO和ORBSLAM等都借鉴了这一思想。ORBSLAM3中跟踪线程用来保证位姿估计的实时性，建图线程对负责构建地图点并以共视图的方式对位姿和地图点中优化（当然IMU模式优化变量还包括速度与零偏），由于两个线程之间共享内存，优化后的地图点，又可以与跟踪线程的图像帧进行投影匹配，利用迭代PnP方法对跟踪线程的位姿进行优化，下面对局部建图流程进行大体介绍。

        开启局部建图模式，首先需要设置不能接受关键帧，告知跟踪线程不要插入关键帧;若关键帧队列有关键帧数据并自IMU数据正常，则通过ProcessNewKeyFrame()函数处理新的关键帧，处理关键帧操作包括计算BOW向量、添加地图点信息（增加关于当前关键帧的观测、平均观测方向、观测深度范围和地图点最具代表性描述子）、更新共视图连接关系、将关键帧插入当前活动地图;对新增的地图点进行筛选，删除质量不好的地图点;遍历相邻的关键帧，生成新的地图点;若关键帧队列的关键帧已处理完毕，则进行地图点重复性检查，融合重复的地图点;之后，则分阶段进行IMU初始化，并进行全局VI-BA;若处于IMU模式并且IMU已经初始化完毕，则进行局部VI-BA，否则做纯视觉局部BA;此外，考虑到关键帧冗余会增加系统的计算量，移除点冗余关键帧。

五、回环检测线程

        相较于ORBSLAM2，ORBSLAM3的回环模块功能添加了不少，主要是因为系统添加了多地图功能，若回环候选帧与当前帧不属于同一个地图，则需要进行地图合并，若属于同一个地图则进行回环矫正。由于传感器的噪声，系统不可避免存在累积误差，需要执行回环检测对系统的状态变量进行调整，直观地解释就是当前发生回环的帧与回环候选帧之间存在一定的误差，系统若是零漂移的，两者的误差应该是一样的，回环的意义就是通过建立优化目标函数，对系统的关键帧进行调整，使得存在的误差达到最小。

六、总结

        本文对ORBSLAM3的大体框架先简单总结到这里，由于涉及到大量算法原理，一篇文章很难进行细致分析，因此后续会针对系统各个部分进行分开解读，陆续会包括：相机模型的推导、系统所涉及的重要数据结构、流形预积分的原理及推导、跟踪线程的四种跟踪模式、系统初始化问题、局部建图线程和回环检测线程详细解读。