企业官网建设流程全解析

做了几年视觉SLAM的工程，最怕的不是新算法学不会，而是调一个看起来人畜无害的后端优化，结果 LM 迭代四五十次还不收敛，残差曲线一直原地抖。最后排查下来，十有八九是重投影误差的雅可比矩阵算错了。在 VSLAM 后端优化里，每一轮求解增量方程之前都要对大量观测逐个算出误差向量对位姿和路标点的导数，这就是本文要完整推导的雅可比。这套 2×6、2×3 的小矩阵决定了梯度的方向和大小，错了优化就错，纯靠数值差分在工程性能上也撑不住。这篇文章适合所有读 SLAM 源码读到 BA 部分、却被矩阵公式卡住的开发者，也适合刚写完一个 VO 工程、想系统核对符号和坐标约定的工程师。我会先把约定钉死，再按链式法则一步步推，最后给一个数值差分校验脚本，让你能在代码里验证自己写的雅可比到底对不对。

1. 后端优化的最小闭环：雅可比是怎么钻进增量方程的

1.1 一次 LM 迭代里，雅可比要做的事

视觉 SLAM 的后端，本质上在解一个大规模非线性最小二乘：把所有相机位姿和所有路标点放进同一个状态向量，最小化投影残差。常见形式是

$$ \min_{\mathbf{x}} \sum_{i,j} \mathbf{e}{ij}^T \mathbf{\Omega}{ij} \mathbf{e}_{ij} $$

其中 $\mathbf{e}{ij}$ 是第 i 个位姿观察第 j 个路标点时产生的 2 维重投影误差，$\mathbf{\Omega}{ij}$ 是信息矩阵。

求解这种问题的标准套路是 Gauss-Newton 或 Levenberg-Marquardt。把误差函数在当前位置 $\mathbf{x}$ 处一阶展开：

$$ \mathbf{e}(\mathbf{x} + \Delta \mathbf{x}) \approx \mathbf{e}(\mathbf{x}) + \mathbf{J} \Delta \mathbf{x} $$

然后解增量方程

$$ (\mathbf{J}^T \mathbf{\Omega} \mathbf{J} + \lambda \mathbf{D}) \Delta \mathbf{x} = -\mathbf{J}^T \mathbf{\Omega} \mathbf{e} $$

这里的 $\mathbf{J}$ 就是每个误差项对状态向量的雅可比矩阵。放在 VSLAM 里，状态向量包含所有位姿和所有路标点，$\mathbf{J}$ 理论上是一个 $2M \times (6P + 3L)$ 的大矩阵（$M$ 是观测数，$P$ 是位姿数，$L$ 是点数），但实际绝不会显式构建它。因为每个观测只连接一个位姿和一个路标点，所以只需要知道两块：误差对位姿扰动的 2×6 块，误差对路标点的 2×3 块。把这些小块按稀疏模式累加进对应的海森矩阵块，增量方程就能求解。

1.2 为什么用解析雅可比，而不是数值差分

有些入门工程偷懒，用中心差分去算雅可比。理论上没问题，工程上一塌糊涂。假设有 1 万对观测，每轮迭代用数值差分算一次雅可比，每个观测都要额外做十几次投影运算（位姿 6 维、路标点 3 维，中心差分翻倍），一轮迭代多出几十万次投影，优化几十轮就是千万级，性能直接崩。

数值差分还有精度问题：步长取大了有截断误差，取小了有浮点舍入误差；遇到点正好在相机前方很近、Z 很小的情况，差分结果会被噪声吞掉。更麻烦的是，数值差分算错了你也不知道错在哪，它只是在"猜"导数。解析推导虽然前期费点脑力，但一旦写对就是精准、快速、可回归的。我在实际工程里的原则是：新写一个投影函数，解析雅可比必须配套写，写完立刻跑数值差分自检，一步都不省。

2. 动手推导前必须先锁死的三件事：误差、坐标系、扰动

2.1 针孔投影与重投影误差的一次定义

没有约定就没有推导。这里我把整篇文章的约定一次性定死，后面所有公式都基于这一套。

位姿用世界到相机的变换 $\mathbf{T}_{wc} = [\mathbf{R} | \mathbf{t}]$，意思是把世界坐标点变换到相机坐标系：

$$ \mathbf{P}_c = \mathbf{R} \mathbf{P}_w + \mathbf{t} $$

针孔投影模型（忽略畸变）把相机坐标投影成像素坐标：

$$ u = f_x \frac{X}{Z} + c_x, \qquad v = f_y \frac{Y}{Z} + c_y $$

其中 $\mathbf{P}_c = (X, Y, Z)$。

重投影误差定义为观测值减预测值：

$$ \mathbf{e} = \mathbf{z}{obs} - \mathbf{h}(\mathbf{T}{wc}, \mathbf{P}_w) $$

注意这里 $\mathbf{h}$ 是完整投影函数。有些代码会写成 $\mathbf{e} = \mathbf{h} - \mathbf{z}_{obs}$，这两种定义在 Gauss-Newton 求解里是等价的，但必须成对使用：误差定义一旦给定，雅可比符号就跟着定死了。最典型的 bug 是误差用 $\mathbf{z} - \mathbf{h}$，雅可比却抄了 $\mathbf{h} - \mathbf{z}$ 的公式，优化直接发散。

2.2 se(3) 的局部坐标与扰动模型

位姿 $\mathbf{T}$ 属于 SE(3) 李群，不是向量空间，不能直接加增量。所以要引入 se(3) 李代数，用一个 6 维向量 $\boldsymbol{\xi} = (\boldsymbol{\rho}, \boldsymbol{\phi})$ 表示局部坐标，其中 $\boldsymbol{\rho}$ 是平移分量，$\boldsymbol{\phi}$ 是旋转分量。对应的反对称矩阵形式是

$$ \boldsymbol{\xi}^{\wedge} = \begin{bmatrix} \boldsymbol{\phi}^{\wedge} & \boldsymbol{\rho} \ \mathbf{0}^T & 0 \end{bmatrix} $$

这里 $\boldsymbol{\phi}^{\wedge}$ 是三维向量对应的 3×3 反对称矩阵，满足 $\boldsymbol{\phi}^{\wedge} \mathbf{v} = \boldsymbol{\phi} \times \mathbf{v}$。

有了局部坐标，还要约定扰动是加在哪一侧。常见两种：

• 左扰动：$\mathbf{T}' = \exp(\boldsymbol{\delta\xi}^{\wedge}) \mathbf{T}$，扰动发生在相机坐标系一侧；
• 右扰动：$\mathbf{T}' = \mathbf{T} \exp(\boldsymbol{\delta\xi}^{\wedge})$，扰动发生在世界坐标系一侧。

本文全程使用**左扰动 + $\mathbf{T}_{wc}$ + 误差 $\mathbf{e} = \mathbf{z} - \mathbf{h}$**这套组合。这不是唯一选择，但强烈建议你在推导阶段只认准一套，别一会儿左一会儿右。后面的公式全部建立在这套约定上。

3. 第一块拼图：误差对三维路标点的雅可比

3.1 链式法则把投影拆成两段

误差对路标点 $\mathbf{P}_w$ 的导数，要从像素层一路链回世界坐标层。投影函数可以拆成两段：先做坐标变换 $\mathbf{P}_w \to \mathbf{P}_c$，再做针孔投影 $\mathbf{P}_c \to \mathbf{u}$。于是

$$ \frac{\partial \mathbf{h}}{\partial \mathbf{P}_w}

\frac{\partial \mathbf{h}}{\partial \mathbf{P}_c} \cdot \frac{\partial \mathbf{P}_c}{\partial \mathbf{P}_w} $$

第二项非常简单，因为 $\mathbf{P}_c = \mathbf{R} \mathbf{P}_w + \mathbf{t}$，所以

$$ \frac{\partial \mathbf{P}_c}{\partial \mathbf{P}_w} = \mathbf{R} $$

这一项是 3×3 旋转矩阵。很多新手在这丢掉 $\mathbf{R}$，直接把相机坐标系下的导数当成世界坐标系下的导数，错得莫名其妙。

3.2 中间层 2×3 矩阵的逐项推导

真正的功夫在第一项 $\partial \mathbf{h} / \partial \mathbf{P}_c$。把 $\mathbf{P}_c = (X, Y, Z)$ 代入投影公式，先看第一个像素分量：

$$ u = f_x \frac{X}{Z} + c_x $$

对 $X$ 求导：$f_x / Z$；对 $Y$ 求导：$0$；对 $Z$ 求导：$-f_x X / Z^2$。第二个分量同理。合并成矩阵：

$$ \frac{\partial \mathbf{h}}{\partial \mathbf{P}_c}

\begin{bmatrix} \dfrac{f_x}{Z} & 0 & -\dfrac{f_x X}{Z^2} \[4pt] 0 & \dfrac{f_y}{Z} & -\dfrac{f_y Y}{Z^2} \end{bmatrix} $$

这里要理解第三列为什么是负的。归一化坐标 $X/Z$ 对 $Z$ 的导数是 $-X/Z^2$，因为分母变化会让整个比值反向变化——点离相机越远，同样的横向位移引起的像素位移越小，投影对深度是"反比"关系，这就是第三列为负的本质原因。

合起来，投影对世界坐标路标点的雅可比就是

$$ \frac{\partial \mathbf{h}}{\partial \mathbf{P}_w}

\begin{bmatrix} \dfrac{f_x}{Z} & 0 & -\dfrac{f_x X}{Z^2} \[4pt] 0 & \dfrac{f_y}{Z} & -\dfrac{f_y Y}{Z^2} \end{bmatrix} \mathbf{R} $$

如果误差定义是 $\mathbf{e} = \mathbf{z}_{obs} - \mathbf{h}$，那么

$$ \frac{\partial \mathbf{e}}{\partial \mathbf{P}_w} = -\frac{\partial \mathbf{h}}{\partial \mathbf{P}_w} $$

所有用投影函数求导得到的矩阵，在写成误差雅可比时都要记得取负号，这是后端优化里最容易踩的坑之一。

3.3 一个带具体数字的算例

光有公式还是虚，我用一组具体数字走一遍。设内参 $f_x = f_y = 600$，$c_x = c_y = 320$；位姿取 $\mathbf{R} = \mathbf{I}$，$\mathbf{t} = (0.2, -0.3, 0.1)$；世界点 $\mathbf{P}_w = (1.0, 2.0, 3.0)$。

先算相机坐标系下的点：

$$ \mathbf{P}_c = (1.0 + 0.2, 2.0 - 0.3, 3.0 + 0.1) = (1.2, 1.7, 3.1) $$

投影结果：

$$ u = 600 \times \frac{1.2}{3.1} + 320 \approx 552.26, \quad v = 600 \times \frac{1.7}{3.1} + 320 \approx 649.03 $$

取 $\mathbf{z}_{obs} = (553.0, 650.0)$，误差就是 $(0.74, 0.97)$。再看雅可比。因为 $\mathbf{R} = \mathbf{I}$，$\partial \mathbf{h}/\partial \mathbf{P}_w$ 等于 $\partial \mathbf{h}/\partial \mathbf{P}_c$：

$$ \frac{\partial \mathbf{h}}{\partial \mathbf{P}_w} \approx \begin{bmatrix} 193.55 & 0 & -74.92 \ 0 & 193.55 & -106.14 \end{bmatrix} $$

于是误差对路标点的雅可比为

$$ \frac{\partial \mathbf{e}}{\partial \mathbf{P}_w} \approx \begin{bmatrix} -193.55 & 0 & 74.92 \ 0 & -193.55 & 106.14 \end{bmatrix} $$

这个矩阵每个元素都可以用图像直觉验证：比如第三列 74.92，意思是如果路标点沿世界 Z 轴远离相机 1 米，像素误差在 u 方向会增加约 75 像素，符合"点越远、投影对深度越不敏感"的直觉——负号的大小正好由 $Z^2$ 决定。

4. 第二块拼图：误差对位姿扰动的 2×6 雅可比

4.1 左扰动直接作用在相机坐标上

位姿雅可比稍微绕一点，因为增量不直接加在坐标上，而是通过左扰动 $\exp(\boldsymbol{\delta\xi}^{\wedge}) \mathbf{T}_{wc}$ 生效。记 $\boldsymbol{\delta\xi} = (\boldsymbol{\delta\rho}, \boldsymbol{\delta\phi})$，其中 $\boldsymbol{\delta\phi}$ 是旋转扰动，$\boldsymbol{\delta\rho}$ 是平移扰动。

扰动后的相机坐标为

$$ \mathbf{P}_c' = \exp(\boldsymbol{\delta\xi}^{\wedge}) \mathbf{P}_c \approx (\mathbf{I} + \boldsymbol{\delta\xi}^{\wedge}) \mathbf{P}_c $$

展开 $\boldsymbol{\delta\xi}^{\wedge} \mathbf{P}_c$。把 $\mathbf{P}_c$ 写成齐次坐标 $(\mathbf{P}_c; 1)$，得到

$$ \boldsymbol{\delta\xi}^{\wedge} \mathbf{P}_c = \begin{bmatrix} \boldsymbol{\delta\phi}^{\wedge} & \boldsymbol{\delta\rho} \ \mathbf{0}^T & 0 \end{bmatrix} \begin{bmatrix} \mathbf{P}_c \ 1 \end{bmatrix}

\begin{bmatrix} \boldsymbol{\delta\phi} \times \mathbf{P}_c + \boldsymbol{\delta\rho} \ 0 \end{bmatrix} $$

所以

$$ \mathbf{P}_c' \approx \mathbf{P}_c + \boldsymbol{\delta\phi} \times \mathbf{P}_c + \boldsymbol{\delta\rho} $$

利用叉乘性质 $\mathbf{a} \times \mathbf{b} = -\mathbf{b} \times \mathbf{a}$，也就是 $\boldsymbol{\delta\phi} \times \mathbf{P}_c = -[\mathbf{P}c]{\times} \boldsymbol{\delta\phi}$，于是

$$ \frac{\partial \mathbf{P}_c}{\partial \boldsymbol{\delta\rho}} = \mathbf{I}_3, \qquad \frac{\partial \mathbf{P}_c}{\partial \boldsymbol{\delta\phi}} = -[\mathbf{P}c]{\times} $$

注意，旋转扰动的导数带负号，而且中间变量是 $[\mathbf{P}c]{\times}$，意味着雅可比的旋转部分只和点在相机坐标系下的坐标有关。这是左扰动的典型特征。

4.2 链式法则合成完整 2×6 矩阵

有了上面的中间结果，位姿雅可比就是 2×3 投影导数和 6 列中间导数的拼接：

$$ \frac{\partial \mathbf{h}}{\partial \boldsymbol{\delta\xi}}

\frac{\partial \mathbf{h}}{\partial \mathbf{P}_c} \cdot \begin{bmatrix} \mathbf{I}_3 & -[\mathbf{P}c]{\times} \end{bmatrix} $$

把矩阵完整写出来：

$$ \frac{\partial \mathbf{h}}{\partial \boldsymbol{\delta\xi}}

\begin{bmatrix} \dfrac{f_x}{Z} & 0 & -\dfrac{f_x X}{Z^2} & -\dfrac{f_x X Y}{Z^2} & f_x + \dfrac{f_x X^2}{Z^2} & -\dfrac{f_x Y}{Z} \[4pt] 0 & \dfrac{f_y}{Z} & -\dfrac{f_y Y}{Z^2} & -f_y - \dfrac{f_y Y^2}{Z^2} & \dfrac{f_y X Y}{Z^2} & \dfrac{f_y X}{Z} \end{bmatrix} $$

这个矩阵的记忆诀窍很实用：前三列就是 $\partial \mathbf{h}/\partial \mathbf{P}_c$（因为平移扰动等效于直接平移相机坐标），后三列等于 $-\partial \mathbf{h}/\partial \mathbf{P}_c \cdot [\mathbf{P}c]{\times}$。具体算一遍就明白了，比如第一行第四列：

$$ -\frac{f_x}{Z} \cdot 0 + 0 \cdot Z + \left(-\frac{f_x X}{Z^2}\right)(-Y) = -\frac{f_x X Y}{Z^2} $$

第一行第五列：

$$ -\frac{f_x}{Z}(-Z) + 0 \cdot 0 + \left(-\frac{f_x X}{Z^2}\right) X = f_x + \frac{f_x X^2}{Z^2} $$

这也解释了第五列为什么有个 $f_x$ 常数项：绕相机 Y 轴的小旋转会让点横向平移、同时改变深度，横向运动对应 $f_x/Z$，深度变化又通过第三列 $-f_x X/Z^2$ 贡献一项，两者叠加出了 $f_x + f_x X^2/Z^2$。

同 3.2 节一致，如果误差定义为 $\mathbf{e} = \mathbf{z}_{obs} - \mathbf{h}$，那么

$$ \frac{\partial \mathbf{e}}{\partial \boldsymbol{\delta\xi}} = -\frac{\partial \mathbf{h}}{\partial \boldsymbol{\delta\xi}} $$

用上一节的数字代入，$\mathbf{P}_c = (1.2, 1.7, 3.1)$，$f_x = f_y = 600$，得到

$$ \frac{\partial \mathbf{h}}{\partial \boldsymbol{\delta\xi}} \approx \begin{bmatrix} 193.55 & 0 & -74.92 & -163.60 & 689.91 & -329.03 \ 0 & 193.55 & -106.14 & -780.44 & 127.28 & 232.26 \end{bmatrix} $$

写代码时可以拿这组数字对拍，任何一列差符号或差数值，都说明推导或者实现出了问题。

4.3 如果位姿存成了 T_cw：分块和符号都会变

很多 VSLAM 工程把相机位姿存成 $\mathbf{T}{cw}$（相机到世界），投影时先求逆。这时候不能照抄上面的公式。假设左扰动作用在 $\mathbf{T}{cw}$ 上：

$$ \mathbf{T}{cw}' = \exp(\boldsymbol{\delta\xi}^{\wedge}) \mathbf{T}{cw} $$

新的相机坐标为

$$ \mathbf{P}c' = (\mathbf{T}{cw}')^{-1} \mathbf{P}w \approx \mathbf{T}{cw}^{-1} (\mathbf{I} - \boldsymbol{\delta\xi}^{\wedge}) \mathbf{P}_w $$

推导得到

$$ \frac{\partial \mathbf{P}c}{\partial \boldsymbol{\delta\rho}} = -\mathbf{R}{cw}^T, \qquad \frac{\partial \mathbf{P}c}{\partial \boldsymbol{\delta\phi}} = \mathbf{R}{cw}^T [\mathbf{P}w]{\times} $$

对比一下就能看到，$\mathbf{T}{cw}$ 约定下的雅可比分块和 $\mathbf{T}{wc}$ 约定完全不同：平移分块变成了 $-\mathbf{R}^T$，旋转分块变成了 $\mathbf{R}^T [\mathbf{P}w]{\times}$，而且中间变量用的是世界点而不是相机点。这就是为什么很多抄公式工程"符号老是反"——位姿存法没对上，再正确的推导也白搭。

5. 把两块拼图装进 BA：稀疏增量方程与分块组装

5.1 完整状态向量下的海森分块

搞定了两小块雅可比，接下来看它们怎么拼回整个 BA 系统。设状态向量为

$$ \mathbf{x} = (\boldsymbol{\xi}_1, \dots, \boldsymbol{\xi}_P, \mathbf{p}_1, \dots, \mathbf{p}_L)^T $$

对第 $i$ 个位姿观察第 $j$ 个路标点的误差项，雅可比只有两个非零块。把小块累加进整体海森矩阵，得到分块结构：

$$ \mathbf{H} = \begin{bmatrix} \mathbf{U} & \mathbf{W} \ \mathbf{W}^T & \mathbf{V} \end{bmatrix}, \qquad \mathbf{g} = \begin{bmatrix} \mathbf{g}{\xi} \ \mathbf{g}{p} \end{bmatrix} $$

其中 $\mathbf{U}$ 是 $6P \times 6P$ 的位姿块，$\mathbf{V}$ 是 $3L \times 3L$ 的路标块，$\mathbf{W}$ 是位姿和路标之间的耦合块。

关键性质在于 $\mathbf{V}$ 是对角块结构：每个路标点只和它被观测到的那些位姿有关，点与点之间没有直接约束。因此每个点的 3×3 块只由它自己的雅可比累加而成：

$$ \mathbf{V}j = \sum{i \in \mathcal{O}j} \mathbf{J}{p,ij}^T \mathbf{\Omega}{ij} \mathbf{J}{p,ij} $$

耦合块则是

$$ \mathbf{W}{ij} = \mathbf{J}{\xi,ij}^T \mathbf{\Omega}{ij} \mathbf{J}{p,ij} $$

如果你的后端带协方差或鲁棒核，信息矩阵 $\mathbf{\Omega}_{ij}$ 也要乘进来，这就解释了为什么每个观测的信息权重可以直接影响海森矩阵。

5.2 舒尔消元：先解位姿，再回代路标

直接求解 $(P \times 6 + L \times 3)$ 维的稠密系统是不现实的，大规模 BA 靠的是利用 $\mathbf{V}$ 的对角块结构做舒尔消元，也叫边缘化路标点。把增量方程写成

$$ \begin{bmatrix} \mathbf{U} & \mathbf{W} \ \mathbf{W}^T & \mathbf{V} \end{bmatrix} \begin{bmatrix} \Delta \boldsymbol{\xi} \ \Delta \mathbf{p} \end{bmatrix}

\begin{bmatrix} -\mathbf{g}{\xi} \ -\mathbf{g}{p} \end{bmatrix} $$

从第二个方程解出 $\Delta \mathbf{p} = \mathbf{V}^{-1}(-\mathbf{g}_p - \mathbf{W}^T \Delta \boldsymbol{\xi})$，代入第一个方程得到只含位姿的舒尔补系统：

$$ (\mathbf{U} - \mathbf{W} \mathbf{V}^{-1} \mathbf{W}^T) \Delta \boldsymbol{\xi} = -\mathbf{g}_{\xi} + \mathbf{W} \mathbf{V}^{-1} \mathbf{g}_p $$

由于 $\mathbf{V}$ 是分块对角的，求逆非常便宜。先解位姿增量，再回代求每个路标点的增量。仔细看这个过程就能理解：路标点雅可比一旦算错，不仅 $\Delta \mathbf{p}$ 错，还会通过 $\mathbf{W} \mathbf{V}^{-1} \mathbf{W}^T$ 和 $\mathbf{W} \mathbf{V}^{-1} \mathbf{g}_p$ 污染位姿增量。所以"点的雅可比错了一点，位姿也跟着偏"不是玄学，是线性代数结构决定的必然结果。

6. 数值差分校验：用代码让推导可证伪

6.1 校验的中心思想与 eps 选择

公式推完，必须用代码验证。数值差分校验的中心思想很简单：用中心差分近似导数，和分析解析雅可比逐元素对比。

$$ \frac{\partial f}{\partial x_k} \approx \frac{f(x_k + \epsilon) - f(x_k - \epsilon)}{2\epsilon} $$

步长 $\epsilon$ 的选择很关键。float64 下一般取 $10^{-6}$ 到 $10^{-7}$；如果你在某套单精度实现里做校验，步长要放到 $10^{-3}$ 量级，否则差分结果会被舍入噪声淹没。不要问为什么序列不收敛时先怀疑雅可比——先用这个流程把雅可比钉死，再谈别的调参。

6.2 一个可直接运行的 Python 校验脚本

下面这套脚本我一直在用，换任何投影模型都能快速改出对应版本。核心是三段：SE(3) 指数映射、投影函数、解析雅可比。

import numpy as np I3 = np.eye(3) def skew(v): return np.array([[0.0, -v[2], v[1]], [v[2], 0.0, -v[0]], [-v[1], v[0], 0.0]]) def se3_exp(xi): """se(3) 指数映射，xi = [rho(3), phi(3)]""" rho, phi = xi[:3], xi[3:] theta = np.linalg.norm(phi) S = skew(phi) if theta < 1e-9: R, V = I3, I3 else: R = I3 + np.sin(theta) / theta * S + (1.0 - np.cos(theta)) / theta**2 * (S @ S) V = I3 + (1.0 - np.cos(theta)) / theta**2 * S + (theta - np.sin(theta)) / theta**3 * (S @ S) T = np.eye(4) T[:3, :3] = R T[:3, 3] = V @ rho return T def project(Twc, Pw, K): fx, fy = K[0, 0], K[1, 1] cx, cy = K[0, 2], K[1, 2] Pc = Twc[:3, :3] @ Pw + Twc[:3, 3] X, Y, Z = Pc return np.array([fx * X / Z + cx, fy * Y / Z + cy]), Pc def analytic_jac(Twc, Pw, K): fx, fy = K[0, 0], K[1, 1] Pc = Twc[:3, :3] @ Pw + Twc[:3, 3] X, Y, Z = Pc dhdPc = np.array([ [fx / Z, 0.0, -fx * X / Z**2], [0.0, fy / Z, -fy * Y / Z**2] ]) dhdPw = dhdPc @ Twc[:3, :3] # 左扰动：xi = (rho, phi)，前三列平移，后三列旋转 dhdxi = np.hstack([dhdPc, -dhdPc @ skew(Pc)]) return dhdPw, dhdxi def num_jac_point(Twc, Pw, K, eps=1e-6): J = np.zeros((2, 3)) for k in range(3): d = np.zeros(3) d[k] = eps up, _ = project(Twc, Pw + d, K) um, _ = project(Twc, Pw - d, K) J[:, k] = (up - um) / (2 * eps) return J def num_jac_pose(Twc, Pw, K, eps=1e-6): J = np.zeros((2, 6)) for k in range(6): xi = np.zeros(6); xi[k] = eps Tp = se3_exp(xi) @ Twc xi = np.zeros(6); xi[k] = -eps Tm = se3_exp(xi) @ Twc up, _ = project(Tp, Pw, K) um, _ = project(Tm, Pw, K) J[:, k] = (up - um) / (2 * eps) return J def rot_x(a): c, s = np.cos(a), np.sin(a) return np.array([[1.0, 0.0, 0.0], [0.0, c, -s], [0.0, s, c]]) def rot_y(a): c, s = np.cos(a), np.sin(a) return np.array([[c, 0.0, s], [0.0, 1.0, 0.0], [-s, 0.0, c]]) T0 = np.eye(4) T0[:3, :3] = rot_x(0.15) @ rot_y(-0.1) T0[:3, 3] = [0.2, -0.3, 1.0] Pw = np.array([1.0, 2.0, 3.0]) K = np.array([[600.0, 0.0, 320.0], [0.0, 600.0, 320.0], [0.0, 0.0, 1.0]]) Jp_a, Jxi_a = analytic_jac(T0, Pw, K) Jp_n = num_jac_point(T0, Pw, K) Jxi_n = num_jac_pose(T0, Pw, K) print("point jacobian max diff:", np.abs(Jp_a - Jp_n).max()) print("pose jacobian max diff:", np.abs(Jxi_a - Jxi_n).max())

这段脚本里数值差分用的是完整 SE(3) 指数映射，解析雅可比用的是线性展开模型，两者在 $\epsilon = 10^{-6}$ 下吻合到 $10^{-6}$ 量级。如果输出差异在 $10^{-5}$ 以内，说明雅可比正确；如果差异在 $10^{-2}$ 以上，就是真错了。

6.3 差异模式的快速诊断表

验证发现不吻合时，不要瞎猜，按差异模式来定位。我在项目里存了一张表，很好用：

这五类问题覆盖了我在实际代码评审里见过的大多数雅可比 bug。

7. 从推导到落地最容易翻车的五个细节

7.1 误差定义与雅可比符号必须成对出现

后端优化里，误差可以定义为 $\mathbf{z} - \mathbf{h}$，也可以定义为 $\mathbf{h} - \mathbf{z}$。两种定义下 GN 的增量结果完全一致，因为 $J^T J$ 对符号不敏感，梯度和残差会成对地翻负号。但如果你把误差定义和雅可比公式从不同资料里混着抄，梯度的符号就错了，优化必定发散。我的习惯是在每个代价函数文件头部注释写清楚：本文件统一 $\mathbf{e} = \mathbf{z} - \mathbf{h}$，雅可比一律是 $\partial \mathbf{e}/\partial(\cdot)$，不是 $\partial \mathbf{h}/\partial(\cdot)$。

7.2 T_wc 和 T_cw 的混乱是重灾区

前面推导已经给出：位姿存成 $\mathbf{T}{wc}$ 还是 $\mathbf{T}{cw}$，投影时的链式法则完全不同。最隐蔽的是有些代码内部用 $\mathbf{T}{cw}$ 存顶点，投影函数里却写"先求逆再用 $\mathbf{T}{wc}$ 公式"，结果视觉上对，雅可比却错。排查这个坑时，不要只看投影结果对不对，要检查雅可比公式到底对应哪种位姿约定。通常一个特征：$\mathbf{T}_{wc}$ 左扰动公式里，旋转分块只出现 $[\mathbf{P}c]{\times}$；一旦出现 $\mathbf{R}^T$ 或 $[\mathbf{P}w]{\times}$，说明你实际在用相反的坐标流。

7.3 左扰动公式不能直接套右扰动场景

左扰动和右扰动在数学上差一个伴随变换，数值上就是分块符号、甚至旋转分块整体不同。你在一个用右扰动的求解器里，套了左扰动推导的 2×6 矩阵，增量方程同样会乱。怎么判断当前场景用哪种扰动？看求解器对位姿顶点重投影时，"加增量"是在位姿左侧还是右侧。读一下图优化框架的顶点更新函数就清楚了，别只看文档。

7.4 畸变模型不会免费消失

这篇文章全部基于理想针孔模型，也是不少 VSLAM 前端在预处理阶段已经把特征点去畸变后的常用假设。但如果你的投影函数里带了径向或切向畸变，像素结果是 $\mathbf{u}d = \text{distort}(\mathbf{u}{ideal})$，那么雅可比还必须多乘一层畸变模型的导数 $\partial \mathbf{u}d / \partial \mathbf{u}{ideal}$。忽略这一层，带畸变模型的工程用理想针孔公式，初期可能勉强能跑，拐弯多了必发散。这也是为什么很多后端的接口要求输入"已去畸变特征点"——不是前端懒得算，是后端想保住这套简洁雅可比。

7.5 鲁棒核和协方差并不会让雅可比变成别的

用了 Huber 之类的鲁棒核，代价函数变成 $\rho(\mathbf{e}^T \mathbf{e})$，实际作用是在增量方程里给每个残差加一个权重，雅可比矩阵本身结构不变。协方差加权也是同理，只是给每个观测的块乘上信息矩阵。经常看到有人为了"适配鲁棒核"把雅可比乱改，改完优化更不稳定。正确的做法是权重放在外层：先在雅可比和残差层面保持干净，再在组装海森矩阵时统一乘权重。

8. 一点从项目里换来的经验

最后分享几个切身教训。第一，代码里务必显式保存中间变量 $\mathbf{P}_c$，不要为了省两行代码把 $\mathbf{R} \mathbf{P}_w + \mathbf{t}$ 整个塞进投影函数的一行表达里。后期一旦要切换坐标约定，那些长表达式会让你改到怀疑人生。第二，每写一个新投影函数，第一时间跑一遍 6.2 节的数值差分脚本再提交，别等整个 BA 图跑挂了再回头查雅可比，那时排查成本至少翻十倍。第三，在工程里保留一个固定随机种子的"黄金测试用例"，把解析雅可比和一组标准数值存成基线；每次大规模改动后跑一遍回归，任何符号或坐标约定的意外变更都能立刻暴露。

重投影误差的雅可比推导本身并不难，难的是把约定统一、把链式法则拆干净、再把每个符号跟代码对上。希望这篇推导能让你下次读后端优化代码时，一眼看出某个 2×6 矩阵是从哪个扰动模型、哪种位姿约定里长出来的。