Basic Calculus

SLAM 的后端把大部分时间都花在最小化非线性代价函数上。微积分中的两个工具让这件事变得可行：求导（雅可比矩阵）和泰勒展开（线性化）。

求导与雅可比矩阵

标量函数 f:Rn→Rf: \mathbb{R}^n \to \mathbb{R} 具有梯度 ∇f=[∂f∂x1,…,∂f∂xn]T\nabla f = \left[\frac{\partial f}{\partial x_1}, \ldots, \frac{\partial f}{\partial x_n}\right]^T。向量函数 f:Rn→Rm\mathbf{f}: \mathbb{R}^n \to \mathbb{R}^m 具有雅可比矩阵：

J=∂f∂x=[∂f1∂x1⋯∂f1∂xn⋮⋱⋮∂fm∂x1⋯∂fm∂xn]∈Rm×nJ = \frac{\partial \mathbf{f}}{\partial \mathbf{x}} = \begin{bmatrix} \frac{\partial f_1}{\partial x_1} & \cdots & \frac{\partial f_1}{\partial x_n} \\ \vdots & \ddots & \vdots \\ \frac{\partial f_m}{\partial x_1} & \cdots & \frac{\partial f_m}{\partial x_n} \end{bmatrix} \in \mathbb{R}^{m \times n}

雅可比矩阵是 SLAM 优化中的关键工具：给定一个残差 e(x)\mathbf{e}(\mathbf{x})（例如重投影误差），其雅可比矩阵 J=∂e∂xJ = \frac{\partial \mathbf{e}}{\partial \mathbf{x}} 告诉我们残差如何随状态的小扰动变化——这正是高斯-牛顿法和列文伯格-马夸尔特法所需要的信息。

SLAM 残差中的链式法则

SLAM 残差几乎总是若干更简单函数的复合，因此它们的雅可比矩阵来自链式法则。在位姿 TT 下、在像素 z\mathbf{z} 处观测到的地图点 X\mathbf{X} 的重投影误差为

e=z−π(T X)\mathbf{e} = \mathbf{z} - \pi\big(T\,\mathbf{X}\big)

这是（1）刚体变换、（2）透视除法和（3）内参映射三者的复合。其雅可比矩阵可以分解为乘积：

∂e∂x=− ∂π∂Xc⋅∂Xc∂x\frac{\partial \mathbf{e}}{\partial \mathbf{x}} = -\,\frac{\partial \pi}{\partial \mathbf{X}_c}\cdot\frac{\partial \mathbf{X}_c}{\partial \mathbf{x}}

其中 Xc=TX\mathbf{X}_c = T\mathbf{X} 是相机坐标系下的点。分别推导每一项再相乘，比一次性对整个表达式求导要不容易出错得多——这也正是 SLAM 库组织其解析雅可比矩阵的方式。

泰勒展开

泰勒级数在点 x0x_0 附近展开光滑函数 ff：

f(x)=f(x0)+f′(x0)(x−x0)+12!f′′(x0)(x−x0)2+⋯f(x) = f(x_0) + f'(x_0)(x - x_0) + \frac{1}{2!}f''(x_0)(x - x_0)^2 + \cdots

对于多元函数 f(x)f(\mathbf{x}) 在 x0\mathbf{x}_0 附近：

f(x)≈f(x0)+J(x0)(x−x0)+12(x−x0)TH(x0)(x−x0)f(\mathbf{x}) \approx f(\mathbf{x}_0) + J(\mathbf{x}_0)(\mathbf{x} - \mathbf{x}_0) + \frac{1}{2}(\mathbf{x} - \mathbf{x}_0)^T H(\mathbf{x}_0)(\mathbf{x} - \mathbf{x}_0)

其中 JJ 是雅可比矩阵（一阶项），HH 是海森矩阵（二阶项）。截断到一阶得到高斯-牛顿法所用的线性近似；截断到二阶得到牛顿法所用的二次近似。

从泰勒展开到高斯-牛顿法

考虑最小化残差平方和 F(x)=12∥e(x)∥2F(\mathbf{x}) = \frac{1}{2}\|\mathbf{e}(\mathbf{x})\|^2。在当前估计值附近对残差线性化，e(x+Δx)≈e+JΔx\mathbf{e}(\mathbf{x} + \Delta\mathbf{x}) \approx \mathbf{e} + J\Delta\mathbf{x}，代入后得到：

F(x+Δx)≈12∥e∥2+eTJ Δx+12ΔxTJTJ ΔxF(\mathbf{x} + \Delta\mathbf{x}) \approx \frac{1}{2}\|\mathbf{e}\|^2 + \mathbf{e}^T J\,\Delta\mathbf{x} + \frac{1}{2}\Delta\mathbf{x}^T J^T J\,\Delta\mathbf{x}

这是关于 Δx\Delta\mathbf{x} 的二次函数；令其导数为零，得到正规方程：

(JTJ) Δx=−JTe(J^T J)\,\Delta\mathbf{x} = -J^T \mathbf{e}

因此高斯-牛顿法本质上是”用 H≈JTJH \approx J^T J 近似的牛顿法”——真实海森矩阵中的二阶导数项被舍去了，当残差较小时这是一个很好的近似。列文伯格-马夸尔特法加入了一个阻尼项，求解 (JTJ+λI)Δx=−JTe(J^TJ + \lambda I)\Delta\mathbf{x} = -J^T\mathbf{e}，在高斯-牛顿法（λ→0\lambda \to 0）和梯度下降法（λ\lambda 较大）之间插值。

数值检查雅可比矩阵

解析雅可比矩阵出错是出了名的容易（一个符号、一个转置错位的分块）。标准的合理性检查是中心差分法：每次扰动一个状态维度，比较

J:,k≈e(x+h 1k)−e(x−h 1k)2hJ_{:,k} \approx \frac{\mathbf{e}(\mathbf{x} + h\,\mathbf{1}_k) - \mathbf{e}(\mathbf{x} - h\,\mathbf{1}_k)}{2h}

其中步长很小（例如 h∼10−6h \sim 10^{-6}）。每一个正规的 SLAM 代码库都会针对每种残差类型编写这样的单元测试。

常见陷阱

对SLAM的意义

在光束法平差中，重投影误差 e(x+Δx)≈e(x)+JΔx\mathbf{e}(\mathbf{x} + \Delta\mathbf{x}) \approx \mathbf{e}(\mathbf{x}) + J\Delta\mathbf{x} 在当前估计值附近被线性化。这把非线性最小二乘问题转化为一系列线性系统 (JTJ)Δx=−JTe(J^T J)\Delta\mathbf{x} = -J^T \mathbf{e}，通过迭代求解。每一个基于优化的 SLAM 系统——从位姿图优化到完整的光束法平差——都建立在这个”线性化-求解-更新”循环之上，因此能够手动推导雅可比矩阵（并用数值方法检验）是一项核心技能。

相关条目