Gauss-Newton

Gauss-Newton 是非线性最小二乘问题的基础迭代算法——几乎每一个 SLAM 后端计算(光束法平差、位姿图优化、PnP 精化、直接图像对齐)都归结为这一类问题。它利用代价函数平方和的特殊结构,在只计算一阶导数的情况下获得接近二阶的收敛速度。

推导

我们在状态 x∈Rn\mathbf{x} \in \mathbb{R}^n 上,基于残差向量 e(x)∈Rm\mathbf{e}(\mathbf{x}) \in \mathbb{R}^m 构造代价函数并将其最小化:

F(x)=12∥e(x)∥2F(\mathbf{x}) = \frac{1}{2} \|\mathbf{e}(\mathbf{x})\|^2

在当前估计 xk\mathbf{x}_k 附近,用雅可比矩阵 Jk=∂e/∂xJ_k = \partial \mathbf{e} / \partial \mathbf{x}(在 xk\mathbf{x}_k 处求值)对残差(而非代价函数本身)进行线性化:

e(xk+Δx)≈e(xk)+JkΔx\mathbf{e}(\mathbf{x}_k + \Delta\mathbf{x}) \approx \mathbf{e}(\mathbf{x}_k) + J_k \Delta\mathbf{x}

代入后得到代价函数关于 Δx\Delta\mathbf{x} 的一个二次模型:

F(xk+Δx)≈12∥ek∥2+ΔxTJkTek+12ΔxTJkTJkΔxF(\mathbf{x}_k + \Delta\mathbf{x}) \approx \frac{1}{2}\|\mathbf{e}_k\|^2 + \Delta\mathbf{x}^T J_k^T \mathbf{e}_k + \frac{1}{2} \Delta\mathbf{x}^T J_k^T J_k \Delta\mathbf{x}

令关于 Δx\Delta\mathbf{x} 的导数为零,得到正规方程:

(JkTJk) Δx=−JkTek(J_k^T J_k)\, \Delta\mathbf{x} = -J_k^T \mathbf{e}_k

求解 Δx\Delta\mathbf{x}(实践中通过对稀疏矩阵 JTJJ^T J 做 Cholesky 分解,而绝不显式求逆),更新 xk+1=xk+Δx\mathbf{x}_{k+1} = \mathbf{x}_k + \Delta\mathbf{x},重新线性化,重复直到更新量或代价变化可以忽略。

在有测量协方差的情况下,残差按信息矩阵 Ω=Σ−1\Omega = \Sigma^{-1} 加权,正规方程变为 JTΩJ Δx=−JTΩ eJ^T \Omega J \,\Delta\mathbf{x} = -J^T \Omega\, \mathbf{e}——代数形式相同,而这恰恰就是高斯噪声下的 MAP 估计。

与牛顿法的关系

牛顿法使用 FF 的真实 Hessian 矩阵:

∇2F=JTJ+∑iei ∇2ei\nabla^2 F = J^T J + \sum_i e_i \, \nabla^2 e_i

Gauss-Newton 舍弃了第二项,用 H≈JTJH \approx J^T J 来近似。这样做代价低(不需要二阶导数),并且当最优点处的残差较小(模型拟合良好)或接近线性时(恰恰是一个收敛中的 SLAM 问题所处的情形)是准确的。在解附近,收敛速度接近二次。该近似还保证了 H⪰0H \succeq 0,因此只要 HH 非奇异,计算出的步长就是一个下降方向。

失效模式

在流形上

位姿存在于 SE(3)\mathrm{SE}(3) 而非 Rn\mathbb{R}^n 上,因此更新是通过指数映射来施加的:将增量参数化为 ξ∈R6\boldsymbol{\xi} \in \mathbb{R}^6,求解关于 ξ\boldsymbol{\xi} 的正规方程,然后更新 T←T⋅exp⁡(ξ^)T \leftarrow T \cdot \exp(\hat{\boldsymbol{\xi}})。雅可比矩阵是相对于这个局部扰动求取的。所有 SLAM 求解器(g2o、Ceres、GTSAM)都以这种”在局部向量上优化、再缩回流形”的形式实现 Gauss-Newton/LM。

对SLAM的意义

Gauss-Newton 是 SLAM 的核心内循环。光束法平差就是在重投影误差上运行 Gauss-Newton/LM,其中 JTJJ^T J 的稀疏块结构(位姿仅通过观测与点耦合)通过 Schur 补加以利用;位姿图优化是在相对位姿残差上运行 Gauss-Newton;直接法(LSD-SLAM、DSO)在光度误差上运行它;甚至 ICP 的对齐步骤本身也是一次 Gauss-Newton 迭代。阅读任何 SLAM 后端论文都需要熟练掌握这里定义的词汇——残差、雅可比、Hessian 近似、正规方程、阻尼——并且大多数实际调试工作(为什么我的优化发散了?为什么我的 Hessian 是奇异的?)都能追溯到上面列出的这些假设。

相关条目