1. 课程导入

在机器人、无人机、AR/VR、工业检测与自动驾驶等领域，系统往往需要回答一个看似简单却极其关键的问题：“相机当前在哪里，朝向何方？”如果相机能识别出若干已知空间坐标的三维点，并在图像中找到它们对应的二维像素点，那么仅凭这些“3D—2D”对应关系，就可以反推出相机的位姿。这正是 PnP（Perspective-n-Point）问题要解决的核心任务。

PnP 视觉定位，是指已知 n 个空间三维点及其在图像中的二维投影点，求解相机相对于这些三维点所在坐标系的旋转与平移。它的输出通常是一个 6 自由度位姿：3 个旋转量（可用旋转矩阵 R 或四元数表示）和 3 个平移量（平移向量 t）。与直接使用深度相机或双目三角化不同，PnP 允许在单目条件下完成定位，因此成本低、部署广，是视觉定位工程中的基础算法之一。

核心概念与数学表达

设世界坐标系下某三维点为 P_i = (X_i, Y_i, Z_i)，其在图像中的像素坐标为 p_i = (u_i, v_i)。相机成像模型可写为：

s * [u_i, v_i, 1]^T = K * [R | t] * [X_i, Y_i, Z_i, 1]^T

其中：
K  为相机内参矩阵（焦距、主点、像素尺度）
R  为旋转矩阵（3×3）
t  为平移向量（3×1）
s  为尺度因子

PnP 的目标就是在已知 K 和若干组 (P_i, p_i) 的情况下，估计 R 与 t。根据点数 n 的不同，问题可分为 P3P、P4P、P5P 以及通用的 PnP。实际工程中，常配合 RANSAC 处理误匹配，再用非线性优化（如 Levenberg-Marquardt）最小化重投影误差，得到高精度位姿。

整体知识框架

本课程围绕“从理论到工程落地”的主线展开，整体知识框架可以概括为五个层次：

• 基础层：相机模型、坐标系变换、内参外参、齐次坐标与投影几何。
• 算法层：P3P、EPnP、DLS、迭代最近点式优化、RANSAC 鲁棒估计。
• 数据层：2D—3D 对应点获取，包括特征匹配、已知标记点、深度图反投影、语义关键点。
• 工程层：标定、时间同步、坐标系统一、异常值剔除、实时性与精度权衡。
• 应用层：机器人抓取、无人机降落、AR 跟踪、视觉伺服、SLAM 重定位。

下图展示了 PnP 视觉定位从输入到输出的核心数据流与模块关系。左侧是输入数据，中间是求解与优化过程，右侧是最终位姿输出，下方则给出典型应用场景。

方法流程：从对应点到稳定位姿

工程中一个完整的 PnP 定位流程通常包含以下步骤：

1. 数据准备：获取图像，提取特征点或检测已知标记，建立 2D—3D 对应关系。
2. 内参标定：提前标定相机内参 K 与畸变系数，必要时对图像去畸变。
3. 鲁棒求解：使用 P3P/EPnP 求初值，结合 RANSAC 剔除误匹配。
4. 非线性优化：以重投影误差为目标函数，迭代优化 R 与 t。
5. 结果评估：检查内点数量、重投影误差、位姿协方差，判断是否可信。
6. 坐标转换：将相机坐标系位姿转换到机器人/世界坐标系，供下游使用。

典型应用场景与参数关注点

不同应用对 PnP 的侧重点不同。下表给出常见场景与关键参数的关系，便于在工程中快速定位优化方向。

常见误区与复盘建议