ORB-SLAM策略思考之优化器策略

ORB-SLAM策略思考之优化器策略

1 跟踪线程中的优化策略

1. 地图初始化阶段：BA优化（初始化帧位姿固定，优化地图点位姿和第二帧位姿）
   1. 当ORB-SLAM判断地图初始化的地图点足以进入地图点位置和第二帧位姿优化阶段时，以初始化帧的相机坐标系为世界坐标系，基于相机的重投影误差进行优化，值得注意的是，这里地图点会分别投影到第二帧和初始化帧中去，即一个地图点会产生两个误差边，值得注意的是，在这部分优化的过程中，需要将初始化帧位姿对应的顶点进行固定。
2. 恒速模型跟踪阶段：BA优化（地图点位置固定，优化跟踪地图点的当前帧位姿）
   1. 在恒速模型跟踪阶段，ORB-SLAM系统认为，被跟踪的地图点的位置是准确无误的，因此以被跟踪的地图点作为参照，将被跟踪地图点投影到当前帧的像素坐标系中去，这里地图点对应的顶点只有一条误差边，值得注意的是，这部分地图点的只是投影到当前帧的像素坐标系中去，对当前地图点对应的其他帧不进行优化处理。值得注意的是，这部分优化过程中，需要将跟踪到的地图点位置固定。
3. 基于上一帧的跟踪阶段：BA优化（地图点位置固定，优化跟踪地图点的当前帧位姿）
   1. 基于上一帧的跟踪阶段和基于恒速模型的跟踪阶段对应的优化是相同的，只是跟踪匹配的方式不同，基于恒速模型跟踪的是重投影匹配的方式，即将上一帧对应的地图点，基于速度信息（位姿偏差）进行重投影匹配，而基于上一帧的跟踪阶段使用的是窗口匹配的方式，即在当前帧的对应上一帧的特征点的位置画一个圆，根据先验的金字塔层级进行2D描述子之间距离计算和匹配。
4. 重定位阶段：BA优化（地图点位置固定，优化跟丢的当前帧位姿）
   1. 在重定位阶段，将当前重定位的候选匹配关键帧拿出来之后，使用基于词袋的匹配方式，找到与当前帧的特征点对应的地图点，将这些地图点投影到当前帧中来，使用和恒速模型相同的优化手段进行优化。
5. 局部地图点跟踪阶段：BA优化（地图点位置固定，通过局部地图增加匹配，对当前帧位姿进行二次优化）
   1. 在跟踪线程中，跟踪局部地图是上述跟踪阶段的后面，主要目的就是通过二阶相连共视图的方式寻找构建局部地图，使用局部地图进行匹配数目的增加，值得注意的是，这里的优化手段依然固定地图点的方式，与恒速模型跟踪中使用的优化方式一致。

2 局部建图线程中的优化策略

1. 局部BA阶段：BA优化（与当前关键帧二阶相连关键帧固定，优化当前关键帧，与当前关键帧一阶相连关键帧的位姿和地图点位置）
   1. 局部建图线程中，维护了最近新增地图点信息，以此为媒介，删除全局地图中的冗余地图点信息（只有这个作用！！）
   2. 在局部建图阶段的BA优化中，拿出与当前关键帧一阶相连的关键帧、二阶向量的关键帧，以当前关键帧和与其一阶相连的关键帧中的地图点作为局部地图点。以局部地图点和上述涉及到的关键帧为顶点，构建地图点和关键帧之间的重投影约束。值得注意的是，其中的二阶相连关键帧只提供额外的约束信息，并不参与优化，进行固定。

3 回环闭合线程中的优化策略

1. 计算SIM3阶段：BA优化（固定地图点，优化回环闭合两帧之间的SIM3矩阵）
   1. 这里值得注意的是，在回环闭合线程中，使用RANSAC算法得到初步的SIM3变换矩阵后，使用基于SIM3的相互投影匹配，实现了两关键帧之间的匹配，与之前匹配方式不同的是，两关键帧对应特征点匹配成功后，都对应的是同一个地图点，但是这种情况匹配成功后两关键帧对应的还是两个不同的地图点，只是由于位姿误差积累和尺度误差导致的，这里就是对闭环关键帧对应的SIM3矩阵进行计算，以此来校正误差。
   2. 该优化过程涉及到的顶点有，匹配特征点对应的不同地图点位置，两个关键帧之间对应的一个SIM3变换矩阵，使用该变换矩阵将匹配特征点对应的地图点进行相互投影，产生优化边。值得注意的是，在优化过程中，需要将对应位置的地图点进行固定。
2. 位姿校正阶段：使用本质图优化（位姿图优化，固定回环闭合匹配关键帧的位姿）
   1. 位姿图优化中的边包含的内容如下：
      1. 生成树内的关键帧对应的连接边
      2. 历史产生的所有的回环连接边
      3. 共视图中，共视权重超过100的连接边
      4. 本次回环新生成的连接边中满足共视权重大于100或者满足有一端连接是当前关键帧或者当前回环闭合匹配关键帧的连接边
   2. 所有的关键帧都参与位姿图优化