1.内容简介

本系列文章主要基于ORB-SLAM3代码、论文以及相关博客，对算法原理进行总结和梳理。

ORB-SLAM系列整体架构是不变的，都包含Tracking、LocalMapping和LoopClosing三个核心线程，中间伴随着优化过程。在ORB-SLAM3算法中比较突出的改进有两个：

引入IMU传感器，系统支持单目+IMU，双目+IMU和RGBD+IMU等三种模式
引入地图集Atlas这个概念，支持多个子地图并存，以及地图的保存和加载

2. 跟踪线程Tracking

跟踪线程是算法的核心模块，输入一帧数据(双目指左右目图像，IMU模式下还包括时间戳上想对应的一组IMU数据)，然后返回该帧对应的相机位姿，同时根据跟踪情况创建关键帧并传递给局部建图线程。跟踪线程主要包括以下几个模块：

纯视觉初始化：初始化第一帧图像位姿，将特征点三角化为3D地图点并初始化地图，主要包括单目初始化和双目初始化;IMU模式下还需要在局部建图线程进一步完成视觉与IMU的联合初始化
帧间跟踪：帧间跟踪包括跟踪运动模型和跟踪参考关键帧两部分，当环境比较友好系统跟踪比较顺利时仅通过跟踪运动模型即可获得较好的匹配关系，只有当跟踪运动模型得到的内点数量较少时才会跟踪参考关键帧
跟踪局部地图：局部地图的信息来自局部建图线程，主要将局部地图中的3D地图点投影到当前帧，进一步对当前帧位姿进行优化
重定位模块(当跟踪到的匹配点比较少时判定系统跟踪失败，此时调用重定位)
创建关键帧(围绕两个核心原则，当间隔时间比较长以及跟踪到的匹配点数量较少时创建关键帧)

在跟踪线程代码还设置了纯定位模式，但是经过实测即使在有先验地图的情况下，当运行时间比较长、经过光线比较差、剧烈运动时会导致相机位姿发生漂移，而且极为容易跟踪失败。

3. 局部建图线程

局部建图线程主要维护以下几个部分：当前关键帧的共视图Covisibility Graph、本质图Essential Graph和生成树Spanning Tree,局部地图主要由关键帧、关键帧特征点对应的3D地图点(世界坐标系)以及关键帧之间的连接关系组成。

当跟踪线程有新的关键帧传来时，会对局部地图中的关键帧进行更新，同时对当前局部地图中关键帧的位姿和地图点进行BA优化，最后对冗余的关键帧及其对应的地图点进行删除。

此外，在IMU模式下局部建图线程需要完成IMU的初始化(主要对偏置信息进行优化)和视觉惯导联合初始化。当IMU未完成初始化时局部建图线程只进行纯视觉BA，当IMU完成初始化后进行视觉-惯导联合BA优化

4. 回环检测线程

当局部建图线程处理完一帧关键帧后，会将该关键帧送入回环检测线程。回环检测线程主要包括检测共视区域、检测是否存在回环、计算相似变换、回环矫正

共视区域检测主要是看关键帧和之前已经创建的关键帧之间是否存在公共单词，如果存在再进一步分析检测到的共视关键帧是其他子弟图的还是当前子地图的
如果检测到的共视关键帧是其他子地图的，则将两个子地图进行融合;如果检测到的共视关键帧是当前子地图的，则有可能存在回环关系
如果存在回环关系则计算回环关键帧之间的相似变换，获取漂移的程度
如果相似变化计算成功，则根据漂移程度纠正回环处的累计误差

5. Atlas地图创建

5.1 Atlas地图简介

在ORB-SLAM3中使用了Atlas地图集的概念，在系统运行过程中支持保留多个子地图，其中已经跟踪失败的被暂时搁置的地图被称为non-active Map即不活跃地图，当前跟踪程序正在使用的被成为active map即活跃地图。

当因为跟踪失败重新初始化程序时，如果此刻已经跟踪了比较多(>15)的关键帧了，系统会将这部分地图保存下来，然后重新创建一个新的子地图。当回环检测模块检测到当前活跃子地图和之前的non-active Map存在共视区域和回环关系时，会将non-active Map融合到当前active map中。

在代码中Atlas地图集本质上就是一个ORB_SLAM3::Map*类型的集合，每新建一个子地图就是在这个集合中新增一个元素。

5.2 Atlas地图的保存与加载

Atlas地图的保存与加载是System类的私有成员函数，只有在配置文件中设置地图储存路径和加载路径时系统才会完成相应的功能，因为是私有成员函数，所以只能在程序结束时自动保存地图文件。可以将地图保存功能进行简单修改，通过ROS服务在终端实现地图的保存。

地图需要保存的信息：

关键帧(包含了关键帧的每个属性)
3D地图点(包含了3D地图点的每个属性)
子地图、关键帧、普通帧、地图点等一系列信息的ID，因为加载完先验地图后，新创建的地图、关键帧等对象的id要和之前的接上

地图保存和加载的方法：
通过boost库的序列化和反序列化功能可以之间将对象以字节的形式储存为二进制文件，在加载先验地图时也可以直接将整个对象读取进来取代当前系统中的Atlas对象

6.其他

6.1 ROS节点程序

作者自带的ROS节点程序较为简略，没有位姿发布的功能，推荐使用https://github.com/thien94/orb_slam3_ros版本

ROS节点程序实时接收图像和IMU数据，并进行时间戳对其，最终将时间对其后的一帧数据传递给SLAM系统，另外上述ORBSLAM的ROS版本还提供了相机位姿和轨迹发布功能，同时对位姿的坐标系转换成机器人坐标系(前-x轴,左-y轴,上-z轴)

6.2 ORBSLAM位姿

在VSLAM中位姿(Pose)指的是相机在世界坐标系下的位置坐标(x,y,z)，以及朝向(qw,qx,qy,qz),ORBSLAM中使用Tcw作为相机的位姿表示，下面进行对该表示的解释

假设坐标远点位于Pw = (0,0,0,1,0,0,0)处

t0时刻相机位于坐标系原点，t0时刻的位姿为：P_0 = T0w * Pw = Pw
t1时刻相机移动到了世界坐标系下的P_1 = (1,0,0,1,0,0,0)位置即t1时刻位姿, 其中P_1 = T10 * P_0 = T10 * T0w * Pw = T1w * Pw,因为P_w为坐标原点，t1时刻的位姿P_1在数值上为T1w中对应的平移和旋转;
t2时刻相机移动到了世界坐标系下的P_2 = (2,0,0,1,0,0,0)位置即t2时刻位姿, 其中P_2 = T21 * P_1 = T21 * T1w * Pw = T2w * Pw,因为P_w为坐标原点，t2时刻的位姿P_2在数值上为T2w中对应的平移和旋转;
可以知道，第k时刻的相机位姿P_k = Tk(k-1) * Tk-1(k-2) * ... * T21 * T10 * Pw = Tkw * Pw,k时刻相机的位姿在数值上等于Tkw中的平移和旋转