Part III · 第 11 章 多视图几何 · 11.4

PnP 位姿估计

机器人常常"知道一些点在哪里，却不知道相机在哪里"：零件的 CAD 模型，贴在地面上的 AprilTag，SLAM 建好的地图。PnP（Perspective-n-Point）用这些已知的三维点和它们在图像里的位置，直接求出相机相对物体的位姿。本节从"一个像素给两个方程、位姿有六个未知数"的计数出发，看 P3P 的四个解；用 DLT 得到线性初值，再在 SE(3) 上用 Gauss–Newton / LM 精修（含解析雅可比）；用协方差回答"这个位姿有多可信"；看平面标签特有的翻转歧义；最后用 RANSAC 对付误匹配。

约 35 分钟 前置：1.12 SVD · 5.3 SE(3) · 5.6 exp/log · 5.7 扰动 · 6.6 Gauss–Newton/LM · 6.10 流形优化 · 10.3 内外参 · 11.3 三角化 6 个交互

    1为什么机器人需要它

    三角化（11.3 节）回答"相机已知，点在哪"。机器人更常遇到相反的问题：点已知，相机在哪？一旦几个点的三维位置是已知的（来自 CAD 模型、一张地图、一块标定板），一张图像就足以给出相机相对它们的完整六自由度位姿：

    • 相对地图定位：视觉重定位和视觉里程计的跟踪线程，把当前帧的特征匹配到地图里的三维点，再用 PnP 求出机器人在地图中的位姿。
    • 物体位姿：模型已知，网络预测出关键点的像素位置（PVNet、DOPE 一类），PnP 把它们变成物体相对相机的位姿，抓取规划拿去用。
    • 标记与标定板：AprilTag、ArUco、棋盘格的四个角点或格点是已知平面上的点，一次 PnP 就给出标记位姿，用于机器人定位、视觉伺服和手眼标定（10.4 节）。

    它与前后几节的关系可以一句话记住：三角化是"2D–2D ⇒ 3D"，PnP 是"3D–2D ⇒ 位姿"，光束法平差（11.6 节）则让点和位姿同时未知。本节的算法分四层：最小解（P3P，3 个点）、线性解（DLT，≥6 个点）、非线性精修（SE(3) 上的 Gauss–Newton / LM）、鲁棒外壳（RANSAC，11.5 节）。每一层都在页面里真实运行。

    2直觉：像素给方向，模型给距离

    先约定坐标系。已知点写在物体系 {o} 里（如果这些点是一张地图，{o} 就是世界系 {w}）；相机系 {c} 是 OpenCV 光学系：x 向右，y 向下，z 向前，像素 \((u,v)\) 的 \(v\) 向下。PnP 求的是 \({}^{c}T_{o}=(R,\mathbf t)\)，它把物体系里的点变到相机系：

    $$ \mathbf x_c=R\,\mathbf x_o+\mathbf t,\qquad \lambda\,\tilde{\mathbf u}=K\,\mathbf x_c . $$

    这正是 OpenCV 里的"外参" \([R\,|\,\mathbf t]\)（10.3 节）。相机自己在物体系里的位姿是它的逆 \({}^{o}T_{c}=({}^{c}T_{o})^{-1}\)，相机中心 \(\mathbf C=-R^{\top}\mathbf t\)。本节的 3D 图都画在物体系里，所以会动的是相机。

    一个像素对应一条射线，只告诉你点在哪个方向，不告诉你有多远。所以每个对应点给 2 个方程，而位姿有 6 个未知数（3 个旋转、3 个平移）：一个点远远不够，两个点还留下 2 维的解族，三个点恰好 \(2\times3=6\) 个方程对 6 个未知数。几何上是这样的：三个像素给出三条射线，模型告诉我们三个点两两之间的距离，于是问题变成"把一个已知形状的刚性三角形的三个顶点分别放在三条射线上，有哪些摆法？"这就是 P3P。三个距离约束是关于三个深度的三个二次方程（余弦定理，见"数学"一节），它们最多有 4 个深度全为正的公共解。

    FIG 1 把这些解全部算出来。每一台"幽灵相机"都把选中的三个点精确投影到你给的像素上（误差不超过约 \(10^{-8}\) 像素，通常小得多），它们只在其余的点上不同意。第四个点一出现，通常只剩一台相机对得上。

    FIG 1三个点够不够？P3P 的全部解
    物体系中的相机，拖动旋转
    图像（640×480 像素，v 向下）
    相机始终看向盒子中心，滑块改变它在物体系里的方位角、俯仰角和距离。琥珀色的三个角点是 P3P 的输入（它们的像素加上盒子的尺寸 0.4×0.3×0.2 m），绿色相机是真值，其它颜色是 P3P 算出的另外的解，每个解的相机中心到三个点之间的细线都画了出来。右图是相机拍到的图像：每个解把整个盒子重投影成自己颜色的线框，在三个选中的点处所有线框完全重合。打开"第 4 个点"后，青色点是观测到的第 4 个角点，每个解预测它的位置与观测的距离写在读数里（红线是那段残差）。
    试一试
    • 转动方位角（或按"找 4 解视角"），让 P3P 给出 4 个解。解的个数通常是 2 或 4；换一组三个点，会在不同的方位出现 4 个解。
    • 在 3 个以上解的视角上打开"第 4 个点"：看读数，只有一个解把第 4 个点也投影到 0 像素误差，其余几个差了几十到几百像素。
    核心洞见

    PnP 的本质是"一幅图像 + 一个已知的刚体 = 一个位姿"。像素只提供方向，模型的几何提供距离，缺一不可。每个对应点是 2 个方程；6 个未知数意味着最少 3 个点（可能有最多 4 个解），4 个点起一般唯一，更多的点用来对抗噪声，所以实际用的是最小二乘。

    3动手玩：先线性猜一个，再在流形上打磨

    线性初值：DLT

    有 \(n\) 个点时最直接的办法是把问题变成线性的。把投影写成 \(\lambda\tilde{\mathbf u}=P\tilde{\mathbf X}\)，\(P\) 是 \(3\times4\) 矩阵。用叉积消掉 \(\lambda\)：\(\tilde{\mathbf u}\times P\tilde{\mathbf X}=\mathbf 0\)，每个点就给出关于 \(P\) 的 12 个元素的 2 个线性方程。堆成 \(A\mathbf p=\mathbf 0\)，解是 \(A\) 的最小奇异值对应的右奇异向量（1.12 节），这就是 DLT（与 10.5 节的单应 DLT 同一套路）。\(P\) 有 11 个自由度，所以至少要 6 个点（"5.5 个"）。K 已知时，\(K^{-1}P\approx\lambda[R\,|\,\mathbf t]\)：用 SVD 把左边 \(3\times3\) 块投影到最近的旋转矩阵 \(R=UV^{\top}\)，符号由 \(\det R=+1\)（等价于点在相机前方）定。

    DLT 有两个典型毛病：它最小化的是代数误差而不是像素误差，噪声下不准；而且点共面时 \(A\) 秩亏，答案不唯一。FIG 2 让你同时看到这两点，并用平面目标的专用解法（单应）补救。

    FIG 2线性 PnP：DLT 与平面目标
    图像：观测 vs 估计的重投影
    旋转误差 vs 点数（24 次噪声的中位数）
    DLT 矩阵 A 的 12 个奇异值（对数，相对最大值）
    琥珀色点是带噪声的观测像素，空心绿圈是无噪声的真值，紫色叉是估计位姿的重投影，红线是残差。右上图是蒙特卡洛：在当前噪声下对每个点数重复 24 次，实线是 DLT（或单应）的旋转误差中位数，虚线是再用 8 步 LM 精修之后（见下一个图）。底部的条形图是 DLT 矩阵 \(A\)（\(2n\times12\)，已做 Hartley 归一化）的奇异值：一般位置的点只有最后一个接近 0，对应唯一的解；共面的点会多出 3 个恰好为 0 的奇异值（红色），无噪声时整个 4 维零空间里的任何 \(P\) 都同样"完美"地拟合数据。真值位姿：旋转约 124°，距离 1.2 m，\(f=600\) 像素。
    试一试
    • 选"共面目标 + 3D DLT"：旋转误差跳到几十度甚至上百度，底部多出 3 根红色的奇异值条掉到底。点数再多也没用。为什么？
    • 保持共面目标，把方法切到"单应"，噪声不小于 1 像素：误差回到几度以内，而且无论点多还是少都正常（4 个点就够）。

    精修：在 SE(3) 上最小化重投影误差

    DLT 只是个初值。我们真正想要的是使重投影误差 \(E(T)=\tfrac12\sum_i\|\pi(K(R\mathbf X_i+\mathbf t))-\mathbf u_i\|^2\) 最小的位姿（像素噪声是高斯时，这就是最大似然，4.6 节）。位姿住在 SE(3) 上，所以每一步在流形上走（6.10 节）：求一个小扭转 \(\delta\boldsymbol\xi=(\delta\boldsymbol\omega,\delta\mathbf v)\)（角速度在前，和全课一致），然后 \(T\leftarrow\operatorname{Exp}(\delta\boldsymbol\xi)\,T\)。这是左扰动（5.7 节），它的雅可比特别干净：\(\delta\mathbf x_c=\delta\boldsymbol\omega\times\mathbf x_c+\delta\mathbf v=[-[\mathbf x_c]_\times\ \ I]\,\delta\boldsymbol\xi\)，再乘投影的 \(2\times3\) 雅可比（3.3 节）即可。

    FIG 3 从一个初值出发迭代，你可以拖动迭代滑块看每一步：相机位姿（3D）、重投影的残差（图像）、代价（曲线）。初值有两种：DLT 的结果，或者把真值转了一个很大的角度。Gauss–Newton（直接解 \(J^{\top}J\,\delta=-J^{\top}\mathbf r\)）从好初值几步就收敛；初值很差时它会一步跳进错误的盆地（停在约 180° 的局部极小）甚至发散，Levenberg–Marquardt 的阻尼项（6.6 节）让每一步都下降。

    FIG 3在 SE(3) 上精修：GN 与 LM
    相机位姿轨迹（物体系），拖动旋转
    图像：重投影与残差
    代价 E 随迭代（对数）
    3D 图里绿色相机是真值，琥珀色相机是第 \(k\) 步的估计，小点和折线是此前各步的相机中心。图像里琥珀点是观测，紫色叉是第 \(k\) 步的重投影，红线是残差 \(\mathbf r_i\)（悬停公式里的 \(\mathbf r\) 会加粗）。曲线是两种方法的代价（虚线 GN，实线 LM），竖线是当前的 \(k\)。读数最后一行是核对：解析雅可比与对 \(\operatorname{Exp}(\pm h\mathbf e_j)\,T\) 做中心差分得到的雅可比之差（相对 \(\|J\|_{\max}\)）。14 个点，噪声 1 像素，随机数固定。
    试一试
    • 选"偏移初值"，把偏移角调到 130° 到 145° 之间，方法选 GN，把迭代滑到底：GN 的代价先乱跳，最后停在一个很大的值（旋转误差约 180° 的局部极小），回不到最优。
    • 同样的偏移角（≥ 125°）切到 LM，滑到底：代价单调下降，最后旋转误差不到 1°。LM 多花了几步，但每一步都是安全的。

    4第二、第三视角：这个位姿有多可信？

    协方差：小而远的目标，位姿不确定

    重投影误差降到 0.3 像素，并不说明位姿准到 0.3 像素对应的量。精修收敛后，估计的不确定性可以从同一个雅可比读出来（4.5 节的不确定性传播）：像素噪声为 \(\sigma\)，则扰动坐标 \(\delta\boldsymbol\xi\) 的协方差是

    $$ \Sigma_{\xi}\;\approx\;\big(J^{\top}\Sigma_u^{-1}J\big)^{-1}=\sigma^{2}\big(J^{\top}J\big)^{-1}\quad(\Sigma_u=\sigma^{2}I). $$

    相机中心 \(\mathbf C=-R^{\top}\mathbf t\)，一阶下 \(\delta\mathbf C=-R^{\top}\delta\mathbf v\)，所以相机位置的协方差是 \(R^{\top}\Sigma_{vv}R\)（\(\Sigma_{vv}\) 是 \(\Sigma_\xi\) 右下 \(3\times3\) 块）。FIG 4 把它画成以相机为中心的椭球。目标越小、越远、点越少，椭球越大，其尺度规律是 \(\sigma_{\text{深度}}\propto \sigma Z^{2}/(f\,s\sqrt n)\)（\(Z\) 为距离，\(s\) 为目标的半径，\(n\) 为点数）。

    FIG 4位姿协方差 \((J^{\top}J)^{-1}\)
    相机位置的 1σ 椭球（放大显示）
    左图：目标点（青色）在原点附近，相机（绿色）在 \(Z\) 远处看它，青色线框是相机中心的 1σ 协方差椭球，已按图中标注的倍数放大，否则看不见；紫色小点是 Monte-Carlo：200 次重新加噪声并用 LM 精修后的相机中心（同样放大）。右图的条形：上面三条是姿态的 1σ（绕相机 x、y、z 轴，单位度），下面三条是相机中心的 1σ（沿相机 x、y、z 轴，z 是光轴即深度，单位毫米）；实心条是 \((J^{\top}J)^{-1}\) 的预测，竖线是 Monte-Carlo 的经验值，两者吻合。（三维里 1σ 椭球只装得下约 20% 的样本，所以紫点比椭球散得开。）注意对小目标，横向（相机绕目标"转圈"，等价于姿态误差）和深度的不确定度是同一量级，二者都随 \(Z^{2}/(s\sqrt n)\) 增长。
    试一试
    • 把距离拉到 4 m 以上、目标半径保持很小：深度 1σ 超过 10 cm。再把点数加到 60、目标半径加大：椭球缩回毫米量级。

    平面标签的翻转歧义

    AprilTag 的四个角点是共面的，位姿只由 4 个点（8 个方程）决定。对一个小的、远的、斜着看的标签，投影近似于弱透视，而弱透视下，把平面沿视线"翻"到另一侧（法向量关于视线镜像）得到的图像几乎一样。透视效应只留下一点点差别，可能被噪声盖过。结果是重投影误差有两个局部极小值，噪声足够大时，错的那个甚至更低。FIG 5 中，先用平面单应得到初值并用 LM 精修，得到候选 1；再把它沿视线翻转作为另一个初值再精修，得到候选 2。IPPE（Collins & Bartoli）用闭式给出这两个解，AprilTag 库为两个候选各算一个误差，原因就在这里。底部曲线是"剖面"重投影误差：固定绕相机 y 轴的倾角 \(s\)，其余 5 个自由度都重新优化。

    FIG 5平面标签的两个极小值
    俯视（相机 x–z 平面，放大到标签）
    图像（放大到标签）
    剖面重投影误差 vs 倾角 s
    俯视图里绿色是真实的标签（一条线段，因为从上面看它是侧立的），琥珀色和紫色是两个候选，虚线是视线。两个候选关于视线互为"镜像"。右图里两个候选的重投影几乎重合（差零点几像素），所以单看图像无法区分。底部曲线横轴是倾角 \(s\)，纵轴是 RMS 重投影误差（像素，每个坐标）：绿色竖线是真值倾角，圆点是两个候选。按"100 次噪声"会在当前设定下重复 100 次，统计两个候选同时存在以及"重投影误差较低的那个其实是错的"的比例。标签：正方形，倾角 \(s\) 为绕相机 y 轴的倾斜。
    试一试
    • 把标签调小、拉远（比如 5 cm、2 m，噪声 1 px），按"新噪声"几次，直到读数显示"误差较低的候选是错的"（约四成到一半的噪声样本会这样；再按"100 次噪声"核对）。再把标签变大或拉近：这种情况几乎消失。
    同一个问题的三种说法

    代数：P3P 是三个二次方程，消元后是关于深度比的一个四次方程，最多 4 个正实根；平面翻转是其中的一对近似重根。几何：把刚性模型放在一束射线上；射线几乎平行（远、小）时，前后翻转几乎不改变图像。统计：最大似然估计，不确定性是 Fisher 信息 \(J^{\top}\Sigma_u^{-1}J\) 的逆；翻转歧义是后验分布有两个峰，高斯近似 \((J^{\top}J)^{-1}\) 只描述其中一个峰的宽度，对"两个峰"的问题无能为力。

    5再看数学

    提示：把鼠标悬停在公式中带底色的符号上，FIG 3 或 FIG 4 中对应的元素会被高亮。

    模型、代价与计数

    物体系中的已知点 \(\mathbf X_i\) 及其观测像素 \(\mathbf u_i\)，\(i=1..n\)；相机内参 \(K\)（\(f_x,f_y,c_x,c_y\)）已知，且图像已去畸变（10.3 节）。令 \(\mathbf x_{c,i}=R\mathbf X_i+\mathbf t\)，重投影残差和代价是

    $$ \hl{resid}{\mathbf r_i}(T)=\pi(\mathbf x_{c,i})-\mathbf u_i,\qquad \pi(\mathbf x)=\begin{bmatrix}f_x\,x/z+c_x\\ f_y\,y/z+c_y\end{bmatrix},\qquad E(T)=\tfrac12\sum_{i=1}^{n}\|\mathbf r_i(T)\|^{2},\qquad \hat T=\arg\min_{T\in\mathrm{SE}(3)}E(T). $$

    像素噪声独立同分布 \(\mathcal N(0,\sigma^2I)\) 时 \(\hat T\) 是最大似然估计（4.6 节）。下表把 PnP 放进它的几个近亲里，计数都是"每个对应点的方程数 vs 未知数"：

    问题 已知 未知（自由度） 最少需要
    三角化（11.3）相机 \(P_j\)，像素 \(\mathbf u_{ij}\)\(\mathbf X\) (3)2 个视图（4 个方程）
    P3P / PnP\(K,\ \mathbf X_i,\ \mathbf u_i\)\({}^{c}T_{o}\) (6)3 个点（至多 4 个解）；4 个点起一般唯一
    DLT（K 未知）\(\mathbf X_i,\ \mathbf u_i\)\(P\) (11)6 个点，且不共面
    平面目标 PnP\(K\)，平面上的点\(H\) (8) → \({}^{c}T_{o}\) (6)4 个点（有翻转歧义）
    光束法平差（11.6）\(K,\ \mathbf u_{ij}\)所有 \(T_j\) 和 \(\mathbf X_i\)多视图

    DLT 与"投影到 SO(3)"

    令 \(\tilde{\mathbf X}_i=(X,Y,Z,1)^{\top}\)，\(\mathbf p\in\mathbb R^{12}\) 是 \(P\) 的各行依次拼起来。\(\tilde{\mathbf u}\times P\tilde{\mathbf X}=\mathbf 0\) 的前两个独立方程是

    $$ \underbrace{\begin{bmatrix}\mathbf 0^{\top} & -\tilde{\mathbf X}_i^{\top} & v_i\tilde{\mathbf X}_i^{\top}\\ \tilde{\mathbf X}_i^{\top} & \mathbf 0^{\top} & -u_i\tilde{\mathbf X}_i^{\top}\end{bmatrix}}_{2\times12}\mathbf p=\mathbf 0,\qquad \hat{\mathbf p}=\arg\min_{\|\mathbf p\|=1}\|A\mathbf p\|=\mathbf v_{12}\ \text{(the last right singular vector of }A). $$

    点和像素都要先做 Hartley 归一化（平移到质心、缩放到平均距离 \(\sqrt2\) 或 \(\sqrt3\)），否则条件数很差。共面退化：如果所有 \(\tilde{\mathbf X}_i\) 满足 \(\boldsymbol\pi^{\top}\tilde{\mathbf X}=0\)，那么对任意 \(\mathbf a\in\mathbb R^3\)，\(P'=P+\mathbf a\boldsymbol\pi^{\top}\) 对这些点给出同样的像，于是 \(\mathbf e_k\boldsymbol\pi^{\top}\)（\(k=1,2,3\)）总是 \(A\) 的零向量，无论有无噪声；无噪声时 \(P\) 本身也是，零空间就是 \(\{P,\ \mathbf e_1\boldsymbol\pi^{\top},\ \mathbf e_2\boldsymbol\pi^{\top},\ \mathbf e_3\boldsymbol\pi^{\top}\}\) 张成的 4 维空间：FIG 2 底部掉到底的红色条。K 已知时：

    $$ K^{-1}\hat P=\big[\,M\mid\mathbf m\,\big]\approx\lambda\,[\,R\mid\mathbf t\,],\qquad M=U\Sigma V^{\top}\ \Rightarrow\ R=UV^{\top},\quad \lambda=\tfrac13\operatorname{tr}\Sigma,\quad \mathbf t=\mathbf m/\lambda . $$

    \(UV^{\top}\) 是 Frobenius 范数下离 \(M\) 最近的正交矩阵（正交 Procrustes，12.1 节的 ICP/Kabsch 用的是同一个引理）；\(\hat P\) 只确定到符号，要求 \(\det R=+1\) 就固定了符号。

    P3P：余弦定理与四次方程

    令 \(\mathbf f_i\) 是像素 \(\mathbf u_i\) 的单位视线（\(K^{-1}\tilde{\mathbf u}_i\) 归一化），\(\mathbf x_{c,i}=d_i\mathbf f_i\)，\(d_i>0\) 是未知的深度（沿视线的距离）。\(a=\|\mathbf X_2-\mathbf X_3\|\)，\(b=\|\mathbf X_1-\mathbf X_3\|\)，\(c=\|\mathbf X_1-\mathbf X_2\|\) 在物体系里已知，\(\cos\alpha=\mathbf f_2\!\cdot\!\mathbf f_3\)，\(\cos\beta=\mathbf f_1\!\cdot\!\mathbf f_3\)，\(\cos\gamma=\mathbf f_1\!\cdot\!\mathbf f_2\)。三角形的三条边在相机系里必须等于模型里的边长：

    $$ d_2^{2}+d_3^{2}-2d_2d_3\cos\alpha=a^{2},\qquad d_1^{2}+d_3^{2}-2d_1d_3\cos\beta=b^{2},\qquad d_1^{2}+d_2^{2}-2d_1d_2\cos\gamma=c^{2}. $$

    Grunert（1841）的消元：令 \(x=d_2/d_1\)，\(y=d_3/d_1\)，\(Q(x)=1+x^2-2x\cos\gamma\)，由第三式 \(d_1^2=c^2/Q(x)\)。把第一、二式用 \(d_1^2\) 消去，再把两者相减消掉 \(y^2\)，得到 \(y\) 是 \(x\) 的有理函数：

    $$ y=\frac{c^{2}(x^{2}-1)+(b^{2}-a^{2})\,Q(x)}{2c^{2}\,(x\cos\alpha-\cos\beta)},\qquad c^{2}y^{2}-2c^{2}\cos\beta\;y+c^{2}-b^{2}Q(x)=0\ \Longrightarrow\ \text{a quartic in }x. $$

    每个满足 \(x,y>0\) 的实根给出一组深度，随后 \((R,\mathbf t)\) 由两组三个点 \(\{\mathbf X_i\}\) 与 \(\{d_i\mathbf f_i\}\) 的刚体对齐得到（三点各定一个标架即可）。FIG 1 里每个解都用牛顿法在原三个方程上抛光，三点的重投影误差不超过约 \(10^{-8}\) 像素（读数里通常是 \(10^{-13}\) 左右）。现代 P3P（Gao 等 2003；Kneip 等 2011）数值更稳，思路相同。EPnP（Lepetit 等 2009）的想法：把每个点写成 4 个"控制点"的重心坐标加权和 \(\mathbf X_i=\sum_{j=1}^4\alpha_{ij}\mathbf C_j\)（权重在两个坐标系里相同），于是相机系里只有 12 个未知数（控制点的坐标），它们线性地进入投影方程，复杂度为 \(O(n)\)；零空间里的组合系数由控制点之间已知的距离定出，最后同样做刚体对齐。

    SE(3) 上的 Gauss–Newton / LM 与解析雅可比

    左扰动 \(T\leftarrow\operatorname{Exp}(\delta\boldsymbol\xi)\,T\)，\(\delta\boldsymbol\xi=(\delta\boldsymbol\omega,\delta\mathbf v)\)（角在前，与 5.5 节的 \(\boldsymbol\xi\) 同序）。一阶下 \(\operatorname{Exp}(\delta\boldsymbol\xi)\,\mathbf x_c\approx\mathbf x_c+\delta\boldsymbol\omega\times\mathbf x_c+\delta\mathbf v\)，而 \(\delta\boldsymbol\omega\times\mathbf x_c=-[\mathbf x_c]_\times\delta\boldsymbol\omega\)（1.8 节）。链式法则：

    $$ J_i=\frac{\partial\mathbf r_i}{\partial\,\delta\boldsymbol\xi}=\underbrace{\begin{bmatrix}f_x/z&0&-f_x x/z^{2}\\ 0&f_y/z&-f_y y/z^{2}\end{bmatrix}}_{\partial\pi/\partial\mathbf x_c}\begin{bmatrix}-[\mathbf x_c]_\times & I_3\end{bmatrix}\in\mathbb R^{2\times6},\qquad J=\begin{bmatrix}J_1\\ \vdots\\ J_n\end{bmatrix}\in\mathbb R^{2n\times6}. $$
    $$ \text{GN:}\ \ J^{\top}J\,\delta\boldsymbol\xi=-J^{\top}\mathbf r,\qquad \text{LM:}\ \ \big(J^{\top}J+\hl{lm}{\mu}\operatorname{diag}(J^{\top}J)\big)\delta\boldsymbol\xi=-J^{\top}\mathbf r,\qquad T\leftarrow\operatorname{Exp}(\delta\boldsymbol\xi)\,T . $$

    LM 在代价上升时把 \(\mu\) 乘大、下降时缩小（6.6 节）。如果改用右扰动 \(T\leftarrow T\operatorname{Exp}(\delta\boldsymbol\xi)\)，雅可比变成 \(\partial\pi/\partial\mathbf x_c\,[-R[\mathbf X_o]_\times\ \ R]\)，最小值点相同，但协方差表示在物体系而不是相机系，两者差一个伴随 \(\mathrm{Ad}_T\)（5.8 节）。FIG 3 读数里的雅可比核对就是对 \(\operatorname{Exp}(\pm h\mathbf e_j)\,T\) 的中心差分，和上式相差约 \(10^{-10}\)（相对 \(\|J\|_{\max}\)）。

    平面目标：从单应读出位姿

    平面取 \(Z=0\)，则 \(\lambda\tilde{\mathbf u}=K[\mathbf r_1\ \mathbf r_2\ \mathbf r_3\ \mathbf t]\,(X,Y,0,1)^{\top}=K[\mathbf r_1\ \mathbf r_2\ \mathbf t]\,(X,Y,1)^{\top}\)，即平面到图像的单应 \(H\simeq K[\mathbf r_1\ \mathbf r_2\ \mathbf t]\)（10.5 节；Zhang 标定里同一个式子）。

    $$ K^{-1}H=[\mathbf h_1\ \mathbf h_2\ \mathbf h_3],\quad \lambda=\tfrac12(\|\mathbf h_1\|+\|\mathbf h_2\|),\quad \mathbf r_1=\tfrac{\mathbf h_1}{\lambda},\ \mathbf r_2=\tfrac{\mathbf h_2}{\lambda},\ \mathbf r_3=\mathbf r_1\times\mathbf r_2,\ \ \mathbf t=\tfrac{\mathbf h_3}{\lambda}\ (t_z>0),\qquad R=\operatorname{polar}\big([\mathbf r_1\ \mathbf r_2\ \mathbf r_3]\big) . $$

    噪声使 \(\mathbf r_1,\mathbf r_2\) 不严格正交，所以最后一步用 SVD 取最近的旋转（极分解）。这是 FIG 2 中"单应"方法，以及 OpenCV 里 \(\textsf{SOLVEPNP\_ITERATIVE}\) 面对平面点时的初值。它没有解决翻转歧义：IPPE 把单应在标签中心处的局部一阶近似分解出两个旋转，对应上面的两个极小值；最终仍应以 LM 精修，再比较两者的重投影误差。

    不确定性与经验规律

    在最小值点，Fisher 信息是 \(\mathcal I=\sigma^{-2}J^{\top}J\)，Cramér–Rao 下界给出 \(\operatorname{cov}(\hat{\boldsymbol\xi})\succeq\hl{cov}{\Sigma_\xi}=\sigma^{2}(J^{\top}J)^{-1}\)，小噪声下 MLE 达到这个界（FIG 4 的 Monte-Carlo 和它吻合到 5% 左右）。设目标在距离 \(Z\) 处，点相对质心的 RMS 横向半径为 \(s\)，共 \(n\) 个点。FIG 4 的数值实验（\(s\) 从 3 cm 到 30 cm，\(Z\in[0.8,3.2]\) m，\(n\in[6,40]\)）给出沿光轴的相机位置不确定度

    $$ \sigma_{\text{depth}}\;\approx\;0.7\,\frac{\sigma\,Z^{2}}{f\,s\,\sqrt n}, $$

    常数 0.7 取决于点的布局，是数值拟合值，不是定理；它在目标比距离小（\(s/Z\lesssim0.1\)）时与 FIG 4 的精确值相差约 10%，目标更大（视场更宽）时公式偏大。\(\sigma\), \(Z^2\), \(1/f\), \(1/s\), \(1/\sqrt n\) 这些依赖是稳固的：目标的像有 \(fs/Z\) 像素宽，深度靠这个"尺度"测出，尺度的相对误差是 \(\sigma/(fs/Z)/\sqrt n\)，再乘 \(Z\)。相机的横向位置（绕目标"转圈"）同样按 \(Z^{2}/(s\sqrt n)\) 增长，量级与深度相当，因为小目标的平移与小角度转动几乎不可区分。

    与邻居的联系

    手眼标定（10.4）：每张标定板图像先做一次 PnP 得到 \({}^{c}T_{b}\)，再解 \(AX=XB\)；PnP 的误差（FIG 4）直接传进标定精度，所以要用多个姿态、尽量大的板和靠近的距离。光束法平差（11.6）：把上式里的 \(\mathbf X_i\) 也变成未知数，对多个相机同时优化；固定所有点只优化一个相机，就是本节的 PnP 精修（"仅位姿 BA"，motion-only BA）。视觉伺服（Part IV，只指路）：基于位置的视觉伺服每帧跑一次 PnP，把 \({}^{c}T_{o}\) 当作反馈，控制它收敛到期望位姿。

    6机器人中的它：带误匹配的定位

    真实的 2D–3D 匹配不是干净的：特征匹配会出错，神经网络会把关键点预测到别的物体上。一个错误的匹配就能毁掉最小二乘（FIG 6 的红色线框）。标准做法是 PnP + RANSAC（Fischler & Bolles 1981，11.5 节）：每次随机抽 3 个对应，P3P 给出至多 4 个候选位姿，按"重投影误差小于阈值的点数"给每个候选打分，保留得分最高的，最后只在内点上做 LM 精修。若内点比例为 \(w\)，每次抽 \(s\) 个点，要以概率 \(p\) 至少抽到一次全是内点的样本，需要迭代

    $$ N\;\ge\;\frac{\log(1-p)}{\log(1-w^{s})} . $$

    \(w=0.7\)、\(s=3\)、\(p=0.99\) 时 \(N\approx11\)；最小样本越小，\(N\) 越小，这是 P3P 在鲁棒估计里比 EPnP/DLT 更受欢迎的原因之一。FIG 6 里有 30 个匹配，你可以改变误匹配比例、迭代次数和阈值。

    FIG 6PnP + RANSAC：30 个匹配，其中有误匹配
    图像：匹配与三种位姿的盒子
    每次迭代的内点数
    每个点是一个 3D–2D 匹配的图像一端；绿色实心是 RANSAC 认定的内点，红色空心是被拒绝的点（其中真正的误匹配的像素是随机撒的，不在盒子上）。线框是同一个盒子在三个位姿下的投影：灰色是真值，绿色是 RANSAC + 内点上的 LM，红色虚线是对全部 30 个点直接做 DLT + LM 的结果。右图是 RANSAC 每次迭代得到的最大内点数（阶梯线）。随机数固定，"换一组"会重新生成匹配与误匹配。
    试一试
    • 误匹配 30%，迭代 ≥ 11 次：RANSAC 的旋转误差在 1.5° 以内，而对全部点做最小二乘的结果错得离谱（几十度）。再把迭代降到 2 到 3 次，多按几次"换一组"，看它偶尔失败。
    真实系统里

    标记：AprilTag 检测器找到标签的四个角点，再做平面 PnP；AprilTag 3 的 C 库用正交迭代（Lu、Hager & Mjolsness 2000）并返回两个候选位姿及各自的误差，让上层决定取舍。OpenCV 的 ArUco 模块也是对角点做 PnP。视觉重定位：hloc（Sarlin 等）和 COLMAP 的绝对位姿估计，用图像检索缩小范围，匹配局部特征，对 2D–3D 匹配做 P3P/EPnP + RANSAC，再做非线性精修。SLAM 跟踪：ORB-SLAM（Mur-Artal 等）跟踪线程把当前帧匹配到局部地图点，用"只优化位姿"的 BA 精修；丢失后用 EPnP + RANSAC 重定位。物体位姿：PVNet、DOPE 这类方法先预测物体关键点的像素位置，再用 PnP 求 \({}^{c}T_{o}\)。标定：棋盘格和 ChArUco 板每张图一次 PnP，作为手眼标定（10.4 节）的输入。标记场定位：地面已知的标记场让机器人每看到一个标记就得到一次位姿测量，可以送进滤波器（Part IV）。

    OpenCV 的 solvePnP 通过 flags 暴露了本节几乎所有算法（具体要求以你所用版本的文档为准）：

    flag 思路 何时用
    SOLVEPNP_ITERATIVEDLT（共面点用单应）初始化 + LM 最小化重投影误差默认；需要好初值，不鲁棒
    SOLVEPNP_P3P, SOLVEPNP_AP3P3 点最小解，用第 4 个点挑解恰好 4 个点；RANSAC 内部
    SOLVEPNP_EPNP4 个控制点，\(O(n)\)点多、要快
    SOLVEPNP_IPPE, SOLVEPNP_IPPE_SQUARE平面目标的闭式解，给出两个候选AprilTag/ArUco 标记，棋盘格
    SOLVEPNP_SQPNP全局最优解（Terzakis & Lourakis 2020）不想依赖初值时
    solvePnPRansac, solvePnPGenericRANSAC 外壳；返回全部解及其重投影误差有误匹配；需要比较候选

    返回的 rvec 是 \(R\) 的 Rodrigues（轴角）向量，tvec 是 \(\mathbf t\)，二者合起来就是 \({}^{c}T_{o}\)，不是相机的位姿。

    7常见误区

    误区 1：用 3D DLT 处理平面标定板

    棋盘格、AprilTag 都是平面，所有点 \(Z=0\)。FIG 2 里这时 \(A\) 至少有 3 维精确的零空间（无噪声时是 4 维），SVD 会毫不犹豫地返回其中任意一个，姿态可以错上百度，而重投影误差在这些点上仍是 0。"点上误差小"不等于"位姿对"：换一个点就暴露出来。平面用单应或 IPPE；近似共面（很薄的物体）同样危险，表现为倒数第二、第三小的奇异值也很小。

    误区 2：重投影误差小的那个候选就是对的

    FIG 5 里，5 cm 的标签在 \(Z=2\) m（图像上只有 15 像素宽）、噪声 1 px 时，约四成到一半的噪声样本让错误的候选误差更低；10 cm 的标签在 3 m 远、倾斜 60° 时也接近一半。直觉"拟合得更好就更可能是真的"在这里失效，因为两个极小值之间的差别比噪声还小。对策：用更大的标签或更近的距离；用多个标签（共同的位姿约束）；用时间上的连续性或 IMU/里程计先验选择；至少别在两个候选的误差接近时把单帧结果当真。

    误区 3：像素误差小就是位姿准

    FIG 4：目标一远一小，同样 1 像素的噪声就变成几厘米到几十厘米的相机位置不确定度（按 \(Z^{2}\)），而重投影误差仍然只有 1 像素。点聚在一起（或近似共线）时 \(J^{\top}J\) 接近奇异，某些方向几乎不被数据约束。检查协方差，而不是检查残差；把点分布得尽可能开；把"位姿测量"送进滤波器（Part IV）时带上这个协方差，不要用一个拍脑袋的常数。

    误区 4：搞混位姿的方向与约定

    solvePnP 返回 \({}^{c}T_{o}\)（物体在相机里），不是相机在物体里。要画相机、做地图定位，需要求逆：\(R_{wc}=R^{\top}\)，\(\mathbf C=-R^{\top}\mathbf t\)。把 tvec 当相机位置是错的：它是物体原点在相机系里的坐标，真正的相机位置 \(-R^{\top}\mathbf t\) 与它差了一个旋转和一个符号。还有：像素的 \(v\) 向下，相机系 y 向下（与 z-up 的世界系相反）；左扰动与右扰动的协方差不在同一个坐标系里；点在相机后面（\(z\le0\)）时 \(\pi\) 仍给出一个"看起来合理"的像素，不会报错。

    误区 5：忽略畸变、误匹配和初值

    LM 只保证局部下降（FIG 3 里 GN 在大偏移下跳进错误的极小值，LM 虽然收敛，但在更差的初值或平面歧义下可能收敛到错误的极小值）：先用 P3P/EPnP/DLT/IPPE 取初值。畸变：边缘的像素若不去畸变，就等于带着系统性的几像素偏差做 PnP，尤其是广角镜头，要么先去畸变，要么在投影函数里带上畸变模型（10.3 节）。误匹配：一个就能毁掉最小二乘（FIG 6），必须有 RANSAC 或鲁棒核函数。噪声模型太乐观：角点检测的误差在模糊、小标签时远大于 0.3 px；用乐观的 \(\sigma\) 算出的协方差也就过于自信。

    8小练习

    1. 内参 \(K\) 已知。至少需要几个 3D–2D 对应点，才能让相机位姿只有有限个解？这时最多有几个（深度全为正的）解？
    每个点 2 个方程，位姿 6 个未知数，所以 3 个点是方程数等于未知数的最小情形。余弦定理给出的三个二次方程最多有 4 个深度全正的公共解（FIG 1 里 2 个或 4 个）。两个点只留下 2 维的解族；第四个点一般能挑出唯一的那个。6 个点是"不知道 K"的 DLT 的最小数。
    2. 把 3D DLT 用在 12 个共面的点上（噪声为 0）。会发生什么？
    平面 \(\boldsymbol\pi^{\top}\tilde{\mathbf X}=0\) 上的点满足 \((P+\mathbf a\boldsymbol\pi^{\top})\tilde{\mathbf X}=P\tilde{\mathbf X}\)，所以 \(P\)、\(\mathbf e_1\boldsymbol\pi^{\top}\)、\(\mathbf e_2\boldsymbol\pi^{\top}\)、\(\mathbf e_3\boldsymbol\pi^{\top}\) 都在零空间里（FIG 2 底部多出红色的、掉到底的条）。点数再多也没用，必须用单应（平面 PnP）。
    3. 用左扰动 \(T\leftarrow\operatorname{Exp}(\delta\boldsymbol\xi)T\)，\(\delta\boldsymbol\xi=(\delta\boldsymbol\omega,\delta\mathbf v)\)。相机系中的点 \(\mathbf x_c\) 对 \(\delta\boldsymbol\xi\) 的雅可比是？
    一阶：\(\delta\mathbf x_c=\delta\boldsymbol\omega\times\mathbf x_c+\delta\mathbf v=-[\mathbf x_c]_\times\delta\boldsymbol\omega+\delta\mathbf v\)，角速度在前，所以是 \([-[\mathbf x_c]_\times\ \ I]\)。第一个选项把顺序写反了；第三个是右扰动 \(T\operatorname{Exp}(\delta\boldsymbol\xi)\) 的雅可比；第四个符号错了（\(\boldsymbol\omega\times\mathbf x=-\mathbf x\times\boldsymbol\omega\)）。
    4. 一个 10 cm 的 AprilTag，在 3 m 远处斜着 60° 放置。检测器返回两个候选位姿，重投影误差分别是 0.8 px 和 0.9 px。最合理的做法是？
    0.1 px 的差别远小于角点噪声（在 FIG 5 里把标签设成 10 cm、3 m、倾斜 60° 并按"100 次噪声"：接近一半的样本里，误差较低的候选是错的）。平均两个旋转没有意义（它们是镜像的），也不是标定问题，而是平面小目标的固有歧义。
    5. 匹配里一半（\(w=0.5\)）是内点，P3P-RANSAC 每次抽 3 个点。要以 99% 的概率至少抽到一次全是内点的样本，大约需要多少次迭代？
    \(N=\log(0.01)/\log(1-0.5^3)=4.605/0.1335\approx34.5\)，取 35。对比 \(w=0.7\)（\(N\approx11\)）：内点比例降一点，迭代数涨很多；若最小样本是 4 个点，\(N\) 还要翻倍（\(w=0.5\)、\(s=4\) 时约 72）。

    9延伸阅读

    • R. Hartley & A. Zisserman, Multiple View Geometry in Computer Vision (2nd ed., Cambridge, 2004), Ch. 7 — 相机矩阵 \(P\) 的估计：DLT、归一化、代数误差与几何误差、共面退化
    • M. A. Fischler & R. C. Bolles, Random Sample Consensus, Communications of the ACM 24(6), 1981 — RANSAC 的原始论文，应用例子正是 P3P 位姿估计
    • R. M. Haralick et al., Review and Analysis of Solutions of the Three Point Perspective Pose Estimation Problem, IJCV 13(3), 1994 — P3P 各种解法（含 Grunert）的比较
    • X.-S. Gao, X.-R. Hou, J. Tang & H.-F. Cheng, Complete Solution Classification for the Perspective-Three-Point Problem, IEEE TPAMI 25(8), 2003 — P3P 解的个数的完整分类
    • V. Lepetit, F. Moreno-Noguer & P. Fua, EPnP: An Accurate O(n) Solution to the PnP Problem, IJCV 81(2), 2009 — 4 个控制点的 \(O(n)\) 解
    • C.-P. Lu, G. D. Hager & E. Mjolsness, Fast and Globally Convergent Pose Estimation from Video Images, IEEE TPAMI 22(6), 2000 — 正交迭代，AprilTag 3 用的精修方法
    • T. Collins & A. Bartoli, Infinitesimal Plane-Based Pose Estimation, IJCV 109(3), 2014 — IPPE：平面目标位姿的两个解的闭式解
    • G. Schweighofer & A. Pinz, Robust Pose Estimation from a Planar Target, IEEE TPAMI 28(12), 2006 — 平面目标位姿的双解歧义分析
    • G. Terzakis & M. Lourakis, A Consistently Fast and Globally Optimal Solution to the Perspective-n-Point Problem, ECCV 2020 — SQPnP
    • E. Olson, AprilTag: A Robust and Flexible Visual Fiducial System, ICRA 2011 — AprilTag 检测系统
    • R. Mur-Artal, J. M. M. Montiel & J. D. Tardós, ORB-SLAM: A Versatile and Accurate Monocular SLAM System, IEEE T-RO 31(5), 2015 — 跟踪、重定位与"仅位姿"优化的实际流程