PNP 算法：从三维到二维的相机姿态估计技术(上)

Perspective-n-Point（PNP）算法作为计算机视觉中求解相机姿态的核心技术，通过已知三维空间点与其在图像中对应二维投影点的关系，计算相机的旋转矩阵与平移向量，实现从三维世界到二维图像的坐标映射。这一技术在相机定位、三维重建、增强现实等领域具有不可替代的作用，尤其在 SLAM（即时定位与地图构建）系统中，PNP 算法是前端视觉里程计估计相机位姿的关键步骤，直接影响后续地图构建与定位的精度。从早期依赖解析解的 P3P 算法，到如今结合非线性优化的鲁棒方法，PNP 算法的演进始终围绕 “精度提升” 与 “鲁棒性增强” 两大目标，其成果已广泛支撑起从手机 AR 到自动驾驶的多样化应用需求。本文将系统阐述 PNP 算法的基本原理、求解方法、应用场景及技术挑战，揭示其在三维坐标映射中的核心价值。

PNP 算法的核心原理建立在透视投影模型之上，其数学本质是求解相机外参以满足三维点到二维投影的几何约束。在针孔相机模型中，空间中一点 P 的三维坐标（X,Y,Z）与其图像投影点 p 的像素坐标（u,v）存在严格的映射关系：通过相机内参矩阵 K（包含焦距、主点坐标）与外参矩阵（旋转矩阵 R、平移向量 t），可表示为 p = K (RP + t)。当已知 n 个三维点及其对应图像点时，PNP 算法的目标便是求解 R 与 t，使上述投影关系的重投影误差最小化。这一过程可转化为非线性优化问题，即最小化所有点的像素距离平方和：Σ||p_i - K (RP_i + t)||²，其中 i 为点的索引。然而，直接求解这一非线性方程往往存在多解或数值不稳定问题，因此需通过解析方法获取初始解，再经迭代优化提升精度，这种 “解析 + 优化” 的框架成为主流 PNP 算法的共同范式。

PNP 算法的求解方法根据点数与数学模型的差异可分为多个分支，各具适用场景与精度特性。P3P 算法作为最经典的解析方法，仅需 3 对对应点即可求解相机姿态，其通过构建三角形相似关系推导出旋转矩阵的解析解，计算效率极高（毫秒级），适用于实时性要求高的场景。但 P3P 存在固有局限：当 3 点构成的三角形与相机光心构成特定几何关系时，可能产生多达 4 个解，需引入第 4 点验证以确定唯一解；且对噪声敏感，在图像点定位误差超过 1 像素时，姿态估计误差会显著增大。为突破点数限制，EPnP（Efficient PNP）算法通过将三维点表示为 4 个虚拟控制点的线性组合，将问题转化为求解控制点的图像投影，从而实现任意 n≥4 点的姿态估计，其计算复杂度与点数无关，在 SLAM 系统中（如 ORB-SLAM）被广泛用于实时位姿求解，重投影误差可控制在 0.5 像素以内。