做了几年视觉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, eps1e-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, eps1e-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 差异模式的快速诊断表验证发现不吻合时不要瞎猜按差异模式来定位。我在项目里存了一张表很好用差异现象大概率原因整行或整列差一个负号误差 $\mathbf{e}$ 与雅可比的符号约定不匹配前三列和后三列内容互换$\boldsymbol{\xi} (\boldsymbol{\rho}, \boldsymbol{\phi})$ 顺序不一致或左右扰动混用点雅可比差一个旋转因子推导时漏乘 $\mathbf{R}$或把相机坐标当世界坐标求导位姿雅可比只在平移分块出错用了 $\mathbf{T}{wc}$ 的公式套 $\mathbf{T}{cw}$ 的约定数值差随 $\epsilon$ 变化剧烈投影函数里有 if 分支、未做齐次归一化或除零保护这五类问题覆盖了我在实际代码评审里见过的大多数雅可比 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 矩阵是从哪个扰动模型、哪种位姿约定里长出来的。 
长河君 · 2026/10/21 19:18:57 · 阅读 1.2k 视觉SLAM后端优化：重投影误差雅可比矩阵推导与校验
做了几年视觉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, eps1e-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, eps1e-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 差异模式的快速诊断表验证发现不吻合时不要瞎猜按差异模式来定位。我在项目里存了一张表很好用差异现象大概率原因整行或整列差一个负号误差 $\mathbf{e}$ 与雅可比的符号约定不匹配前三列和后三列内容互换$\boldsymbol{\xi} (\boldsymbol{\rho}, \boldsymbol{\phi})$ 顺序不一致或左右扰动混用点雅可比差一个旋转因子推导时漏乘 $\mathbf{R}$或把相机坐标当世界坐标求导位姿雅可比只在平移分块出错用了 $\mathbf{T}{wc}$ 的公式套 $\mathbf{T}{cw}$ 的约定数值差随 $\epsilon$ 变化剧烈投影函数里有 if 分支、未做齐次归一化或除零保护这五类问题覆盖了我在实际代码评审里见过的大多数雅可比 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 矩阵是从哪个扰动模型、哪种位姿约定里长出来的。