PnP 算法的本质

本文最后更新于：2026年7月9日上午

PnP（Perspective-n-Point）是已知 3D 点位置、它们的 2D 像素投影、相机内参，反推相机位姿的问题。它是相机定位、SLAM 重定位、AR 追踪的核心。这篇不重复 DLT/P3P 的力学推导和 OpenCV 接口（另见 PnP 算法），而是讲 PnP 的本质——射线交汇——以及最常用方法 EPnP 的完整推导。

问题

一句话：已知 3D 点在哪、它们在图像上的像素在哪、相机的光学参数（内参），反推相机在哪、朝哪看。数学表述：

$$ \lambda_j\mathbf{p}_j=K(RX_j+\mathbf{t}),\quad j=1,\ldots,N $$

已知	未知
$X_j$：3D 点世界坐标	$R$：相机旋转（3 DOF）
$\mathbf{p}_j$：像素坐标	$\mathbf{t}$：相机平移（3 DOF）
$K$：内参矩阵	$\lambda_j$：每点深度（nuisance，不求）

真正要求的是 6 个未知数：$R$（3）加 $\mathbf{t}$（3）。$\lambda_j$ 是齐次坐标引入的深度因子，通过交叉相乘可以消掉，不需要求。

本质一：约束

展开投影方程（消去 $\lambda_j$）：

$$ u_j=\frac{f_x(r_1^\top X_j+t_x)}{r_3^\top X_j+t_z}+c_x,\qquad v_j=\frac{f_y(r_2^\top X_j+t_y)}{r_3^\top X_j+t_z}+c_y $$

每个 3D-2D 对应给出 2 个方程（$u,v$ 各一个），6 个未知数除以每对应 2 方程，理论最少 3 个点。但 3 点会产生多解（P3P 有 4 解），实际用 ≥4 点消歧。

本质二：射线交汇

PnP 的核心理解是射线交汇。每个像素加内参定义了一条从相机出发的射线方向 $\mathbf{d}_j=K^{-1}\mathbf{p}_j$；PnP 就是找一个相机位姿，使得从那个位置出发、沿这些射线方向看出去，恰好能"穿过"已知的 3D 点：

           相机 C（位置未知）
          /  |  \
        d1   d2   d3    ← 射线方向（从像素+内参算出，已知）
        /    |    \
      X1    X2    X3     ← 3D 点位置（已知）

"从哪看、朝哪看，才能让这些射线穿过这些点？"

这就是为什么 P3P 能用余弦定理求解——三条射线的两两夹角可从像素算出，三个 3D 点的间距可从坐标算出，于是变成"已知三角形三边和三角，求顶点（相机）到三顶点距离"的几何题。

本质三：方程组求解

PnP 的不同算法（P3P / EPnP / DLT / Iterative）本质都是用不同数学技巧求解同一个非线性方程组：

算法	核心技巧	把非线性问题转化成什么
P3P	余弦定理 + 代数消元	四次方程（最多 4 解）
EPnP	控制点加权（降维）	12 维线性方程 $M\mathbf{x}=\mathbf{0}$
DLT	把 $R,\mathbf{t}$ 当一般矩阵	12 维线性方程 $A\mathbf{p}=\mathbf{0}$
Iterative	泰勒展开 + LM 迭代	每轮一个线性方程组

下面挑两个最有代表性的展开：P3P（纯几何）和 EPnP（代数降维）。

P3P

不碰线性代数，用初中几何直接解。已知三点间距 a, b, c（3D 坐标算）、三条射线间夹角 θ12, θ13, θ23（像素加内参算），求相机到三点距离 s1, s2, s3。三个余弦定理方程：

$$ s_1^2+s_2^2-2s_1s_2\cos\theta_{12}=c^2 $$ $$ s_1^2+s_3^2-2s_1s_3\cos\theta_{13}=b^2 $$ $$ s_2^2+s_3^2-2s_2s_3\cos\theta_{23}=a^2 $$

代换 $s_2=v\cdot s_1,\ s_3=w\cdot s_1$ 化为 $s_1$ 的一元方程，得到四次方程，最多 4 个实数解，用第 4 个点（已知其 3D 位置和像素）验证重投影误差最小的那个为唯一解。本质是把位姿估计变成纯距离求解，求出距离后三边定位（类似 GPS 三球交汇）。

EPnP

EPnP 的思路：N 个 3D 点的相机系坐标有 3N 个未知数太多，用 4 个控制点"压缩"到 12 维，把非线性问题变成线性。

控制点的已知与未知

EPnP 有两套控制点，分属两个坐标系：

控制点	坐标系	已知/未知	用途
$C_k^w$（世界系控制点）	世界坐标系	已知（自己选的）	算权重 $\alpha_{jk}$
$C_k^c$（相机系控制点）	相机坐标系	未知（要求解）	投影方程的 12 个未知数

选控制点

从 N 个已知 3D 世界点里选 4 个控制点：质心加 PCA 三个主方向上的点。这 4 个点在世界系下完全确定。

计算权重

每个 3D 世界点表示为 4 个控制点的重心坐标（仿射组合）：

$$ X_j^w=\sum_{k=1}^{4}\alpha_{jk}C_k^w,\qquad\sum_{k=1}^{4}\alpha_{jk}=1 $$

权重归一

把权重关系写成矩阵形式，第 4 行就是 $1=\sum\alpha_{jk}$。几何上，4 个不共面的 3D 点构成四面体，空间任一点是 4 顶点的仿射组合，仿射组合的定义就是权重之和为 1。

更关键的是，这个条件是后续权重不变性成立的前提。刚体变换把世界系变到相机系：

$$ X_j^c=R X_j^w+\mathbf{t}=R\Big(\sum_k\alpha_{jk}C_k^w\Big)+\mathbf{t}=\sum_k\alpha_{jk}(RC_k^w+\mathbf{t})=\sum_k\alpha_{jk}C_k^c $$

最后一个等号能成立，恰恰因为 $\sum_k\alpha_{jk}=1$，平移 $\mathbf{t}$ 才能吸收进求和（$\sum_k\alpha_{jk}\mathbf{t}=\mathbf{t}$）。若 $\sum\alpha\ne1$，权重不变性被破坏，整个 EPnP 推导失效。

这也是为什么选 4 个控制点而不是 3 个：3D 空间的仿射基需要 4 个点（3 个向量定方向加 1 个点定原点），3 个点只能定义平面（2D 仿射）。$\alpha_{jk}$ 算完就固定了，已知。

投影约束

代入投影方程 $\lambda_j\mathbf{p}_j=K X_j^c=K\sum_k\alpha_{jk}C_k^c$，设 $C_k^c=(x_k^c,y_k^c,z_k^c)^\top$，由第三行 $\lambda_j=\sum_k\alpha_{jk}z_k^c$，代入前两行消去 $\lambda_j$：

$$ \sum_{k=1}^{4}\alpha_{jk}\big(f_x x_k^c+c_x z_k^c-u_j z_k^c\big)=0 $$ $$ \sum_{k=1}^{4}\alpha_{jk}\big(f_y y_k^c+c_y z_k^c-v_j z_k^c\big)=0 $$

每个点给 2 个关于 $\{x_k^c,y_k^c,z_k^c\}_{k=1}^4$ 的线性方程。N 个点堆叠为：

$$ M\mathbf{x}=\mathbf{0},\quad M\in\mathbb{R}^{2N\times 12},\quad \mathbf{x}=[x_1^c,y_1^c,z_1^c,\ldots,x_4^c,y_4^c,z_4^c]^\top $$

SVD 求 $M$ 的零空间，得到相机系控制点 $C_k^c$。

SVD 恢复位姿

有了 $\{C_k^c\}$（相机系）和 $\{C_k^w\}$（世界系），用和 ICP 完全相同的 SVD 刚体对齐。记 $\bar{C}^c,\bar{C}^w$ 为两组控制点各自的质心：

$$ H=\sum_{k=1}^{4}(C_k^c-\bar{C}^c)(C_k^w-\bar{C}^w)^\top=U\Sigma V^\top\implies R=VU^\top,\ \mathbf{t}=\bar{C}^c-R\bar{C}^w $$

EPnP 的设计要点

不直接求 $R,\mathbf{t}$（6 个未知数但高度非线性），而是求 $C_k^c$（12 个未知数但线性）。求出后用 SVD 刚体对齐恢复 $R,\mathbf{t}$。用更多未知数（12 vs 6）换取线性性，一次 SVD 求解，不依赖初值、对噪声鲁棒。

DLT 与 Iterative

DLT：不管 $R$ 是不是旋转矩阵，把 $P=K[R|\mathbf{t}]$ 当一般 $3\times4$ 矩阵（12 个数），线性求解后再 RQ 分解。代价是解可能不是有效旋转，需后处理投影到 SO(3)。

Iterative：直接最小化重投影误差 $\min_{R,\mathbf{t}}\sum_j\|\mathbf{p}_j-\pi(K,R,\mathbf{t},X_j)\|^2$，用 LM 迭代。最直接但需初值，通常用 EPnP 给初值再 LM 精化。

与三角化、BA 的关系

PnP 不是孤立的，它和多视图几何的其它问题有深层联系：

问题	已知	求解	关系
三角化	两个相机位姿 + 像素	3D 点位置	PnP 的逆问题
PnP	3D 点位置 + 像素 + 内参	相机位姿	三角化的逆问题
手眼标定	多组 PnP 结果 + 机械臂位姿	相机↔机械臂变换	PnP 的串联
BA	所有 3D 点 + 所有像素	所有位姿 + 所有 3D 点	PnP 的批量版

统一视角：都是在"3D 点位置"和"相机位姿"之间，通过投影方程建立约束，已知一组求另一组——知道位姿求点是三角化，知道点求位姿是 PnP，全都不知道是 SfM/BA。

小结

PnP 的本质是射线交汇问题：每个像素加内参定义一条从相机出发的射线方向，PnP 找一个相机位姿使这些射线恰好穿过已知 3D 点。不同算法是求解它的不同数学策略：P3P 用余弦定理把位姿变成距离方程（几何法），EPnP 用控制点降维到 12 维线性方程（代数法），Iterative 直接最小化重投影误差（优化法）。EPnP 的精髓是用 12 个线性未知数代替 6 个非线性未知数，靠权重不变性（$\sum\alpha=1$）把刚体变换吸收进控制点表示。

参考资料

V. Lepetit, F. Moreno-Noguer, P. Fua, “EPnP: An Accurate O(n) Solution to the PnP Problem,” IJCV, 2009.
Y. Zheng et al., “A Complete Solution Classification for the Perspective-Three-Point Problem (P3P),” IEEE PAMI, 2013.
R. Hartley and A. Zisserman, Multiple View Geometry in Computer Vision, ch.4（DLT）.
高翔，《视觉SLAM十四讲》。
相关笔记：PnP 算法（DLT/P3P 力学推导与 OpenCV 接口）、ICP 配准、SLAM 全景