PnP 协方差怎么估：从像素噪声传播到位姿退化告警

solvePnP 返回一组 $rvec,tvec$ 后，下游若直接当「真值」塞进融合或抓取，系统会在退化配置上安静地给出错误自信。现场常见的不是算法完全失败，而是位姿看起来合理、某一自由度其实不可观——平面靶、远距离小视场、点分布呈细长条时尤其如此。需要的不是更小的重投影误差，而是可告警的协方差与条件数。

主线：固定噪声与扰动约定 → 推导含内参的重投影雅可比 → 用谱与覆盖率验收 → 在不可观方向上拒绝硬约束，并把手眼 / 融合接口一次说清。

1. 测量模型与扰动约定

已知三维点 ${}^W{X}_i$（或标定板坐标系中的点）与像素观测 $z_i\in {\mathbb{R}}^2$。相机位姿把点变到相机系。本文采用「先平移到相机中心、再旋到相机系」的常见形式：

OpenCV 的 solvePnP 输出的是把世界点变到相机系的 $R,t$，满足 ${}^{C}X=R,{}^{W}X+t$。实现时必须与所选库的约定对齐，否则雅可比符号会系统性反号。针孔投影

${\delta }_i$ 为像素噪声，协方差 ${\Sigma }_i\in {\mathbb{R}}^{2\times 2}$。位姿参数 $\xi \in {\mathbb{R}}^6$ 取 $SE(3)$ 李代数扰动，残差

最小二乘

后文协方差都是在 $\hat{\xi }$ 处线性化得到的局部近似，不是全局后验的魔法真值。

1.1 左扰动与右扰动

同一位姿，扰动乘在左边或右边，雅可比差一个伴随变换，混用会让信息矩阵「看起来很漂亮但方向是错的」。

右扰动（body / local）：$T\leftarrow T\exp ({\xi }^{\wedge })$，$\xi$ 在相机或体坐标系下解释。对相机系点 ${}^{C}X$，右乘扰动对应

左扰动（world / global）：$T\leftarrow \exp ({\xi }^{\wedge })T$，$\xi$ 在世界系解释。二者关系

协方差亦按 ${\Sigma }_{\text{left}}=\mathrm{Ad}T\Sigma \text{right}{\mathrm{Ad}}_T^{\mathsf{T}}$ 变换。与 IMU / 位姿图对接时，先统一扰动侧再传 $\Sigma$；手眼标定若在板坐标系参数化，更要在接口文档写死「右乘于相机系」还是「左乘于世界系」。

OpenCV 的 rvec 是轴角，小角度下可当作右扰动旋转向量的一阶近似，但精炼迭代里应用 $\exp$ / $\mathrm{Log}$ 更新，而不是直接把 $\Delta rvec$ 当欧氏向量无限加。

2. 完整雅可比：从三维点到像素（含内参）

记相机系点 $X=(X,Y,Z{)}^{\mathsf{T}}$，归一化平面 $u=X/Z$，$v=Y/Z$，像素 $p=(f_{x}u+c_x,,f_{y}v+c_y{)}^{\mathsf{T}}$。

2.1 投影对相机系点的导数

畸变模型若在归一化平面上施加 $d(u,v)$，则在 $\partial \pi /\partial X$ 左侧再乘 $\partial p/\partial (u^{\prime },v^{\prime })$ 与 $\partial d/\partial (u,v)$。下面先给无畸变针孔；有畸变时把该链式乘进去即可，不要在像素域「事后乘个缩放」糊弄。

2.2 位姿扰动下的点增量

右扰动下

于是单点残差对位姿的雅可比（注意残差定义为 $z-\pi$，故带负号）

堆叠所有内点得 $J\in {\mathbb{R}}^{2n\times 6}$。

2.3 内参作为「已知但有不确定度」时

若 $f_x,f_y,c_x,c_y$ 来自标定，带协方差 ${\Sigma }_K$，可把内参拼进增广参数 $\theta =(\xi ,k)$，或做一阶边缘化。对 $k=(f_x,f_y,c_x,c_y)$：

完整残差微分

在 $\mathrm{d}k\sim \mathcal{N}(0,{\Sigma }_K)$、$\mathrm{d}z\sim \mathcal{N}(0,\Sigma )$ 且与 $\xi$ 独立时，位姿信息矩阵的 schur / 一阶传播形式为

标定很紧时 ${\Sigma }_K\approx 0$，退化为常用的 $J^{\mathsf{T}}{\Sigma }^{-1}J$；标定松或跨相机复用内参时，忽略 ${\Sigma }_K$ 会系统性低估深度方向方差。

2.4 信息矩阵与伪逆

高斯-牛顿近似下

对满秩取逆，对秩亏取伪逆并显式标出零空间。对角线 ${\Sigma }_{\xi ,kk}$ 是边缘方差近似，但告警不能只看对角线——强相关时单个标准差会掩盖不可观组合。更稳的是看特征值 ${\lambda }_1\le \cdots \le {\lambda }_6$：

• 条件数 $\kappa ={\lambda }_6/{\lambda }_1$ 过大 → 数值病态；
• ${\lambda }_{\min }$ 低于阈值 → 存在弱可观方向；
• 对应特征向量给出「哪一种位姿扰动几乎不改变重投影」。

把特征向量按扰动约定映回物理语义（沿光轴平移、绕视轴旋转、沿点云长轴滑动等），告警文案才对集成者有用。

3. 数值微分交叉检查

解析雅可比写错时，协方差会漂亮地错。对右扰动用群上的中心差分：

要求 $|J-J_{\text{num}}{|}_F/|J{|}_F$ 低于约定阈值（例如 $10^{-5}$ 量级，视尺度而定）。旋转与平移量纲不同：${ϵ}_{\text{rot}}$ 取 $10^{-6}\sim 10^{-4},\mathrm{rad}$，${ϵ}_{\text{trans}}$ 取相对场景尺度的小量。把该检查留在 debug 构建或定期 CI，能避免「协方差模块」成为摆设。

左扰动实现则应对 $T$ 左乘 $\exp (\pm ϵe_k^{\wedge })$；若数值检查用右乘、解析用左乘，相对误差会稳定地达不到阈值——这是约定 bug，不是步长问题。

4. OpenCV 实作路径

solvePnP / solvePnPRansac 默认不给完整 $6\times 6$ 协方差。calibrateCamera 返回的标准差是标定问题的，不能直接抄到单帧 PnP。推荐流水线：

1. RANSAC（或 solvePnPRansac）拿内点；
2. 对内点用迭代法精炼（SOLVEPNP_ITERATIVE，或自己做 GN/LM）；
3. 在最优 $R,t$ 处按上文重算 $J$，用 Huber 权重构造 $\Lambda$；
4. 输出 ${\Sigma }_{\xi }$、${\lambda }_{\min }$、弱方向标签、内点分布指标。

import cv2
import numpy as np

def skew(v):
    x, y, z = v
    return np.array([[0, -z, y], [z, 0, -x], [-y, x, 0]], float)

def project_jacobian_point(Xc, fx, fy):
    X, Y, Z = Xc
    return np.array([
        [fx / Z, 0.0, -fx * X / (Z * Z)],
        [0.0, fy / Z, -fy * Y / (Z * Z)],
    ], float)

def pnp_pose_covariance(object_pts, image_pts, K, dist, sigma_px=1.0):
    ok, rvec, tvec, inliers = cv2.solvePnPRansac(
        object_pts, image_pts, K, dist,
        flags=cv2.SOLVEPNP_EPNP,
    )
    if not ok or inliers is None or len(inliers) < 6:
        raise RuntimeError("insufficient inliers")
    inl = inliers.reshape(-1)
    obj_i, img_i = object_pts[inl], image_pts[inl]
    ok, rvec, tvec = cv2.solvePnP(
        obj_i, img_i, K, dist, rvec, tvec,
        useExtrinsicGuess=True,
        flags=cv2.SOLVEPNP_ITERATIVE,
    )
    R, _ = cv2.Rodrigues(rvec)
    t = tvec.reshape(3)
    fx, fy = K[0, 0], K[1, 1]
    Js, Ws = [], []
    for Xw, z in zip(obj_i, img_i):
        Xc = R @ Xw.reshape(3) + t
        # 右扰动：dXc = [-[Xc]_x | I] dxi
        G = np.hstack([-skew(Xc), np.eye(3)])
        J = -project_jacobian_point(Xc, fx, fy) @ G  # 2x6
        # Huber：重投影大则降权（此处略去残差计算细节）
        W = np.eye(2) / (sigma_px ** 2)
        Js.append(J)
        Ws.append(W)
    J = np.vstack(Js)
    W = np.block([[Ws[i] if i == j else np.zeros((2, 2))
                   for j in range(len(Ws))] for i in range(len(Ws))])
    # 更稳：逐点 J_i.T @ W_i @ J_i 累加，避免巨型 block
    Lambda = np.zeros((6, 6))
    for Ji, Wi in zip(Js, Ws):
        Lambda += Ji.T @ Wi @ Ji
    # 伪逆：对弱方向保持可报警
    evals, evecs = np.linalg.eigh(Lambda)
    return rvec, tvec, Lambda, evals, evecs, inl

注意 RANSAC 的离散选择不在高斯近似里。内点集跳变时，协方差应视为「条件于当前内点」。因此要同时上报内点数、深度范围、图像平面上的点协方差椭圆面积、共面残差（点到拟合平面距离的 RMS）。

5. 共面点、IPPE 与特解

平面靶是工业视觉的日常，也是 6DoF 协方差最容易撒谎的地方。

几何事实。 共面点的绝对位姿与单应之间存在混叠；在仅有针孔投影约束、无额外尺度/平面先验时，信息矩阵 $\Lambda$ 接近奇异或呈现一对接近的局部极小（位姿歧义）。SOLVEPNP_IPPE / SOLVEPNP_IPPE_SQUARE 正是针对平面（或正方形）目标给出至多两个解，并按重投影排序——这比强行跑 EPnP 再假装满秩 6DoF 更诚实。

实践策略：

• 平面靶做手眼或近距离抓取时，把平面约束写进状态（估 3DoF 平面内位姿 + 已知板平面，或显式估单应再分解），而不是输出「伪满秩」${\Sigma }_{\xi }\in {\mathbb{R}}^{6\times 6}$；
• 若必须输出 6DoF，用 ${\lambda }_{\min }$ 与特征向量标记「出平面平移 / 绕板轴旋转」等弱方向，并把这些方向的信息值夹到接近 0 再送给融合；
• 双解都过重投影阈时，不要只取第一名：检查与上一帧 / IMU 先验的马氏距离，或要求二次观测（相机运动、第二相机）消歧。

窄视场 + 远距离。 点在图像上挤成一团，深度与焦距方向平移难分；$\lambda$ 对应「沿光轴平移」的分量往往最先塌掉，而重投影误差仍可能 $<1,\mathrm{px}$。

细长分布。 特征沿一条线或窄带分布，绕该轴旋转或沿长轴平移弱可观。线扫、长条码、边缘上的稀疏角点都常见。

对这三类，固定像素阈值的「重投影均值 < 1px」完全不够。必须看 $\Lambda$ 的谱，或看蒙特卡洛里位姿误差的各向异性。

6. 蒙特卡洛设计：校准线性化，而不是再画一条误差曲线

一阶近似是否可信，用仿真校准。完整设计应固定因子、切片报告，而不是只报一个全局覆盖率。

6.1 因子与重复

1. 真值位姿 ${\xi }^{\text{gt}}$：覆盖近距大视场、远距窄视场、掠射角；
2. 三维点布局：立体靶 / 共面格点 / 细长条；
3. 内参：标称 $K$；可选扰动 $\mathrm{d}k\sim \mathcal{N}(0,{\Sigma }_K)$；
4. 像素噪声：各向同性 $\sigma$、各向异性、含偶尔外点（再跑 RANSAC）；
5. 每次试验：注入噪声 → PnP + 精炼 → 估 ${\Sigma }_{\xi }$ → 算误差 $e=\xi \ominus {\xi }^{\text{gt}}$（右扰动对数）；
6. 重复 $N\ge 1000$（退化切片可更高）。

6.2 对照量

• 样本协方差 $S=\frac{1}{N-1}\sum e^{(n)}{e}^{(n)\mathsf{T}}$ 与平均预测 ${\bar{\Sigma }}_{\xi }$；
• 马氏距离 $d^2=e^{\mathsf{T}}{\Sigma }_{\xi }^{-1}e$ 的经验分布，对照 ${\chi }_6^2$ 的 $95$ 分位；
• 按特征向量投影：在可观子空间算降维 ${\chi }^2$，避免被不可观分量污染；
• 报告 ${\lambda }_{\min }$ 分位数与「任务拒绝率」随布局的变化。

# 覆盖率（可观子空间）
def coverage_rate(errors, covs, mask_eigvec, q=0.95):
    # mask_eigvec: 丢掉弱方向后的投影基 P (6xk)
    d2 = []
    for e, C in zip(errors, covs):
        P = mask_eigvec  # 由 Lambda 的特征向量构造
        e_s = P.T @ e
        C_s = P.T @ C @ P
        d2.append(float(e_s.T @ np.linalg.inv(C_s) @ e_s))
    thr = chi2.ppf(q, df=mask_eigvec.shape[1])
    return np.mean(np.asarray(d2) < thr)

覆盖率长期显著低于名义 $95$，说明噪声模型偏乐观或线性化失效（大噪声、强畸变未建模、RANSAC 内点抖动）；显著高于则可能过于保守。按深度、视场角、共面 RMS 切片统计，比一个全局数字有用。

人为把 ${\Sigma }_i$ 缩小一档，门控应变紧；放大一档应变松——这是对「模块是否真用了协方差」的廉价回归。

7. 下游接口：融合、手眼、抓取

7.1 融合（滤波 / 因子图）

视觉观测噪声 $R$ 不要塞各向同性 ${\sigma }^2{I}_6$。把 ${\Sigma }_{\xi }$ 按统一扰动侧变换到滤波状态定义后接入；对 ${\lambda }_{\min }$ 过小的方向，用信息形式把对应特征值夹到 ${ϵ}_{\text{info}}\approx 0$，让 IMU / 里程计主导。位姿图边同理：高分匹配若无协方差，等于默认极紧约束，回环一次就能打爆图优化。

7.2 手眼与标定链

手眼把 ${,}^E{T}_C$ 或 ${,}^B{T}_T$ 与每次 PnP 的 ${,}^C{T}_T$ 链在一起。误差传播时：

其中 $J_j$ 是链式乘法对第 $j$ 段位姿的雅可比（同样要左右扰动一致）。平面靶 PnP 的弱方向会沿链式「污染」手眼解；验收时应在弱方向注入扰动，确认手眼残差或抓取误差的敏感度与预测同阶。

7.3 抓取 / 对接门控

对任务敏感轴（深度、偏航、绕工具轴旋转）单独设方差上限 ${\sigma }_{k,\max }^2$。超限则拒绝本次 PnP，要求靠近、换角度或改立体靶。阈值来自任务误差预算反推，而不是「条件数 > 1e6」这种万能数：

${\kappa }_{\text{gate}}$ 取 $2\sim 3$ 一类工程余量即可，关键是预算来自装配公差或抓取成功率实验，而不是拍脑袋。

8. 验收清单

• 解析 / 数值雅可比在左、右扰动各自约定下一致；
• 良态立体点云上，$95$ 覆盖率接近名义值；
• 共面 / IPPE 双解 / 远距离 / 细长分布切片上，弱方向被检出且任务拒绝率上升；
• 纳入 ${\Sigma }_K$ 后，深度方差不低于「仅像素噪声」预测；
• 人为缩小像素 $\Sigma$ 时门控变紧，加大则变松；
• 内点不足或分布指标恶化时，即使重投影小也不得输出「高置信」；
• 融合侧抽检：夹掉弱方向信息后，滤波器不再被视觉瞎拽。

9. 畸变链与稳健核：协方差里常被丢掉的两项

9.1 径向—切向畸变的链式

多数工业镜头在归一化平面上施加畸变。记无畸变归一化坐标 $(u,v)=(X/Z,Y/Z)$，畸变后 $(u^{\prime },v^{\prime })=d(u,v;,k_1,k_2,p_1,p_2,\dots )$，再乘内参。完整投影雅可比是

若精炼时用了 projectPoints 的畸变模型，协方差却按「无畸变针孔」算 $J$，远端点的 $\partial \pi /\partial X$ 会偏掉，弱方向特征向量也会偏。务实做法：与 OpenCV 同一套 projectPoints 做数值差分核验；解析式至少覆盖 $k_1,k_2,p_1,p_2$。畸变参数本身若也来自标定，可像内参一样以 ${\Sigma }_{dist}$ 进入 $\Sigma +J_d{\Sigma }_{dist}{J}_d^{\mathsf{T}}$。

9.2 Huber / Cauchy 核下的信息矩阵

外点未剔净时，用核函数 $\rho (|r{|}_{{\Sigma }^{-1}})$ 比直接 $|r{|}^2$ 稳。一阶近似下常用加权形式

其中 $w_i={\rho }^{\prime }(s_i)/s_i$（$s_i$ 为马氏残差）。Huber 在阈值内 $w=1$，阈值外 $w\propto 1/s$。要点：

• 权重随当前残差变，$\Lambda$ 是「条件于当前内点与权重」的局部信息；
• 外点权重大幅下降时，有效约束变少，${\lambda }_{\min }$ 应变差——若你的实现外点再多 ${\lambda }_{\min }$ 也不动，多半是权重没进 $\Lambda$；
• RANSAC 之后再对内点做一次 Huber 精炼，比「RANSAC 完直接用等权 GN」更接近实图。

不要对核函数再发明一套「经验放大系数」去凑覆盖率；先把 $w_i$ 做对，再用蒙特卡洛校准 ${\Sigma }_i$。

10. IPPE 双解与协方差的正确姿势

平面点 PnP 的目标函数常有两个局部极小。IPPE 给出解析候选 $T_1,T_2$，再按重投影排序。协方差是在选定的那个极小处线性化得到的，它不描述「落到另一个极小」的概率。

因此平面场景要同时输出：

1. 主解 $\hat{T}$ 与 ${\Sigma }_{\xi }$；
2. 次解是否存在、两次解的重投影差 $\Delta e$；
3. 两解在位姿空间的间隔 $|\mathrm{Log}(T_1^{-1}{T}_2)|$。

若 $\Delta e$ 很小而间隔很大，说明歧义未被观测消掉：即使主解的 ${\lambda }_{\min }$ 看起来还行，也应对任务敏感轴拒绝，或要求帧间先验。把次解也纳入门控，比单纯收紧 ${\sigma }_{px}$ 更有效——收紧噪声只能让两个错解都「更自信」。

正方形标记（SOLVEPNP_IPPE_SQUARE）同理，还多了尺寸已知带来的尺度锚定；尺度锚定改善的是平移模长可观性，并不自动修复掠射角下的旋转歧义。

11. 与手眼、多相机外参的接口约定

11.1 单次 PnP 观测消息

建议在自定义消息或诊断里固定字段，避免融合侧猜：

• pose：$T$（注明 parent/child 坐标系）；
• covariance[36]：行主序 $6\times 6$，并注明右扰动于 child（相机）系或你们选的约定；
• eigenvalues[6] / weak_axis_label；
• inlier_count、depth_span、planar_rms、ippe_second_reproj；
• status: OK / WEAK / AMBIGUOUS / REJECT。

融合节点应先读 status，再决定是否把 covariance 塞进更新。REJECT 时不要用「巨大协方差」软进滤波——巨大 $R$ 在错误相关结构下仍可能拉偏。

11.2 手眼链上的一阶传播

以眼在手上、板在固定座标为例，一次观测：

对其中一段 ${}^C{T}_T$ 施右扰动 $\xi$，链式位姿的一阶误差为 $J_{\text{chain}}\xi$，于是

（其余段同理）。实现时用伴随把各段 $\Sigma$ 变到同一代数坐标再加，比「把平移方差当欧氏、旋转当度」拼对角阵可靠。验收：在仿真里只放大 PnP 弱方向方差，手眼残差椭球应沿预测方向变长。

11.3 双目 / 多相机

双目相当于把两套投影残差堆进同一个 $\xi$（外参已知时）。信息矩阵

共面单目下的弱方向常被第二台相机抬起来，但基线与板法向接近平行时仍会留下退化。协方差模块对多相机应复用同一扰动约定与同一套谱告警，而不是「双目就直接信任」。

12. 蒙特卡洛切片表（建议固定进 CI）

每条切片报告：平均重投影、$\mathrm{median}({\lambda }_{\min })$、$95$ 覆盖率（全空间与可观子空间）、任务门控拒绝率。CI 对 A 的覆盖率设区间，对 C/D 的拒绝率设下限——防止有人为了「好看」去掉谱门控。

13. 数值实现细节：病态、缩放与伪逆

$\Lambda \in {\mathbb{R}}^{6\times 6}$ 的条件数常被平移/旋转量纲放大：平移以米计、旋转以弧度计时，特征值本身不可比。两件事要分开做：

1. 物理可观性：用特征向量看方向，用任务轴上的边缘方差 ${\sigma }_k$ 做门控；
2. 数值稳定：组装 $\Lambda$ 前对参数做缩放，例如令 ${\xi }^{\prime }=S\xi$，$S=\mathrm{diag}(s_r,s_r,s_r,s_t,s_t,s_t)$，在缩放空间求逆再变回。

伪逆阈值不要写成绝对 $10^{-12}$ 了事。更稳的是相对阈值：把小于 $\tau {\lambda }_{\max }$ 的特征值视为零空间，$\tau$ 取 $10^{-6}\sim 10^{-4}$ 并结合蒙特卡洛切片标定。被剪掉的特征方向必须写进 weak_axis_label，否则伪逆会静默丢掉不可观方向上的方差（伪逆在零空间给出 0 方差——那是最危险的「过度自信」）。

正确做法是信息形式输出：对外发布 $\Lambda$（或夹紧后的 ${\Lambda }_{ϵ}$），由融合侧去取逆；若必须发布 $\Sigma$，对弱方向赋很大的方差而不是 0。

def covariance_from_information(Lambda, rel_eps=1e-5, weak_var=1e6):
    evals, evecs = np.linalg.eigh(Lambda)
    max_e = evals.max()
    thr = rel_eps * max_e
    inv = np.zeros_like(evals)
    weak = evals < thr
    inv[~weak] = 1.0 / evals[~weak]
    inv[weak] = weak_var
    Sigma = (evecs * inv) @ evecs.T
    return Sigma, weak, evals, evecs

13.1 与 projectPoints 对齐的差分核验

除了对位姿差分，也对内参加差分，防止 $J_k$ 写错：

def numerical_Jk(T, Xw, K, dist, eps=1e-3):
    # 对 fx,fy,cx,cy 中心差分 projectPoints
    base, _ = cv2.projectPoints(Xw[None], *T, K, dist)
    cols = []
    for idx, e in enumerate(np.eye(4)):
        Kp = K.copy(); Km = K.copy()
        # 映射 e -> K 元素略
        ...
    return np.column_stack(cols)

解析 $J_{\xi },J_k$ 与数值版本一起进 CI；改投影模型（加 $k_3$、加薄棱镜）时先修雅可比，再谈覆盖率。

14. 从任务预算反推门控：一个可算的例子

假设对接允许沿光轴误差 $e_z\le 3,\mathrm{mm}$（$3\sigma$ 意义），则门控

若蒙特卡洛在「远距离平面码」切片上给出 $\mathrm{median}({\sigma }_z)=4,\mathrm{mm}$，那不是把阈值放宽到 $4,\mathrm{mm}$，而是：该距离下禁止硬对接，或要求靠近到 ${\sigma }_z$ 降到预算内。横向同理。把预算表贴进运维文档，现场才知道「拒识别」是在保护机构，而不是视觉偶发抽风。

对旋转轴用装配角隙预算同样反推。切记：${\sigma }_k$ 来自你选择的扰动约定；门控比较前确认融合与视觉用的是同一组轴。

15. 常见失败模式速查

16. 精炼算法与协方差是否匹配

SOLVEPNP_ITERATIVE 本质是重投影最小二乘，与 $J^{\mathsf{T}}{\Sigma }^{-1}J$ 同族，协方差口径一致。EPNP / SQPNP 等闭式解提供初值很合适，但若最终位姿停在闭式解、只在该处拼一个「形式协方差」，会与真正最小化的目标不一致——尤其外点权或畸变与闭式假设不符时。

建议固定流程：闭式/RANSAC 初值 → 与协方差同模型的迭代精炼 → 在精炼点算 $\Lambda$。LM 阻尼在病态时有帮助，但阻尼项会改变正规方程；对外报告协方差时应用阻尼前的 $J^{\mathsf{T}}WJ$（或明确报告「带阻尼的数值 Hessian」并在蒙特卡洛里用同一套）。不要把 LM 的 $(J^{\mathsf{T}}WJ+\lambda D{)}^{-1}$ 直接当传感器噪声传播结果。

若用自动微分（Ceres 等）求 $J$，把残差定义、扰动侧、核函数与线上一致，再导出 $\Lambda$；和手写雅可比互相对照一次，比只信 AD 更稳。

17. 时间戳与运动中的靶

以上推导默认「曝光瞬间靶静止、点对应无时间误差」。运动靶或卷帘快门下，像素噪声之外还有同步误差，一阶上可把等效像素扰动写成 $\delta z\approx J_v\Delta t$。若融合时间戳与图像时间差达到数毫秒，而你仍用静止靶 ${\Sigma }_i$，覆盖率会假性崩掉。处理方式是：收紧同步、把时间误差扩进 ${\Sigma }_i$，或在门控里对「点速度过大」直接 REJECT。协方差模块解决的是几何可观性，不是时间对齐的全部问题——但要把时间误差从「未知偏差」里拆出来，否则你会误伤几何门控阈值。

18. 收束

PnP 给的是点到像素的局部契约。协方差模块的职责，是把契约的松紧说清楚，并在失约束时大声失败。残差小可以是收敛，也可以是「沿着不可观方向随便站」。把含内参的雅可比、扰动侧、IPPE 特解、谱与覆盖率放进验收，位姿才会从「一个数」变成「一个可融合的观测」。

落地时优先完成三件事：统一左/右扰动并与融合对齐；在精炼点用与投影模型一致的 $J$ 组 $\Lambda$，弱方向用大方差或信息夹紧而不是伪逆静默归零；用切片蒙特卡洛看守覆盖率与任务拒绝率。这三件事比再调一点 RANSAC 阈值更能决定系统在退化配置下是否诚实。

上次在平面码对接上栽跟头，不是 solvePnP 没收敛，而是沿光轴的 $\sigma$ 被当成了与横向同级；门控加上之后，拒绝率上升，但误抓归零——这才是协方差该买到的东西。