视觉SLAM必知：高斯牛顿与LM算法在Bundle Adjustment中的性能对比实验

原创

于 2026-02-28 08:06:26 发布 · 1k 阅读

标签

视觉SLAM实战：高斯牛顿与LM算法在BA优化中的深度性能剖析

在视觉SLAM（即时定位与地图构建）系统中，后端优化是决定最终地图精度与轨迹准确性的核心环节。Bundle Adjustment（光束法平差，简称BA）作为后端优化的基石，其本质是一个大规模的非线性最小二乘问题。当我们手头有从特征匹配得到的大量三维点与相机位姿的观测数据时，目标就是调整这些未知参数，使得重投影误差的平方和最小。这个过程听起来简单，但面对成千上万个参数和观测方程，如何高效、稳定地求解，就成了算法工程师们每天都要面对的挑战。

高斯牛顿法（Gauss-Newton）和列文伯格-马夸尔特法（Levenberg-Marquardt, LM）是解决此类非线性最小二乘问题的两柄利剑。坊间教程常会告诉你，高斯牛顿法收敛快但容易“翻车”，LM法更稳健但可能慢一些。然而，在实际的SLAM工程项目中，这种笼统的认知远远不够。算法的选择与调参，往往依赖于对问题本身“病态”程度的直觉、对计算资源的权衡，甚至是对代码库熟悉度的微妙把握。本文将从一个实践者的视角，深入探讨这两种算法在BA场景下的内在机理，并通过设计实验，量化分析它们在收敛速度、稳定性以及对初始值敏感度等方面的表现，最终为你提供一套可落地的算法选择与参数配置策略。

1. 理论基础：从非线性最小二乘到BA问题建模

要理解高斯牛顿和LM在BA中的表现，我们首先需要清晰地构建问题的数学模型。BA优化的是相机位姿（旋转和平移）和三维地图点的坐标。假设我们有 m 个相机位姿和 n 个三维点，第 i 个相机观测到第 j 个三维点，其像素观测值为 z_{ij} ∈ R²。

根据相机投影模型，我们可以计算出该三维点在该相机坐标系下的投影预测值 f_{ij}(ξ_i, p_j)。这里 ξ_i ∈ se(3) 是第 i 个相机位姿的李代数表示（一个6维向量），pj ∈ R³ 是第 j 个三维点的坐标。那么，重投影误差 e{ij} 定义为：

e{ij} = z{ij} - f_{ij}(ξ_i, p_j)

BA的目标函数，即所有观测误差的加权平方和，可以写为：

F(x) = ½ Σ_{i,j} ρ( ||e{ij}||²{Σ_{ij}} )

其中，x = [ξ₁, ..., ξm, p₁, ..., pn]^T 是待优化的全部参数向量，维度通常非常庞大（成千上万）。Σ{ij} 是观测 z{ij} 对应的协方差矩阵，用于加权。ρ(·) 是一个鲁棒核函数（如Huber核），用于抑制外点（错误匹配）的影响，使其不破坏整体的优化。为了聚焦于算法核心对比，我们后续实验会先使用最简单的平方损失（即 ρ(s)=s）。

这个目标函数 F(x) 关于参数 x 是非线性的，因为相机投影模型 f 本身是非线性的。我们的任务就是找到一组参数 x，使得 F(x) 最小化。

注意：在实际的SLAM中，BA问题具有特殊的稀疏结构。相机位姿只与它观测到的点产生关联，点与点之间没有直接联系。这种结构会使得海森矩阵（或高斯牛顿法中的近似海森矩阵）呈现为分块对角或箭头状，利用这种稀疏性可以极大加速求解，这也是g2o、Ceres等优化库的核心优势。本文的算法对比是在利用了这一稀疏性求解增量方程的前提下进行的。

1.1 迭代优化框架与两种策略

无论是高斯牛顿还是LM，都遵循一个共同的迭代框架：

给定初始猜测 x_k。
在当前点 x_k 处，对目标函数进行局部模型近似。
基于该近似模型，求解一个子问题，得到参数增量 Δx。
检查该增量是否能使目标函数下降，并决定是否接受：x_{k+1} = x_k + Δx。
重复步骤2-4，直到满足收敛条件（如增量范数小于阈值或目标函数下降不明显）。

两者的核心区别在于 步骤2和3中如何构建局部模型以及如何控制增量 Δx 的大小。

高斯牛顿法 采用一阶泰勒展开来近似误差函数 e(x)，从而得到一个关于增量 Δx 的二次代价函数。它直接求解这个二次函数的最小值，没有显式地限制增量步长。其增量方程来源于对近似二次函数的导数置零。
LM方法 则属于信赖域法。它意识到泰勒展开的近似只在当前点附近的一个小区域内有效。因此，它在构建二次模型的同时，额外增加了一个约束，要求增量 Δx 的范数被限制在一个信赖区域半径 μ 内。求解的是一个带约束的优化问题。

这种根本性的差异，导致了它们截然不同的数值行为。

2. 算法机理深度解析与直观对比

让我们暂时抛开复杂的公式，从几何和计算的角度来感受一下这两种算法。

2.1 高斯牛顿法：在局部抛物线里寻找谷底

想象你站在一个复杂山谷的某处，四周浓雾弥漫（非线性），你只能看清脚下很小一块地方。高斯牛顿法的策略是：蹲下来，用手触摸地面，感受当前点的坡度（雅可比矩阵 J）和地面的弯曲趋势（用 J^TJ 来近似曲率）。然后，你根据这个局部感受，拟合出一个完美的抛物线（二次模型），并直接走到这个抛物线的底部。你认为这就是下山方向上的最优一点。

它的增量方程 推导自最小化 ||e + JΔx||²，最终形式为：

(J^T J) Δx = -J^T e

这里 H_gn = J^T J 被称为高斯牛顿海森矩阵（近似海森矩阵）。求解这个线性方程组，就得到了增量 Δx。

优势与风险：