PnP 算法 — DLT, P3P, EPnP, RANSAC, GN

  • Description：Perspective-n-Point 问题的各种解法 — 线性的 DLT、最少点的 P3P、效率更高的 EPnP、RANSAC 抗外点框架、Gauss-Newton 精化。含代码层面的实现细节
  • Source：从 [K2E-2] SLAM Common (Notion id fb32092b-1e02-4220-9f09-8d991febb891) 的 PnP 章节拆分
  • Created：2023-04-10
  • Updated：2026-06-02
  • License：转载欢迎 — 请署名 Yu Zhang 并链回 yuzhang.io 原文

Table of Contents

1. PnP 问题

Perspective-n-Point — 已知 N 个 3D 空间点 $P_i$ 及其在图像上的 2D 像素投影 $p_i$，求相机位姿 $(R, t)$。

PnP 几何结构 — 已知 3D 点 P_i 及其像素投影 p_i，求相机相对世界坐标系的位姿 (R, t)

VSLAM 典型场景：

  • 第 1 帧用双目三角化建立世界坐标系下的 3D 地图点 ${P_i}$
  • 第 K 帧检测特征 + 描述子匹配得到 2D 像素 ${p_i}$
  • N 组 2D-3D 匹配对 → PnP 解出第 K 帧相机相对于世界坐标系的位姿

数学建模 (截断最小二乘 truncated least squares，考虑外点)：

$$ (R^, t^) = \arg\min_{R, t} \sum_{i=1}^{N} \min \left( | p_i - \pi(K(RP_i + t)) |^2, , \tau^2 \right) $$

其中：

  • $\pi(\cdot)$ 是投影函数 (除以 $Z^c$ 归一化，再乘内参 $K$ 得像素坐标)
  • $\tau$ 是内点/外点阈值 (单位 pixel，工程上常取 5)
  • 截断项 $\min(\cdot, \tau^2)$ 等价于隐式引入 "是否内点" 的二值指示变量 → 严格说不是纯连续优化，混入组合优化

实践中拆成两步：RANSAC 框架剔除外点 + DLT/P3P 出初值 + GN/LM 精化内点最小二乘。

2. DLT 求解

Direct Linear Transform — 把非线性的 SO(3) 约束松弛掉，写成线性方程组求解。

(本节的 OpenCV 实现技巧 — 去畸变与归一化坐标、$\lambda$ 尺度恢复、标定板平面退化第三列叉乘补 — 整理自知乎作者无偏估计的 深入浅出 PnP)

公式推导

去畸变后用归一化坐标 $(x, y)$ 代替像素坐标 (这一步用 OpenCV 的 cv::undistortPoints(pts_2, un_pts_2, K, D)，输出已经在归一化平面)。

设 $H = [R \mid t]$ 是增广 $3 \times 4$ 矩阵，记其三行为 $h_1, h_2, h_3$，每对匹配点提供：

$$ \begin{cases} h_1^T P - h_3^T P \cdot x = 0 \ h_2^T P - h_3^T P \cdot y = 0 \end{cases} $$

12 个未知数 ($h_1, h_2, h_3$ 各 4 个分量)，每对点提供 2 个约束 → 最少 6 对点。

构造超定方程 $Ax = 0$ (其中 $x = [h_1; h_2; h_3] \in \mathbb{R}^{12}$)：

$$ \begin{bmatrix} P_1^T & 0 & -x_1 P_1^T \ 0 & P_1^T & -y_1 P_1^T \ \vdots & \vdots & \vdots \ P_N^T & 0 & -x_N P_N^T \ 0 & P_N^T & -y_N P_N^T \end{bmatrix} \begin{bmatrix} h_1 \ h_2 \ h_3 \end{bmatrix} = 0 $$

加约束 $| x |_2 = 1$ 避免平凡零解 → SVD 分解 $A = U D V^T$，最优解 = 最小奇异值对应的 $V$ 的列向量。

投影回 SO(3)

DLT 解出来的 $H_R$ (前三列) 不一定是合法旋转矩阵，需要投影回 SO(3)：

  1. 正负号修正：把任一 $P_i$ 代回检查 $Z^c > 0$ (相机只能看见前方物体)，若不满足则整体乘 $-1$
  2. 正交投影：对 $H_R$ 做 SVD，$H_R = U \Sigma V^T$，则最近正交矩阵为 $R = U V^T$
  3. 右手系修正：若 $\det(R) = -1$ (反射矩阵)，把 $V$ 最后一列取负号后重新算 $R$

平移恢复：DLT 解和真实 $H$ 差一个尺度因子 $\lambda$。优化后的 $R$ 第一列模长 = 1，原始 $H$ 第一列模长 ≠ 1 → $\lambda = 1 / | H[:, 0] |$，再 $t = \lambda \cdot H[:, 3]$。

退化情形

标定板做 PnP 时，3D 点都在 $z=0$ 平面 → $H$ 第三列不受测量影响 → 公式需要重推。求 $R$ 时第三列可由前两列叉乘补出。

3. P3P (最少点解法)

3 对匹配点求解 PnP 的最少点解法 (Grunert 1841 / Gao 2003 / Kneip 2011)。

核心思路：

  • 用 3 个 3D-2D 匹配对，利用三角形余弦定理推导 3 个相机到空间点的距离
  • 得到一元四次方程 → 最多 4 组候选解
  • 需要第 4 对匹配点做 disambiguation 选出唯一正确解

优点：

  • 最少点数 (DLT 需要 6 个) → RANSAC 迭代次数大幅减少
  • $C(N, 3)$ vs $C(N, 6)$，复杂度低很多

缺点：

  • 仅用 3 个点，未利用所有信息 → 精度低，必须配合精化
  • 4 个候选解需要额外验证

实务中 P3P 通常作为 RANSAC 内的最小解算器，配合 EPnP 或 GN 精化。

4. EPnP (高效 PnP)

Efficient PnP (Lepetit, Moreno-Noguer, Fua 2009)，O(N) 复杂度的非迭代解法。

核心思路

把每个 3D 参考点表示为 4 个控制点的重心坐标 (barycentric) 线性组合，优化只针对这 4 个控制点 (而不是 $N$ 个参考点)，所以与点数无关、速度快。

步骤

Step 1. 世界系下选 4 个控制点 $c^w_j$

原则上不共面即可，但论文推荐通过 PCA 选最优配置：

  • $c^w_1$ = 所有参考点的重心 (作为坐标原点)
  • 构造 $A_{n \times 3}$，行为 $(P_i^w - c^w_1)^T$
  • 计算 $A^T A$ 的特征值 $\lambda_1, \lambda_2, \lambda_3$ 和对应特征向量 $v_1, v_2, v_3$
  • 剩余三个控制点：$c^w_{k+1} = c^w_1 + \sqrt{\lambda_k} , v_k$，$k = 1, 2, 3$

这相当于沿点云主轴方向 (PCA) 放置控制点，能保证 4 点不共面且数值稳定。

Step 2. 计算重心坐标 $\alpha_{ij}$

参考点写为控制点加权和：

$$ P^w_i = \sum_{j=1}^{4} \alpha_{ij} c^w_j, \quad \sum_{j=1}^{4} \alpha_{ij} = 1 $$

由约束矩阵形式：

$$ \begin{bmatrix} P^w_i \ 1 \end{bmatrix} = C \begin{bmatrix} \alpha_{i1} \ \alpha_{i2} \ \alpha_{i3} \ \alpha_{i4} \end{bmatrix}, \quad C = \begin{bmatrix} c^w_1 & c^w_2 & c^w_3 & c^w_4 \ 1 & 1 & 1 & 1 \end{bmatrix}_{4\times 4} $$

直接求逆得 $\alpha_i = C^{-1} [P_i^w; 1]$。

关键性质 (简书作者瀚文文文问问 文 中强调的)：$\alpha_{ij}$ 是几何不变量 — 在世界系和相机系下完全相同。这意味着可以在世界系求出 $\alpha$，直接搬到相机系用，省掉一组未知数 — 这就是 EPnP 高效的关键。

Step 3. 解出控制点在相机系下坐标 $c^c_j$

相机系下同样有 $P^c_i = \sum_j \alpha_{ij} c^c_j$。结合投影模型 $\pi(P^c_i) = p_i$ (像素投影)，每对 2D-3D 提供 2 个方程，组建 $M x = 0$，其中 $x \in \mathbb{R}^{12}$ 是 4 个控制点的相机系坐标。

$M$ 是 $2N \times 12$，直接对 $M$ 做 SVD 的复杂度是 $O(N)$ (按行数线性增长)。EPnP 的优化点：对 $M^T M$ 做特征值分解，复杂度同为 $O(N)$，但常数更小 (因为 $M^T M$ 是固定的 $12 \times 12$，特征分解是 $O(1)$；只需 $O(N)$ 构造 $M^T M$，避免对整个 $2N \times 12$ 矩阵做 SVD)。

Step 4. 求解尺度系数 $\beta_k$

$x$ 写成 $M^T M$ 零空间特征向量的线性组合：

$$ x = \sum_{k=1}^{N} \beta_k v_k $$

其中 $v_k$ 是 $M^T M$ 的最小 $N$ 个特征值对应的特征向量。$N$ 一般取 1, 2, 3, 4，依焦距和点数选择 — 论文建议尝试所有情况并比较重投影误差。

$\beta_k$ 通过控制点距离约束求解 — 4 个控制点在世界系和相机系下两两距离相等：

$$ | c^c_i - c^c_j | = | c^w_i - c^w_j |, \quad i, j \in {1,2,3,4}, i < j $$

共 $C(4, 2) = 6$ 个方程。展开后是关于 $\beta$ 的二次方程组：

$N$ 未知数 方程 处理
1 1 ($\beta_1$) 6 超定，最小二乘
2 3 ($\beta_{11}, \beta_{12}, \beta_{22}$ 等) 6 线性化 + 最小二乘
3 6 6 直接解 (或 OpenCV 用近似)
4 10 6 欠定，OpenCV 取 4 列做 SVD 近似

Step 5. GN 精化 $\beta$

目标函数 — 缩小两个坐标系下控制点间距差：

$$ \beta^* = \arg\min_\beta \sum_{i<j} \left( | c^c_i - c^c_j |^2 - | c^w_i - c^w_j |^2 \right)^2 $$

每个 $\beta_k$ 候选解 (N=1..4) 都做 GN 精化，比较重投影误差选最优。

Step 6. 由 $c^c_j$ 恢复 $R, t$ — 退化为 ICP

得到控制点在相机系坐标后，剩下就是：已知一组 3D 点 ($c^w_j$) 和它们在另一坐标系下的坐标 ($c^c_j$)，求刚体变换 $(R, t)$ — 这就是 3D-3D ICP 问题，闭式 SVD 解。

优点

  • $O(N)$ 复杂度
  • 精度比 DLT 高 (用了所有点 + 控制点约束)
  • 可处理平面退化情形 (取 3 控制点的特例)

缺点

  • 实现较复杂 (6 个步骤)
  • $\beta$ 求解的 $N$ 选择有启发式成分
  • 仍需后续 GN 精化以达到最优

5. RANSAC 抗外点

RANdom SAmple Consensus — 基于"最大一致性"假设剔除外点。

步骤

  1. 随机抽 $k$ 个点 (DLT $k=6$, P3P $k=3$) → 算出位姿
  2. 用该位姿计算所有匹配对的重投影误差，小于 $\tau$ (如 5 px) 的标记为内点
  3. 统计内点数 → 维护最大内点集对应的模型
  4. 重复 1-3 (放回抽样，始终从总体抽，不是从已有内点集抽)
  5. 终止条件：内点比例够大 (如 > 50%)，或达到最大迭代次数 (如 1000)

关键：缩小最小解集 $k$

RANSAC 找到全内点子集的概率：

$$ p_{\text{success}} = 1 - (1 - w^k)^{N_{\text{iter}}} $$

其中 $w$ 是内点比例，$k$ 是最小解集大小。$k$ 越小，需要的迭代次数越少。这就是为什么 P3P ($k=3$) 配合 RANSAC 比 DLT ($k=6$) 更高效。

关于 "外点率 > 50%" 的误解

(以下论证整理自无偏估计 文)

常见说法 "RANSAC 不能处理外点率超过 50%" 不严谨。只要外点是随机分布、相互不一致 (generic outliers)，即使内点只占 1% 也能找出来 — 因为外点之间形成的一致性集合很小 (< 内点数)，足够多迭代后还是会随机命中正确的内点子集。

真正麻烦的是对抗性外点 (adversarial outliers) — 比如重复纹理场景，错配的特征点之间也满足某种一致性，会形成虚假的"大一致集"，迷惑 RANSAC。

6. Gauss-Newton 精化

DLT/P3P/EPnP 都是初值，需要 GN/LM 精化以达到最优。

残差 (归一化平面上的重投影误差)：

$$ e_i = \begin{bmatrix} x_i \ y_i \end{bmatrix} - \frac{1}{Z_i^c} \begin{bmatrix} X_i^c \ Y_i^c \end{bmatrix}, \quad [X_i^c; Y_i^c; Z_i^c] = R P_i + t $$

雅可比 (误差对位姿 $\xi = [t; \phi] \in \mathfrak{se}(3)$ 的偏导，左扰动模型)：

$$ \frac{\partial e}{\partial \xi} = -\begin{bmatrix} \frac{1}{Z^c} & 0 & -\frac{X^c}{(Z^c)^2} & -\frac{X^c Y^c}{(Z^c)^2} & 1 + \frac{(X^c)^2}{(Z^c)^2} & -\frac{Y^c}{Z^c} \ 0 & \frac{1}{Z^c} & -\frac{Y^c}{(Z^c)^2} & -1 - \frac{(Y^c)^2}{(Z^c)^2} & \frac{X^c Y^c}{(Z^c)^2} & \frac{X^c}{Z^c} \end{bmatrix} $$

(推导见视觉 SLAM 十四讲 §7.7.3；书中残差含内参 $f_x, f_y$，此处残差定义在归一化平面 ($f_x = f_y = 1$)，故雅可比中无 $f_x, f_y$ 因子)

GN 迭代：$J^T J \Delta \xi = -J^T e$，更新 $T \leftarrow \exp(\Delta \xi^\wedge) T$。

实务中用 LM (Levenberg-Marquardt) 替代 GN，加阻尼项 $J^T J + \lambda I$ 避免 $J^T J$ 奇异。

7. 实际工程选择

场景 推荐
几乎无外点 + 想要高精度 直接 EPnP + GN 精化
有外点 (任意比例) RANSAC (P3P 当作最小解算器) + EPnP/GN 在内点上精化
标定板 / 平面场景 OpenCV SOLVEPNP_IPPE 或 SOLVEPNP_IPPE_SQUARE (针对平面)
实时性要求高 P3P 的快速变体 (AP3P, Kneip) + RANSAC

OpenCV 的 cv::solvePnP 默认是 SOLVEPNP_ITERATIVE (LM)。其他可选：

  • SOLVEPNP_EPNP — EPnP，O(n)
  • SOLVEPNP_P3P — 历史上为 Gao 2003，现行 OpenCV 4.x 已改用 Ding et al. CVPR 2023 (Revisiting the P3P Problem)
  • SOLVEPNP_AP3P — Ke & Roumeliotis 2017 的代数 P3P (Algebraic P3P)
  • SOLVEPNP_DLS — Direct Least-Squares (Hesch 2011)
  • SOLVEPNP_UPNP — Unified PnP (Penate-Sanchez 2013)

抗外点用 cv::solvePnPRansac (内部 P3P + RANSAC + LM 精化)。

8. 速查对比

算法 最少点 解集 复杂度 精度 退化
DLT 6 1 (SVD) $O(N)$ 中 (松弛 SO(3) 约束) 标定板共面退化
P3P 3 4 (需第 4 点 disambiguation) $O(1)$ 低 (只用 3 点) 三点共线
EPnP 4 (理论)/6+ (推荐) 1 $O(N)$ 高 4 控制点配置不当
GN/LM 任意 ≥ 3 局部最优 $O(N \cdot \text{iter})$ 最高 (依赖初值) 初值远 → 局部极小

References