4. PnP问题数学描述

PnP（Perspective-n-Point）是视觉定位中最核心的几何问题之一。它的输入非常明确：一组已知世界坐标系下的三维点，以及这些点在图像上对应的二维像素坐标；它的输出则是相机相对于世界坐标系的位姿，即旋转矩阵 R 与平移向量 t。换句话说，PnP 要回答的是：当我已经知道若干空间点和它们在图像中的投影位置时，相机究竟站在哪里、朝哪个方向看？

这一节不急于推导求解算法，而是先把 PnP 的数学描述讲清楚：未知量有哪些、约束如何建立、问题如何分类。只有把问题定义清楚，后续的 DLT、P3P、EPnP、迭代优化等方法才有统一的落脚点。

一、核心原理：投影方程是唯一约束来源

PnP 的数学基础是针孔相机模型。设世界坐标系下第 i 个三维点为 P_i = [X_i, Y_i, Z_i]^T，其对应的图像齐次坐标为 p_i = [u_i, v_i, 1]^T。相机内参矩阵记为 K，外参为旋转 R 和平移 t。则投影关系可写为：

s_i * [u_i, v_i, 1]^T = K * (R * P_i + t)

其中：
K = [fx  0  cx]
    [0  fy  cy]
    [0   0   1]

R ∈ SO(3)，t ∈ R^3，s_i 为深度尺度因子

将 K 展开后，每个三维点会给出两个独立方程。若共有 n 个点，则得到 2n 个标量方程。这些方程共同约束相机位姿，使 PnP 成为一个非线性、带几何约束的估计问题。

二、未知量分析：6 自由度与尺度问题

相机外参包含旋转和平移。旋转矩阵 R 有 9 个元素，但受正交性和行列式为 1 的约束，实际只有 3 个自由度；平移向量 t 有 3 个自由度。因此，PnP 的未知量总数为 6 个自由度。

如果相机内参 K 未知，问题会升级为内参和外参联合估计，未知量增加到 9 个甚至更多。但在经典 PnP 中，通常假设 K 已通过标定获得，因此只求解 6 自由度位姿。

另一个容易忽略的问题是尺度。单目相机无法仅凭图像恢复绝对尺度，因此 PnP 求解出的 t 通常与三维点的世界尺度一致；如果三维点本身带有真实尺度，则 t 也具有真实尺度。若三维点尺度未知，则只能得到相对位姿。

三、约束条件与问题分类

PnP 的约束来自投影方程，但不同点数、不同配置会带来不同的求解性质。按点数分类是最常见的分类方式：

类型	点数 n	方程数 2n	求解特点	典型方法
P1P	1	2	严重欠约束，无法确定 6 自由度位姿	不可解
P2P	2	4	仍欠约束，通常有无穷多解	不可解
P3P	3	6	恰好 6 方程，最多 4 组解	P3P + 第四点验证
P4P	4	8	超定，通常唯一解	DLT、EPnP
PnP（n≥4）	n	2n	超定，需最小化重投影误差	EPnP、迭代优化

从表中可以看出，n=3 是 PnP 能否求解的临界点。P3P 虽然方程数刚好等于未知量数，但由于非线性，可能存在多解，因此实际中常配合第四个点进行验证。当 n≥4 时，方程数超过未知量数，问题变为超定，通常通过最小化重投影误差求解。

四、案例场景：从二维码定位理解 PnP

假设一个二维码的四个角点在二维码坐标系下的三维坐标已知，例如 (0,0,0)、(1,0,0)、(1,1,0)、(0,1,0)。相机拍摄二维码后，在图像中检测到这四个角点的像素坐标。此时，四个三维点与四个二维点构成 P4P 问题。通过 PnP 求解，即可得到相机相对于二维码坐标系的位姿。

如果二维码贴在机器人末端，那么求解出的位姿就可以用于手眼标定、抓取定位或视觉伺服。这个场景中，三维点来自已知模型，二维点来自图像检测，PnP 负责把两者对齐。

五、误区与复盘

• 误区一：认为 PnP 只需要 3 个点就足够。P3P 虽然可解，但存在多解，且对噪声敏感。工程中通常使用 4 个以上点，并通过 RANSAC 剔除误匹配。
• 误区二：忽略内参误差。PnP 假设 K 已知且准确。若内参标定不准，求解出的位姿会系统性偏移。
• 误区三：把 PnP 当成线性问题。投影方程中 R 与 t 耦合，且存在尺度因子，本质是非线性问题。DLT 只是线性化近似，最终仍需非线性优化。
• 误区四：不检查重投影误差。求解完成后，应将三维点重新投影到图像，计算与观测点的误差。误差过大说明匹配错误或位姿不可信。

总结来看，PnP 的数学描述可以浓缩为一句话：在已知内参和若干 3D-2D 对应点的条件下，利用投影方程约束，求解相机的 6 自由度外参。它的未知量是 R 和 t，约束来自每个点提供的两个投影方程，问题按点数分为欠约束、临界和超定三类。理解这一层，后续的求解方法就不再是零散公式，而是同一问题在不同条件下的具体解法。