orb-slam3的优化函数

orb-slam3的优化函数

ORB-SLAM3中Optimizer.cc中定义的优化函数包括：

ORB-SLAM2中Optimizer.cc中定义的优化函数包括：

单帧优化

1. PoseOptimization

int PoseOptimization(Frame *pFrame)

• 位姿优化，纯视觉时使用。优化目标：单帧的位姿
  • 3D-2D 最小化重投影误差 e = (u,v) - project(Tcw*Pw) \n
  • 只优化Frame的Tcw，不优化MapPoints的坐标
• IO
  • [in] pFrame 待优化的帧
  • 返回内点数目
• 用于：Tracking线程中局部地图运动跟踪。IMU未初始化，或间隔太短时。

2. PoseInertialOptimizationLastKeyFrame

int PoseInertialOptimizationLastKeyFrame(Frame *pFrame, bool bRecInit)

• 使用上一关键帧+当前帧的视觉信息和IMU信息，联合优化当前帧位姿
  • Step 1：创建g2o优化器，初始化顶点和边
  • Step 2：启动多轮优化，剔除外点
  • Step 3：更新当前帧位姿、速度、IMU偏置
  • Step 4：记录当前帧的优化状态，包括参数信息和对应的海森矩阵
• I/O
  • [in] pFrame 当前帧，也是待优化的帧
  • [in] bRecInit 调用这个函数的位置并没有传这个参数，因此它的值默认为false
  • 返回优化后的内点数
• 用于：局部地图跟踪，当局部地图更新时：1. 回环或融合； 2. 局部地图LocalBundleAdjustment. 3. IMU三阶段的初始化

3. PoseInertialOptimizationLastFrame

int PoseInertialOptimizationLastFrame(Frame *pFrame, bool bRecInit)

• 使用上一帧+当前帧的视觉信息和IMU信息，优化当前帧位姿
• 可分为以下几个步骤：
  • Step 1：创建g2o优化器，初始化顶点和边
  • Step 2：启动多轮优化，剔除外点
  • Step 3：更新当前帧位姿、速度、IMU偏置
  • Step 4：记录当前帧的优化状态，包括参数信息和边缘化后的海森矩阵
• I/O
  • [in] pFrame 当前帧，也是待优化的帧
  • [in] bRecInit 调用这个函数的位置并没有传这个参数，因此它的值默认为false
  • 返回优化后的内点数
• 用于： 第二阶段跟踪，当局部地图未更新时：

4. Marginalize

Eigen::MatrixXd Marginalize(const Eigen::MatrixXd &H, const int &start, const int &end)

• PoseInertialOptimizationLastFrame 中使用 Marginalize(H, 0, 14);
• 使用舒尔补的方式边缘化海森矩阵，边缘化。
• I/O
  • H 30*30的海森矩阵
  • start 开始位置
  • end 结束位置
  • 返回H矩阵，大小（6,3,3,3,6,3,3,3）^2

局部地图优化

5. LocalBundleAdjustment

void LocalBundleAdjustment(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, int &num_fixedKF, int &num_OptKF, int &num_MPs, int &num_edges)

• Local Bundle Adjustment ，对当前关键帧及其相邻关键帧进行局部地图优化。获得关键帧位姿、地图点