ORB-SLAM3跟踪线程与局部建图线程

一.总体流程 

二.跟踪线程

ORB-SLAM3与ORB-SLAM2的新增

第一阶段：

1.增加了一个状态---RECENTLY_LOST，可以在短时间内跟丢的情况下，用IMU支撑。

2.参考帧跟踪：把跟踪成功的判断标准变得宽松了。

3.恒速跟踪：IMU如果初始化成功，就用IMU估计位姿，否则用速度模型。同时也扩宽了跟踪成功的条件。

4.重定位跟踪：把EPnP换成了MLPnP(至少需要6对点)。

第二阶段：

1.如果IMU未初始化成功或者虽然初始化成功但距离上一次重定位时间比较近，仅优化位姿。否则，如果地图未变更，则通过上一个普通帧以及当前帧的视觉信息和IMU信息联合优化当前帧位姿，速度，和IMU零偏。如果地图变更，则用上一关键帧以及当前帧的视觉信息和IMU信息联合优化当前帧位姿，速度，和IMU零偏。

2.跟踪成功的条件变了。

三.局部建图线程的目的和意义 

1.承上启下，接受跟踪线程输入的关键帧并进行局部地图优化，删除冗余关键帧等，将优化后的关键帧发送给闭环线程。

2.实现中期数据关联

 三.IMU初始化

3.1为什么要IMU初始化？

惯性变量包括：重力方向和IMU零偏。先说零偏，IMU零偏不是固定的，是随时间变化的。由于零偏对IMU的影响较大，所以通常作为一个独立的状态来优化。再说重力方向，在视觉惯性模式下，系统以视觉初始化成功的第一帧作为世界坐标系原点，如果不进行IMU初始化，则无法消除重力对IMU积分的影响。IMU初始化的目的就是把图像建立的世界坐标系的z轴拉到和重力方向平行的状态。

3.2IMU初始化是如何进行的

单目情况：

1.在纯视觉单目SLAM模式下，通过运动恢复结构的方式完成了地图初始化，估计的位姿比较准确，但是缺少尺度，先解决纯视觉地图初始化，将会促进IMU的初始化。

2.纯视觉模式估计的位姿的不确定性远小于IMU的不确定性，因此可以在第一次求解IMU参数时忽略不计。所以，后续只进行纯惯性最大后验估计，将缺少尺度的视觉SLAM轨迹看作常量。

3.IMU初始化主要分为

• 纯视觉最大后验估计

在纯视觉单目SLAM模式下，用运动恢复结构的方式完成地图初始化后，用较高的频率插入关键帧。因为关键帧之间的时间间隔短，所以对关键帧之间的IMU数据计算预积分分量时的不确定性也比较低。这样就可以得到一个由10个关键帧位姿和几百个地图点组成的地图。注意，此时的地图尺度是未知的。

• 纯惯性最大后验估计 

 这一步的目的是获得惯性变量的最佳估计。因为上一步的尺度是未知的，此时没有惯性变量的可靠估计，如果贸然地进行视觉惯性联合优化，很容易陷入局部极小值，而且计算量也较大。一种有效的解决方案是固定轨迹，执行纯惯性优化。当完成纯惯性优化后，会用估计的尺度值将视觉的结果缩放到真实的尺度，包括帧的位姿、速度和地图点，并旋转地图坐标系以使z轴与估计的重力方向对齐。IMU零偏初始值为0，优化后更新为更合理的估计值，并且用最新的惯性参数更新IMU预积分结果，以减少后续的线性误差。

• 视觉惯性联合最大后验估计

经过前两个步骤，对惯性和视觉参数有了良好的估计，就可以执行视觉惯性联合优化，以进一步对之前的估计结果进行优化。此时认为IMU初始化成功，尺度、IMU参数、重力方向和地图都是准确的。

双目情况：

对于双目惯性初始化，只需要将尺度因子固定为1，并将其从纯惯性的优化变量中删除，目的就是加速其收敛，这样就完成了单目->双目。

3.3IMU初始化失败的可能

速度很慢可能失败。

改进：在纯惯性优化的基础上进行修改，虽然包含所有插入的关键帧，但只估计尺度和重力方向两个参数，其他不参与优化，提高计算效率。

四.IMU初始化代码实现

在局部见图中完成初始化，要求地图中存在10帧以上的关键帧才可以。目的就是积累足够的数据来进行初始化。分为以下几步：

1. 第一阶段初始化，目的是快速初始化IMU，尽快用IMU来跟踪。完成第一阶段初始化之后就可以用IMU预积分结果来预测跟踪线程中当前帧的位姿了，同时在进入跟踪第二阶段---局部地图跟踪时，会使用视觉+IMU联合优化位姿。
2. 目的是快速修正IMU，在短时间内使得IMU参数相对可靠。
3. 再次优化IMU，保证IMU参数的高精度。
4. 在单目模式下增加了单独优化重力方向和尺度。 

五.IMU初始化代码

代码：InitializeIMU

1. 对于不满足初始化的条件直接退出，条件包括有位置请求，地图中关键帧数目小于10和留存时间太短。
2. 在IMU初始化之前，通知跟踪线程不再创建新的关键帧，将局部建图线程缓存队列中未处理的新关键帧也加入进来。
3. 开始初始化，计算重力方向，然后纯惯性优化尺度、重力方向及零偏。
4. 用上一步得到的惯性参数恢复重力方向与尺度信息，同时更新跟踪线程中的普通帧的位姿，标记为IMU初始化成功。
5. 执行视觉惯性全局BA，更新地图中的位姿和地图点坐标。