ORB3-SLAM 知识梳理

只对ORB3-SLAM与ORB2-SLAM的不同之处进行梳理。

一、IMU(惯性导航)传感器

(1)IMU(惯性导航)概述：

- 目的：利用IMU测量的角速度、加速度，根据上一时刻导航信息，推算出当前时刻导航信息，包括姿态解算、速度解算、位置解算
- 方法：由姿态、速度、位置的微分方程，推导出它们的解，并转变成离散时间下的近似形式，从而可以在离散时间采样下，完成导航信息求解。
- 优点：IMU不受外界信号干扰，可以得到稳定、平滑的导航结果
- 缺点：误差随时间累计，一般时间越长，累计误差越大。因此需要融合，而导航解算及其误差分析，是融合的基础

(2)IMU(惯性导航)噪声值：

角速度的噪声来源：零偏，高斯白噪声
真实值：

线速度的噪声来源：零偏，高斯白噪声
真实值：

加速度计这边是重力对齐了，如果没进行重力对齐还需要减去一个g(重力影响)

(3)IMU(惯性导航)积分：

连续时间下，从i时刻到j时刻的积分公式：

(4)IMU(惯性导航)预积分：

什么是预积分：
  - 预积分里面的每一项与起始状态无关，可以认为是相对量，这个好处在于计算预积分时不需要考虑起始状态，值得注意的是关于速度和位置的预积分都包含了重力
连续时间下的积分公式(预积分)：

整理公式，把预积分相关的项用下面的式子代替：

实际使用中使用离散形式，而非连续形式，由于在解算中，一般采用中值积分方法

那么预积分的离散形式可以表示为

经过以上的推导，此时状态更新的公式可以整理为：

需要注意的是，陀螺仪和加速度计的模型为

即认为bias是在变化的，这样便于估计不同时刻的bias值，而不是整个系统运行时间内都当做常值
对待。这更符合低精度mems的实际情况。但在预积分时，由于两个关键帧之间的时间较短，因此
认为𝑖𝑖和𝑗𝑗时刻的bias相等

(5)IMU(惯性导航)残差计算公式：

在优化时，需要知道残差关于状态量的雅可比。由于已知姿态位姿更新的方法如下

因此，可以很容易写出一种残差形式如下：

但是和预积分相关的量，仍然与上一时刻的姿态有关，无法直接加减，因此，把残差修正为以下形
式

待优化的变量是：

如果我们的传感器只有IMU时，残差计算出来的都是0，但是信息矩阵会越来越小，因为误差一直在累计，所以信息矩阵越来越小，置信度越来越低。
当我们融合了其他传感器，我们会根据信息矩阵来判断设置置信度。信息矩阵的值越大，那表达的信息越多，在优化的过程中就越会被重视，置信度就越高。
例如融合了相机，假设相机的置信度为0.8，IMU为0.2，那么在计算融合的优化问题的残差时，相机的残差占比会高于IMU的残差占比，而我们要最小化这两个残差值之和。那么就会更关注于相机的残差，所以优化出来的位姿会偏向于相机传感器。

(6)IMU(惯性导航)雅克比计算公式：

使用扰动量，对以下变量求雅可比

(7)IMU(惯性导航)误差方程连续时间下的推导：

误差方程：状态量(速度误差、位置误差、姿态误差、bias误差等)误差形式的表示。
误差方程及其推导方法，是后续滤波、图优化等融合方案的基础，是重中之重。
速度的误差方程推导(连续时间下)：

位置的误差方程推导(连续时间下)：

Bias的误差方程推导(连续时间下)：

惯性导航误差总结：

(8)IMU(惯性导航)方差计算公式(信息矩阵)：

在融合时，需要给不同信息设置权重，而权重由方差得来，因此对于IMU积分，也要计算其
方差，离散情况下的方差计算公式：

但需注意的是，此处公式离散时间下的状态传递方程中的矩阵，而我们一般是在连续 时间下推导微分方程，再用它计算离散时间下的传递方程
连续时间下的方差计算公式

我们已知IMU的在连续时间下的误差公式：

得到连续时间微分方程以后，就可以计算离散时间的递推方程了，表示为

离散时间下的传递方程：

(9)IMU(惯性导航)预积分更新：

二、IMU初始化(InitializeIMU)

ORB3-SLAM的IMU初始化和优化都在局部建图线程中

(1)为什么要进行IMU的初始化

      我们在做预积分时，是将重力因素也带入进行计算的，所以这里我们需要去除重力因素的影响，要将重力方向与我们世界坐标系的z轴进行对齐，消除重力的影响。
     世界坐标系的z轴与重力坐标系下的重力想要对齐，我们需要求出世界坐标系到重力坐标系的外参，通过外参，进行重力方向的对齐从而消除重力因素带来的印象
     我们最后都是在将世界坐标系转化到重力坐标系，在重力坐标系做后续操作的

(2)计算出世界坐标系到重力坐标系的外参（初始值）

     如果我们的IMU还没有初始化过，则要进行初始化，获得一个世界坐标系到重力坐标系的外参与尺度信息
- 获取所有关键帧通过imu得到的旋转
- 通过公式初始一个旋转矩阵(初始化的第一步)
  - 推导的公式(8)与我们的预积分速度公式类似，只不过将s度信息添加上去了

  - 初始化时，我们的bias都设置为0，所以我们只关心公式(8)前面的
  - 我们再乘上一个旋转R，那么整个公式就剩下括号里的部分

  - 拿出关键帧内的Imu数据进行累减，发现最后的如下

  - 将前面的V1和Vn忽略不计，最终的到我们想要的世界坐标系到重力坐标系的旋转矩阵

(3)优化尺度、重力方向及IMU参数（速度、位移、偏置）(InertialOptimization)

(4)视觉和IMU融合的BA优化(FullInertialBA)

• 承接上一步纯imu优化，按照之前的结果更新了尺度信息及适应重力方向，所以要结合地图进行一次视觉加imu的全局优化，这次带了地图点等信息
• 我们的IMU初始化是在局部建图线程中，再IMU没有初始化之前，我们都是用纯视觉进行追踪等。我们在用纯视觉时，肯定已经做过相关的纯视觉的相关优化了，这里是通过纯视觉与我们的IMU进行融合优化，获得更加准确的位姿。

(5)通过BA优化，得到更准确的位姿和地图点，进行更新位姿与地图点

• 视觉与IMU的全局优化过程中，将后面新进来的关键帧保存起来

• 在我们进行视觉与IMU优化的过程中，跟踪线程还是会给建图线程发送关键帧，将这些优化过程中新加入的关键帧进行保存，后续通过已经优化好的关键帧来更新他们的位姿信息

• 更新位姿与三维点

• 视觉和IMU优化是在局部建图线程中，所以局部建图线程的帧全是关键帧，我们通过优化出来的关键帧信息对其他帧进行更新，如刚刚的在优化过程中新增进来的关键帧和普通帧

三、多地图系统

(1)多地图系统的简介

• 当我们在跟踪丢失的情况下，不至于在唯一的一张地图中疯狂重定位。因为重定位的结果只有两个，一个是马上重定位成功，那么这是最理想的情况，我们可以继续进行环境探索；另一种就比较吃亏了，我们一直无法重定位成功，最终的结果就是系统判断当前跟踪状态为跟踪丢失，系统会直接将之前所重建的环境信息全部清除！重新生成一张地图。

(2)多地图系统的好处

• 当我们只维护一个地图，在跟踪丢失的情况下会进行重定位。此时，重定位有两种情况：一种是重定位失败，地图重置；另一种是重定位成功，但是重定位成功也可能会出现两种情况：第一种是估算的相对变换准确，使得我们的机器人跟踪回到正确的道路上，这当然是最理想的情况！但是还有另一种情况会非常致命，那就是估算的相对变换出现偏差，导致最终的地图出现严重的不一致性
• 当我们维护多个地图时,可以尽可能保证不丢帧，毕竟ORB-SLAM2需要进行多次重定位失败才会最终进行地图重置，而ORB-SLAM3只要一定位失败则直接重新创建子地图，可以有效地保存尽可能多的图像，这对于帧率较低的应用场景非常友好！

• 上面的是ORB-SLAM2中只维护一个地图会出现的情况，下面则是采用子地图的策略得到的结果，孰优孰劣一目了然、

(3)什么时候新建地图

  1. 处理时间戳异常的情况
    -  如果当前图像时间戳比前一帧图像时间戳小，说明出错了，清除imu数据，创建新的子地图
    - 如果当前图像时间戳和前一帧图像时间戳大于1s，说明时间戳明显跳变了。 
      - 如果当前子图中imu没有经过BA2，重置active地图，也就是之前的数据不要了。
      - 如果当前子图中imu进行了BA2，重新创建新的子图，保存当前地图
  2. SLAM系统刚启动，会创建一个地图
  3. 在跟踪参考关键帧、恒速模型跟踪都失败的话，且经过RECENTLY_LOST处理后还是失败，则判断当前地图的关键帧数量，大于10则保留这个地图，新建一个地图。如果小于10个关键帧，则直接重置当前地图
  4. 局部地图跟踪失败
    - 如果地图中关键帧小于10，重置当前地图，退出当前跟踪
    - 如果是IMU模式并且还未进行IMU初始化，重置当前地图，退出当前跟踪
    - 如果地图中关键帧超过10 并且 纯视觉模式 或 虽然是IMU模式但是已经完成IMU初始化了，保存当前地图，创建新的地图

(4)地图更新标志

这个标志位表示，我们的关键帧已经进行了BA优化过。
  地图更新标志的作用
  - 如果地图已经更新过，那么我们的关键帧就更加准确，后续再恒速模型跟踪时，采用上一帧关键帧到这一帧的预积分进行IMU估计位姿，反之则使用上一帧普通帧。
  - 在局部地图跟踪中，进行IMU与相机的优化时，如果更新了地图，使用上一关键帧以及当前帧的视觉信息和IMU信息联合优化当前帧位姿、速度和IMU零偏，反之z使用上一普通帧以及当前帧的视觉信息和IMU信息联合优化当前帧位姿、速度和IMU零偏e

(5)什么时候进行地图更新

  1. 闭环检测线程中的矫正闭环、融合地图、全局BA等
  2. 局部建图线程中IMU三阶段的初始化
  3. 各种优化过程
  总体来说，就是对关键帧进行了IMU与视觉的融合优化后，地图内的关键帧的位姿更加准确了，就相当于地图更新了

后续对于多地图的功能，在具体使用的地方讲解

四、跟踪线程

ORB3-SLAM跟踪线程流程图：

(1)参考关键帧跟踪

与ORB-SLAM2的不同之处：
  - 运动模型是空的并且imu未初始化或刚完成重定位，跟踪参考关键帧；否则恒速模型跟踪。
  - ORB2中，需要最后匹配的内点数量大于10才算成功，ORB3中，如果IMU模式直接认为成功，如果是纯视觉模式，需要成功匹配内点数量超过10才可算成功。

(2)恒速模型跟踪

  - 如果是IMU模式下，且IMU初始化完成，则使用IMU的来估计位姿，不使用ORB2恒速模型跟踪那一套流程
  - 如果IMU还没初始化完成，则纯视觉恒速模型跟踪。如果是IMU模式，不需要匹配内点的数量来判断是否跟踪成功，因为等待IMU初始化成功后，后续都是用IMU来估计位姿

(3)重定位

  - 因为ORB3采用多地图系统，因为我们重定位只是为了起一个短时追回的作用，所以只适合在当前地图上找。
  - ORB3重定位和ORB2重定位流程一样，只不过将EPnP改成MLPnP

(4)局部地图跟踪

  - 如果IMU没有初始化成功，那么就跟ORB2的局部地图跟踪一样(PoseOptimization)
  - 如果IMU初始化成功了，但是前不久做过初始化、重定位、重建地图，那么还是做纯视觉的局部地图跟踪。累积IMU数据不够多。(PoseOptimization)
  - 如果没有更新地图，那么使用上一普通帧以及当前帧的视觉信息和IMU信息联合优化当前帧位姿、速度和IMU零偏(PoseInertialOptimizationLastFrame)
  - 如果更新了地图，那么使用上一关键帧以及当前帧的视觉信息和IMU信息联合优化当前帧位姿、速度和IMU零偏(PoseInertialOptimizationLastKeyFrame)
  - 根据跟踪匹配数目及重定位情况决定是否跟踪成功
    - RECENTLY_LOST状态下，至少成功跟踪10个才算成功
    - 单目IMU模式下做完初始化至少成功跟踪15个才算成功，没做初始化需要50个
    -  其他情况只要跟踪的地图点大于30个就认为成功了

(5)新增RECENTLY_LOST跟踪状态

因为我们有了IMU，新增了一个状态RECENTLY_LOST，主要是结合IMU看看能不能拽回来
  RECENTLY_LOST状态标记条件：
  - 如果经过跟踪参考关键帧、恒速模型跟踪都失败的话，并满足一定条件就要标记为RECENTLY_LOST或LOST
    - 标记成LOST：
    - 条件1：如果当前帧距离上次重定位成功不到1s，
    - 条件2：单目+IMU 或者 双目+IMU模式
    - 同时满足条件1，2，标记为LOST
    - 标记成RECENTLY_LOST：
    - 条件1：当前地图中关键帧数目较多（大于10） 
    - 条件2（隐藏条件）：当前帧距离上次重定位帧超过1s（说明还比较争气，值的救）或者非IMU模式
    - 同时满足条件1，2，则将状态标记为RECENTLY_LOST，后面会结合IMU预测的位姿看看能不能拽回来
RECENTLY_LOST状态处理：
  - 如果当前地图中IMU已经成功初始化，就用IMU数据预测位姿(PredictStateIMU)
    - 跟踪不成功的时候，用初始化好的imu数据做跟踪处理，通过IMU预测状态
    - 如果地图没更新，那么就用上一帧的位姿加上上一帧到这一帧的预积分进行预测
    - 如果地图更新，那么就用上一关键帧的位姿加上上一关键帧到这一帧的预积分进行预测(我们在做预积分时，会同时预积分上一帧普通帧到当前帧的和上一帧关键帧到当前帧的)
  - 如果IMU模式下当前帧距离跟丢帧超过5s还没有找回（time_recently_lost默认为5s），放弃了，将RECENTLY_LOST状态改为LOST(我们通过IMU恢复了位姿，但是通过恢复的位姿，跟踪参考关键帧、恒速模型跟踪还都失败了)
  - 如果IMU没有初始化，则直接进行重定位
  - 纯视觉模式则进行重定位。主要是BOW搜索，EPnP求解位姿，跟ORB2的基本一致
RECENTLY_LOST状态处理后还是失败，则新建地图：
  经过重定位后，还是失败。那么根据当前地图的关键帧数量，判断是否保存这个地图
  - 当前地图中关键帧数目小于10，重置当前地图
  - 当前地图中关键帧数目超过10，创建新地图

五、ORB3-SLAM中的预积分

ORB3-SLAM的预积分，只在没有新建地图的情况下才进行预积分，因为新建地图时，会把之前的图像数据和IMU数据都存储在上一个地图中，新的地图重新开始，所以不能与上一个地图的进行预积分，预积分也要重头开始。

(1)IMU与相机时间戳对齐(PreintegrateIMU)

- 拿到第一个imu数据作为起始数据
- imu起始数据会比当前帧的前一帧时间(图像)戳早,如果相差0.001则舍弃这个imu数据
- 同样最后一个的imu数据时间戳也不能理当前帧(图像)时间间隔多余0.001
- 得到两帧间的imu数据放入mvImuFromLastFrame中,得到后面预积分的处理数据

(2)IMU预积分(PreintegrateIMU)

IMU的预积分在跟踪线程中
预积分，对于一个帧有两种预积分，一种是相对于上一帧，一种是相对于上一个关键帧

• 上一帧不存在,说明两帧之间没有imu数据，不进行预积分，没有imu数据,不进行预积分
• 得到两帧间的imu数据放入mvImuFromLastFrame中,得到后面预积分的处理数据
• imu的第一帧数据和倒数第二帧数据都需要进行特殊处理，原因：我们的相机抓取的图像时间戳很难刚刚好落在Imu的时间戳上，所以需要做特殊处理。
• 我们的IMU数据第一帧应该在图像上一帧之前，IMU第二帧数据在图像上一帧之后。我们当前图像帧的前后也是一样
• 对于第一帧的特殊处理：取第一帧的IMU，获取相邻两段imu的时间间隔，获取imu到上一帧的时间间隔。设当前时刻imu的加速度a0，下一时刻加速度a1，时间间隔tab 为t10，tini t0p，正常情况下时为了求上一帧到当前时刻imu的一个平均加速度，但是imu时间不会正好落在上一帧的时刻，需要做补偿，要求得a0时刻到上一帧这段时间加速度的改变量，有了这个改变量将其加到a0上之后就可以表示上一帧时的加速度了。其中a0 - (a1-a0)(tini/tab) 为上一帧时刻的加速度再加上a1 之后除以2就为这段时间的加速度平均值，其中tstep表示a1到上一帧的时间间隔，a0 - (a1-a0)(tini/tab)这个式子中tini可以是正也可以是负表示时间上的先后，(a1-a0)也是一样，多种情况下这个式子依然成立
• 其余中间按正常的中值积分即可
• 直到倒数第二个imu时刻时，计算过程跟第一时刻类似，都需要考虑帧与imu时刻的关系
• 依次进行预积分计算
  • 应该是必存在的吧，一个是相对上一关键帧，一个是相对上一帧
• 记录当前预积分的图像帧

六、局部建图线程

局部建图线程对比ORB2-SLAM，多了IMU的初始化(三个阶段)。还有在IMU优化完毕后，局部BA的优化方式发生了改变

(1)如果IMU没有初始化完成，IMU进行初始化(三次初始化)

• IMU在第一次初始化完毕后，会将当前地图的IMU标志位初始化完毕，后续的第二次第三次优化，更多是优化尺度、外惨等信息

IMU的第一次初始化：

  在IMU连一次初始化都没有做的情况下进行。
    - 计算出世界坐标系到重力坐标系的外参（初始值），这个外参只在IMU第一次初始化进行
    - 优化尺度、重力方向及IMU参数（速度、位移、偏置）(InertialOptimization)

IMU的第二次初始化：

  如果距离IMU第一阶段初始化成功累计时间差小于50s,则进行第二次初始化
  根据条件判断是否进行VIBA1（IMU第二次初始化）
    1. 当前关键帧所在的地图还未完成IMU初始化
    2. 正常跟踪状态
    3. 当前关键帧所在的地图还未完成VIBA1
    4. 如果累计时间差大于5s，开始VIBA1（IMU第二阶段初始化）
    5. 也是调用InitializeIMU，只不过传入的陀螺仪偏置、加速度计偏置的信息矩阵系数不一致

IMU的第三次初始化：

  根据条件判断是否进行VIBA2（IMU第三次初始化）
    - 当前关键帧所在的地图还未完成VIBA 2
    - 第一阶段初始化成功累计时间大于15s

(2)在关键帧小于100时，会在满足一定时间间隔后多次进行尺度、重力方向优化(ScaleRefinement)

使用了所有关键帧，但只优化尺度和重力方向以及速度和偏执（其实就是一切跟惯性相关的量）

(3)处于IMU模式并且当前关键帧所在的地图已经完成IMU初始化，进行BA

IMU在第一次初始化完毕后，那么当前地图IMU就算是初始化完成，就能进行视觉与IMU的融合优化。
对IMU优化进行检查：
  1. 计算上一关键帧到当前关键帧相机光心的距离 + 上上关键帧到上一关键帧相机光心的距离，如果距离大于5厘米，记录当前KF和上一KF时间戳的差，累加到mTinit
    - 对移动距离有要求，只有大于移动的距离才会累加时间，这是因为IMU需要进行移动才会采集到IMU数据，所以对移动距离有一定要求，如果移动距离太短或者不进行移动，那么IMU采集到的数据就比较少，后续根据这个mTinit判断这次IMU优化是否有效。
  2. 当前关键帧所在的地图尚未完成IMU BA2（IMU第三阶段初始化）
    - 如果累计时间差小于10s 并且 距离小于2厘米，认为运动幅度太小，不足以初始化IMU，将mbBadImu设置为true
    - mbBadImu为true的话在跟踪线程里会重置当前活跃地图
    - 这里是认为如果IMU初始化过程中，移动的距离太小，而且累计时间小于10s，认为这次IMU的初始化不够准确，重置当前的地图，重新进行IMU初始化。
  3. 判断成功跟踪匹配的点数是否足够多
    - 如果是单目，那么跟踪成功的内点数目大于75
    - 如果不是单目，跟踪成功的内点数目大于100
当IMU优化检测完毕后，在进行局部地图+IMU一起优化，优化关键帧位姿、地图点、IMU参数
如果IMU为进行初始化，则进行局部地图优化，不包括IMU信息。优化关键帧位姿、地图点(跟ORB2-SLAM的类似)

七、闭环检测线程

ORB3的闭环检测线程中，因为多地图，所以增加了地图的融合合并而且闭环检测的方式与ORB2不同
ORB2检测闭环的过程中，需要在相同的位置连续成功匹配三个关键帧，才算成功，这样的方式相应速度缓慢
ORB3的回环、融合检测流程：

  1. 取出一帧关键帧，在所有关键帧中找回环候选帧和融合候选帧(两者的区别在于回环候选帧在当前地图上，融合候选帧在其他地图上)，
    - 寻找候选关键帧流程(与ORB2的流程基本一致)：取出所有与当前帧有公共单词的关键帧，获得最高的公共单词数量，公共单词筛选的阈值为最高的公共单词数量0.8。初步筛选关键帧，剩余的候选关键帧进行分组计算得分，即取出候选关键帧的共视关键帧，如果共视关键帧在当前候选关键帧上则进行得分。保留得分大于阈值的候选关键帧
    - 对候选关键帧进行分类：回环候选帧在当前地图上，融合候选帧在其他地图上。
  2. 取出候选关键帧，将候选关键帧的10个共视帧组成候选帧窗口，将候选窗口的地图点投影到当前帧上，根据Sim3迭代器，得到初始的Sim3位姿，通过初始的Sim3变化，重新将候选窗口的地图点投影到当前帧数，寻找新的匹配点，在进行优化，得到更加准确的位姿。
  3. 得到了准确的Sim3位姿(当前帧到候选关键帧的相对位姿)，取当前帧的五个共视关键帧，将候选窗口的地图点投影到当前帧的五个共视关键帧中，进行匹配，如果这五个共视关键帧中，匹配上的地图点大于一定阈值的数量大于3个，则认为回环，否则记录成功的共视几何关键帧，返回让下一个关键帧进入检测，进行时序几何校验，如果累计大于3个则认为回环了(成功的关键帧+时序几何校验成功的关键帧)
  4. 如果上一帧关键帧的共视几何有值，那么当前帧就要进行时序几何校验，将上一帧的候选窗口中的地图点投影到当前帧上，如果匹配的地图点大于一定阈值，则认为成功，那么时序集合校验的数量加一，当时序集合校验+共视几何总数大于3，则认为检测回环、融合成功

共视几何：上述五个共视关键帧匹配上的地图点大于一定阈值的帧数

(1)找出和当前关键帧有闭环关系或融合关系的关键帧

1. 从队列中取出一个关键帧,作为当前检测共同区域的关键帧
2. 在某些情况下不进行共同区域检测
   - imu模式下还没经过第二阶段初始化则不考虑
   - 双目模式下且当前地图关键帧数少于5则不考虑
   - 当前地图关键帧少于12则不进行检测
3. 基于前一帧的历史信息判断是否进行时序几何校验，有回环的时序几何校验和融合的时序几何校验
4. 回环的时序几何校验。(当上一帧关键帧共视几何数量不足三个，则当前帧进行时序几何校验)
   - 通过上一关键帧的信息,计算新的当前帧的sim3
   - 通过把候选帧局部窗口内的地图点向新进来的关键帧投影来验证回环检测结果,并优化Sim3位姿(DetectAndReffineSim3FromLastKF)
   • 把候选帧局部窗口内的地图点投向新进来的当前关键帧,看是否有足够的匹配点
   • 匹配点数如果不符合返回失败
   • 匹配点如果够进行一次优化
   • 再次通过优化后的Sim3搜索匹配点，匹配点大于期望数目,则认为成功

- 选帧局部窗口内的地图点向新进来的关键帧投影的匹配点大于阈值，则认为时序几何校验成功，成功的帧数增加一，如果总数大于三，则认为检测到回环(总数等于上一帧关键帧校验成功的帧数加上本次时序几何校验)
- 选帧局部窗口内的地图点向新进来的关键帧投影的匹配点小于阈值，则认为时序几何校验失败，失败的帧数增加一，如果失败总数大于二，则认为回环失败，重置信息(重置之前几何校验的帧)
- 若校验成功则把当前帧添加进数据库,且返回true表示找到共同区域

5. 融合的时序几何校验。(当上一帧关键帧共视几何数量不足三个，则当前帧进行时序几何校验) (与回环的时序几何校验一样)
6. 若当前关键帧没有被检测到回环或融合,则分别通过bow拿到当前帧最好的三个回环候选帧和融合候选帧(DetectNBestCandidates)
   - 统计与当前关键帧有相同单词的关键帧
   - 统计所有候选帧中与当前关键帧的公共单词数最多的单词数maxCommonWords,并筛选
   - 用小组得分排序得到top3总分里最高分的关键帧,作为候选帧
   - 如果候选帧与当前关键帧在同一个地图里,且候选者数量还不足够,添加到回环候选帧里
   - 如果候选者与当前关键帧不再同一个地图里, 且候选者数量还不足够, 且候选者所在地图不是bad,添加到融合候选帧里
7. 若当前关键帧没有被检测到回环,并且候选帧数量不为0，进行回环检测(DetectCommonRegionsFromBoW)
   - 对每个候选关键帧都进行详细的分析
   • 拿到候选关键帧的10个最优共视帧,构建候选关键帧的窗口
   • 标记是否因为窗口内有当前关键帧的共视关键帧，如果窗口中的帧是当前帧的共视帧则结束
   • 通过Bow寻找候选帧窗口内的关键帧地图点与当前关键帧的匹配点
   • 把窗口内的匹配点转换为Sim3Solver接口定义的格式

- 利用RANSAC寻找候选关键帧窗口与当前关键帧的相对位姿T_cm的初始值(可能是Sim3)
  - 通过候选窗口与当前帧进行Sim3计算，初步得到Sim3位姿
  - 利用初始的Scm信息,进行双向重投影,并非线性优化得到更精确的Scm
    - 重新利用之前计算的mScw信息, 通过投影寻找更多的匹配点
    - 利用搜索到的更多的匹配点用Sim3优化投影误差得到的更好的
    - 如果内点足够多,用更小的半径搜索匹配点,并且再次进行优化
  - 用当前关键帧的相邻关键来验证前面得到的位姿(共视几何校验)
    - 拿到用来验证的关键帧组(后称为验证组): 当前关键帧的共视关键帧
    - 遍历验证组当有三个关键帧验证成功或遍历所有的关键帧后结束循环
      - 拿出验证组中的一个关键帧
      - 几何校验函数, 这个函数里面其实是个searchByProjection : 通过之前计算的位姿转换地图点并通过投影搜索匹配点, 若大于一定数目的任务成功验证一次(DetectCommonRegionsFromLastKF)
      
  - 如果成功找到了共同区域帧把记录的最优值存到输出变量中
    - 主要记录成功共视几何的数量

8. 若当前关键帧没有被检测到融合,并且候选帧数量不为0(DetectCommonRegionsFromBoW)(与检测回环的流程一样，只不过这里的候选关键帧不在当前地图上)

(2)如果检测到融合（当前关键帧与其他地图有关联）, 则合并地图

纯视觉下的地图融合(Mergelocal)：

  地图的融合是在不同的地图上进行的，因为地图的融合可能性比较大，为了提高实时性。在进行完局部窗口BA后，就开启了局部建图线程，之后再进行剩下的地图中对地图和位姿的矫正传播，然后进行本质图优化。
  地图融合：是将当前地图融合到旧的地图上，但是地图点保留的是当前地图的地图点
  地图融合简要流程：主要是优化当前帧关键帧的局部窗口(共视关键帧加上本身组成)和融合关键帧的局部窗口，我们已知当前帧与融合关键帧的准确sim3位姿，关键帧与自身的窗口的位姿也是已知，所以能得到当前关键帧窗口在旧地图的初步位姿，通过初步位姿也能计算出地图点，如果与旧地图的地图点产生冲突，那么保留当前地图的地图点，更改新旧地图的生成树(父子节点)，更新关键帧的连接关系。
  接下来就行局部的BA,只优化刚转化到旧地图的关键帧窗口的位姿，固定融合关键帧的窗口。
  得到准确地关键帧窗口在旧地图的位姿信息，进行位姿传播，将当前地图的所有关键帧，转化到旧地图来，进行本质图BA(固定所有融合帧共视窗口内的关键帧 + 所有关键帧窗口在旧地图的位姿，因为之前已经单独进行优化过),(优化当前关键帧所在地图里的所有关键帧(除了当前关键帧共视窗口内的关键帧) + 当前地图里的所有地图点)。最后对旧地图进行全局BA。
  1. 构建当前关键帧和融合关键帧的局部窗口(关键帧+地图点)
    - 构造当前关键帧局部共视帧窗口
    - 拿到当前关键帧的15个最佳共视关键帧,如果窗口里的关键帧数量不够就再拉一些窗口里的关键帧的共视关键帧(二级共视关键帧)进来
    - 把当前关键帧窗口里关键帧观测到的地图点加进来
    - 构造融合帧的共视帧窗口
      - 拿到融合关键帧最好的25个最佳共视关键帧
      -  如果融合关键帧窗口里的关键帧不够就再拉一些窗口里的关键帧的共视帧进来(二级共视关键帧)
    -  把融合关键帧窗口里关键帧观测到的地图点加进来
  2. 根据之前的Sim3初始值, 记录当前帧窗口内关键帧,地图点的矫正前的值,和矫正后的初始值
    - 先计算当前关键帧矫正前的值,和矫正后的初始值
    - 通过当前关键帧融合矫正前后的位姿,把当前关键帧的共视窗口里面剩余的关键帧矫正前后的位姿都给填写上
      - 根据链式法则,利用当前关键帧和第i个共视关键帧的位姿关系,以及当前关键帧矫正后的初始位姿,推得第i个共视关键帧的矫正后的初始位姿
      - 根据上面计算得到的融合矫正后的初始位姿(Sim3),更新窗口内每个关键帧的矫正后的初始位姿
  3. 记录所有地图点矫正前的位置,和矫正后的初始值
  4. 两个地图以新（当前帧所在地图）换旧（融合帧所在地图），包括关键帧及地图点关联地图的以新换旧、地图集的以新换旧
    - 更新当前关键帧共视窗口内的关键帧信息
    - 更新当前关键帧共视帧窗口所能观测到的地图点的信息
    - 更新剩余信息,如Altas和融合帧所在地图的信息
  5. 融合新旧地图的生成树
    - 因为整个待融合地图要融入到当前地图里，为了避免有两个父节点，mpMergeMatchedKF的原始父节点变成了它的子节点，而当前关键帧成了mpMergeMatchedKF的父节点
    - 同理，为了避免mpMergeMatchedKF原始父节点（现在已成它的子节点）有两个父节点，需要向上一直改到待融合地图最顶端的父节点
  6. 因为融合导致地图点变化。需要更新关键帧中图的连接关系
  7. 在缝合(Welding)区域进行local BA 
    - LocalBundleAdjustment(优化所有的当前关键帧共视窗口里的关键帧和地图点, 固定所有融合帧共视窗口里的帧)
  8. 在当前帧整个剩下的地图中（局部窗口外，认为没那么紧急处理）对位姿和地图点进行矫正传播
    - 拿到当前帧所在地图里的所有关键帧, 重复之前的过程, 不过这里是对于地图里剩余的所有的关键帧
  9. 本质图优化(OptimizeEssentialGraph)
    - 固定 : 所有融合帧共视窗口内的关键帧 + 所有当前关键帧共视窗口内的关键帧
    - 优化:  当前关键帧所在地图里的所有关键帧(除了当前关键帧共视窗口内的关键帧) + 当前地图里的所有地图点
    
  10. 全局BA(RunGlobalBundleAdjustment)

IMU(惯性)下的地图融合(MergeLocal2)：

有IMU的情况下，没有类似纯视觉融合哪里的本质图优化和全局优化，只对当前帧关键帧的窗口和融合帧的窗口进行优化，其他的部分都是通过位姿变换过来的，因为IMU的位姿相对准确，所以不需要纯视觉融合那么多的优化，才能得到准确的位姿
IMU下的地图融合，是将旧地图融合到当前地图上
  1. 如果当前地图IMU没有完全初始化，帮助IMU快速优化；反正都要融合了，这里就拔苗助长完成IMU优化，回头直接全部放到融合地图里就好了
  2. 地图以旧换新。把融合帧所在地图里的关键帧和地图点从原地图里删掉，变更为当前关键帧所在地图。
  3. 融合新旧地图的生成树
  4. 把融合关键帧的共视窗口里的地图点投到当前关键帧的共视窗口里，把重复的点融合掉（以旧换新）
  5. 针对缝合区域的窗口内进行进行welding BA

(3)如果(没有检测到融合)检测到回环, 则回环矫正(CorrectLoop)

和ORB2的回环矫正一致

八、优化函数

IMU模式下的单帧优化：
跟踪关键帧：

跟踪普通帧：