EPnP 算法详解

在计算机视觉和摄影测量中，PnP (Perspective-n-Point) 问题 是指：已知 n n n 个 3D 空间点及其在相机图像平面的 2D 投影点，求相机的位姿（旋转矩阵 R R R 和平移向量 t t t）。

EPnP (Efficient Perspective-n-Point) 是由 Lepetit 等人在 2009 年提出的一种非迭代的 PnP 求解算法。它的核心优势在于：

计算复杂度为 O ( n ) O(n) O(n)（而传统的基于非线性优化的方法复杂度通常更高）
速度快且精度高
非常适合处理大量特征点的情况

一、EPnP 的核心思想

EPnP 的巧妙之处在于，它没有直接去求解 n n n 个 3D 点在相机坐标系下的坐标，而是引入了 4 个虚拟的"控制点"（Control Points）。

整体思路可以概括为：

将 n n n 个参考 3D 点表示为这 4 个控制点的加权和
因为刚体变换（从世界坐标系到相机坐标系）不改变相对的权重（即重心坐标不变），所以我们可以把"求 n n n 个点的相机坐标"转化为"求 4 个控制点的相机坐标"
通过 2D-3D 投影关系建立线性方程组，求出 4 个控制点在相机坐标系下的坐标
利用已知的权重恢复出 n n n 个 3D 点在相机坐标系下的坐标
最后通过 3D-3D 的点云配准（如 ICP 的 SVD 求解法）计算出 R R R 和 t t t

二、详细数学推导

1. 定义控制点与重心坐标（齐次表示）

在世界坐标系下，选择 4 个不共面的控制点 C j w ( j = 1 , 2 , 3 , 4 ) C_j^w \ (j=1,2,3,4) Cjw (j=1,2,3,4)。

通常为了稳定性：

选择所有 3D 点的质心作为第一个控制点
另外三个控制点通过对数据进行主成分分析（PCA） 得到的主方向来确定

任意一个 3D 点 P i w P_i^w Piw（其中 i = 1 , ... , n i=1, \dots, n i=1,...,n）都可以表示为这 4 个控制点的线性组合：

P i w = ∑ j = 1 4 α i j C j w P_i^w = \sum_{j=1}^{4} \alpha_{ij} C_j^w Piw=j=1∑4αijCjw

其中 α i j \alpha_{ij} αij 是齐次重心坐标，满足归一化条件：

∑ j = 1 4 α i j = 1 \sum_{j=1}^{4} \alpha_{ij} = 1 j=1∑4αij=1

由于世界坐标系下的 P i w P_i^w Piw 和 C j w C_j^w Cjw 都是已知的，权重因子 α i j \alpha_{ij} αij 可以通过解线性方程组提前计算出来，在后续过程中保持常数不变。

2. 坐标系转换的权重不变性

从世界坐标系到相机坐标系的变换是刚体变换，形式为：

P c = R P w + t P^c = R P^w + t Pc=RPw+t

我们将 P i w P_i^w Piw 的表达式代入：

P i c = R ( ∑ j = 1 4 α i j C j w ) + t P_i^c = R \left( \sum_{j=1}^{4} \alpha_{ij} C_j^w \right) + t Pic=R(j=1∑4αijCjw)+t

利用 ∑ j = 1 4 α i j = 1 \sum_{j=1}^{4} \alpha_{ij} = 1 ∑j=14αij=1 的性质，可以将 t t t 写进求和符号内：

P i c = ∑ j = 1 4 α i j R C j w + ∑ j = 1 4 α i j t = ∑ j = 1 4 α i j ( R C j w + t ) P_i^c = \sum_{j=1}^{4} \alpha_{ij} R C_j^w + \sum_{j=1}^{4} \alpha_{ij} t = \sum_{j=1}^{4} \alpha_{ij} (R C_j^w + t) Pic=j=1∑4αijRCjw+j=1∑4αijt=j=1∑4αij(RCjw+t)

由于 C j c = R C j w + t C_j^c = R C_j^w + t Cjc=RCjw+t，我们得到非常关键的结论：

在相机坐标系下，3D 点同样可以用同样的权重 α i j \alpha_{ij} αij 来表示：

P i c = ∑ j = 1 4 α i j C j c \boxed{P_i^c = \sum_{j=1}^{4} \alpha_{ij} C_j^c} Pic=j=1∑4αijCjc

3. 建立相机的投影方程

设相机内参矩阵为 K K K，对于第 i i i 个点：

其在相机坐标系下的坐标为 P i c = [ x i c , y i c , z i c ] T P_i^c = [x_i^c, y_i^c, z_i^c]^T Pic=[xic,yic,zic]T
其在图像上的 2D 投影点为 [ u i , v i ] T [u_i, v_i]^T [ui,vi]T

根据针孔相机模型：

s i [ u i v i 1 ] = [ f x 0 c x 0 f y c y 0 0 1 ] [ x i c y i c z i c ] s_i \begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x_i^c \\ y_i^c \\ z_i^c \end{bmatrix} si uivi1 = fx000fy0cxcy1 xicyiczic

其中 s i s_i si 是深度因子 ，即 s i = z i c s_i = z_i^c si=zic。

展开上述方程，得到：

s i u i = f x x i c + c x z i c s_i u_i = f_x x_i^c + c_x z_i^c siui=fxxic+cxzic

s i v i = f y y i c + c y z i c s_i v_i = f_y y_i^c + c_y z_i^c sivi=fyyic+cyzic

将 P i c = ∑ j = 1 4 α i j C j c P_i^c = \sum_{j=1}^{4} \alpha_{ij} C_j^c Pic=∑j=14αijCjc 代入。设第 j j j 个控制点在相机坐标系下为 C j c = [ x j c , y j c , z j c ] T C_j^c = [x_j^c, y_j^c, z_j^c]^T Cjc=[xjc,yjc,zjc]T：

x i c = ∑ j = 1 4 α i j x j c , y i c = ∑ j = 1 4 α i j y j c , z i c = ∑ j = 1 4 α i j z j c x_i^c = \sum_{j=1}^{4} \alpha_{ij} x_j^c, \quad y_i^c = \sum_{j=1}^{4} \alpha_{ij} y_j^c, \quad z_i^c = \sum_{j=1}^{4} \alpha_{ij} z_j^c xic=j=1∑4αijxjc,yic=j=1∑4αijyjc,zic=j=1∑4αijzjc

代入投影方程并消去 s i s_i si（即 z i c z_i^c zic），我们可以得到两个线性方程：

∑ j = 1 4 α i j f x x j c + ∑ j = 1 4 α i j ( c x − u i ) z j c = 0 \sum_{j=1}^{4} \alpha_{ij} f_x x_j^c + \sum_{j=1}^{4} \alpha_{ij} (c_x - u_i) z_j^c = 0 j=1∑4αijfxxjc+j=1∑4αij(cx−ui)zjc=0

∑ j = 1 4 α i j f y y j c + ∑ j = 1 4 α i j ( c y − v i ) z j c = 0 \sum_{j=1}^{4} \alpha_{ij} f_y y_j^c + \sum_{j=1}^{4} \alpha_{ij} (c_y - v_i) z_j^c = 0 j=1∑4αijfyyjc+j=1∑4αij(cy−vi)zjc=0

4. 构建并求解线性方程组

我们将 4 个未知控制点的相机坐标组合成一个 12 × 1 12 \times 1 12×1 的列向量：

x = [ C 1 c T , C 2 c T , C 3 c T , C 4 c T ] T = [ x 1 c , y 1 c , z 1 c , ... , x 4 c , y 4 c , z 4 c ] T \mathbf{x} = \left[ C_1^{cT}, C_2^{cT}, C_3^{cT}, C_4^{cT} \right]^T = \left[ x_1^c, y_1^c, z_1^c, \dots, x_4^c, y_4^c, z_4^c \right]^T x=[C1cT,C2cT,C3cT,C4cT]T=[x1c,y1c,z1c,...,x4c,y4c,z4c]T

每个 3D-2D 点对提供 2 个方程， n n n 个点对可以构建一个 2 n × 12 2n \times 12 2n×12 的矩阵 M M M：

M x = 0 M \mathbf{x} = 0 Mx=0

为了解这个齐次线性方程组，我们计算 M T M M^T M MTM 并对其进行特征值分解 （或对 M M M 进行 SVD 分解 ）。 x \mathbf{x} x 的解在 M M M 的零空间（Null space）中。

计算复杂度说明

构建 M T M M^T M MTM 的大小固定为 12 × 12 12 \times 12 12×12，这一步遍历 n n n 个点，复杂度为 O ( n ) O(n) O(n)
求解 12 × 12 12 \times 12 12×12 矩阵的特征向量复杂度为 O ( 1 ) O(1) O(1)
因此整体算法是高效的

由于噪声和点构型的影响，零空间的维度 N N N 可能是 1, 2, 3 或 4。真正的解 x \mathbf{x} x 可以表示为属于最小奇异值的特征向量的线性组合：

x = ∑ k = 1 N β k v k \mathbf{x} = \sum_{k=1}^{N} \beta_k \mathbf{v}_k x=k=1∑Nβkvk

其中 v k \mathbf{v}_k vk 是通过 SVD 求得的特征向量， β k \beta_k βk 是需要进一步确定的系数。

5. 确定组合系数 β k \beta_k βk

为了求出 β k \beta_k βk，EPnP 利用了距离保持原理（刚体变换不改变距离）：

在世界坐标系下控制点之间的距离，应当等于在相机坐标系下控制点之间的距离。

∥ C a c − C b c ∥ 2 = ∥ C a w − C b w ∥ 2 , ∀ a , b ∈ { 1 , 2 , 3 , 4 } , a ≠ b \|C_a^c - C_b^c\|^2 = \|C_a^w - C_b^w\|^2, \quad \forall a, b \in \{1,2,3,4\}, a \neq b ∥Cac−Cbc∥2=∥Caw−Cbw∥2,∀a,b∈{1,2,3,4},a=b

因为有 4 个控制点，所以可以列出 C 4 2 = 6 C_4^2 = 6 C42=6 个距离约束方程。

将 x = ∑ β k v k \mathbf{x} = \sum \beta_k \mathbf{v}_k x=∑βkvk 代入距离方程中，可以将其转化为关于 β k \beta_k βk 的方程组。根据零空间维度 N N N 的不同（1 到 4），EPnP 论文分别给出了使用线性化（Linearization） 求解 β k \beta_k βk 的具体方法，并选出重投影误差最小的那组解。

6. 恢复位姿 ( R , t ) (R, t) (R,t)

求出 x \mathbf{x} x（即 4 个控制点在相机坐标系下的坐标 C j c C_j^c Cjc）后，我们通过前面的公式直接算出所有 n n n 个点在相机坐标系下的坐标：

P i c = ∑ j = 1 4 α i j C j c P_i^c = \sum_{j=1}^{4} \alpha_{ij} C_j^c Pic=j=1∑4αijCjc

至此，我们拥有了 n n n 个点在世界坐标系下的坐标 P i w P_i^w Piw 和在相机坐标系下的对应坐标 P i c P_i^c Pic。这就变成了一个标准的 3D-3D 绝对定向问题（Absolute Orientation Problem）。

这可以通过以下经典方法轻松求解出位姿 R R R 和 t t t：

Kabsch 算法（基于 SVD）
Horn 方法（基于四元数）

P i c = R P i w + t P_i^c = R P_i^w + t Pic=RPiw+t

三、算法总结

EPnP 将求解 n n n 个点的位姿估计问题，巧妙地转化成了求解 4 个控制点的问题。其数学步骤可以概括为：

步骤	内容
① 世界系下选点	选 4 个控制点 C w C^w Cw，算权重 α \alpha α
② 构建投影方程	利用 α \alpha α 和 2D 坐标构建 M x = 0 M \mathbf{x} = 0 Mx=0
③ 求控制点相机坐标	SVD 解 M M M，结合控制点间距离约束求出真正的 C c C^c Cc
④ 配准求解 R , t R, t R,t	恢复 P c P^c Pc，执行 3D-3D 配准

目录