高斯牛顿法(GN)与列文伯格-马夸尔特方法在ORB-SLAM3中的应用

问题背景

高斯牛顿法（Gauss-Newton, GN）和列文伯格-马夸尔特方法（Levenburg-Marquadt, LM）是两种最常用的非线性优化方法，这两种方法在ORB-SLAM3系统中均有使用。

在ORB-SLAM3前端跟踪线程(Tracking)中，局部地图跟踪过程 Tracking::TrackLocalMap 的优化函数Optimizer::PoseInertialOptimizationLastFrame 使用了高斯牛顿法作为非线性优化问题求解器，代码如下：

// Optimizer::PoseInertialOptimizationLastFrame(Frame *pFrame, bool bRecInit) 函数
g2o::OptimizationAlgorithmGaussNewton *solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr);

而在ORB-SLAM3后端局部建图线程(LocalMapping)中，局部地图优化函数 Optimizer::LocalInertialBA 却使用了列文伯格-马夸尔特作为其非线性优化问题的求解器，代码如下：

//  Optimizer::LocalInertialBA 函数
g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);

为什么在不同的阶段使用了这两种不同的非线性优化求解方法，这与ORB-SLAM3不同线程中实时性和效率的要求、优化问题的规模等问题有关，下面具体进行叙述分析。

前端跟踪中使用高斯牛顿法的原因

• 实时性要求高
  跟踪线程需要实时运行，以确保相机的位姿能够在每一帧图像抵达时及时更新。高斯-牛顿法计算效率较高，迭代过程较为简单，能够满足实时性要求。

• 初始估计较好
  在跟踪线程中，当前帧的位姿初始估计通常由前一帧的位姿和两帧之间的运动测量推导而来，初始值较为准确。高斯-牛顿法在初始估计较好的情况下能够快速收敛，不需要复杂的迭代调整。

• 问题规模较小
  跟踪线程的目标是优化得到当前帧的位姿，跟踪线程中处理的优化问题规模相对较小，只涉及当前帧和部分局部地图点的关系。参数数量有限，问题条件较好，不容易出现病态约束。高斯-牛顿法在这种情况下能够提供足够的优化效果。

后端建图优化中使用LM法的原因

• 鲁棒性和稳定性
  局部建图线程中，局部捆绑调整涉及大量关键帧和地图点的优化，优化问题的规模较大且参数空间复杂。这种复杂性导致误差函数可能具有更强的非线性和病态约束，即误差函数的Hessian矩阵可能接近奇异，导致优化过程不稳定。Levenberg-Marquardt方法通过在高斯-牛顿法基础上引入阻尼因子，可以在处理这样的误差函数时表现得更加鲁棒和稳定，避免迭代发散或陷入局部最优解。

• 全局优化需求
  局部建图线程的目标是对局部地图进行精确优化，提升局部窗口内的地图点和位姿的估计精度。Levenberg-Marquardt方法能够更好地进行全局优化，找到误差函数的全局最小值，从而提高优化结果的准确性。

• 计算资源允许
  局部建图线程不需要实时运行，可以在相对较长的时间内进行计算。这使得它可以使用计算复杂度较高但优化效果更好的Levenberg-Marquardt方法。

总结

综上所述，在前端跟踪中使用了GN方法而在后端局部建图中使用了LM方法，主要是考虑计算效率与精度的平衡：

• 跟踪线程主要负责实时的位姿跟踪，要求快速响应，实时性是首要考虑因素，选择计算效率更高的高斯-牛顿法。
• 局部建图线程则负责对局部地图进行精细优化，允许较长的计算时间，选择 Levenberg-Marquardt 方法能够提供更高的优化精度和更强的鲁棒性。

通过在不同的阶段选择不同的优化方法，ORB-SLAM3方法能够在保证实时性能的同时，提高整体系统的精度和稳定性。