Contents — find the section you need

假设相机拍摄的图像在地图上对应着若干已知的三维点。确定相机的放置位置和指向方向的问题就是 PnP（透视 n 点）算法。它广泛应用于视觉 SLAM 地图跟踪、AR 中的虚拟物体叠加、机器人手眼标定以及测绘相机的姿态估计。

0. 30 秒概要

  • 输入包括相机内参矩阵 K、已知的三维点 \mathbf X_i 及其对应的图像点 \mathbf u_i。输出包括旋转 R 和平移 t。

  • 该算法旨在最小化投影方程 \mathbf u_i\sim K(R\mathbf X_i+t) 的重投影误差。对于 3 个点，P3P 算法可以提供候选解；对于 4 个或更多点，冗余信息可以帮助检测异常值。 - EPnP 将每个点表示为 4 个虚拟控制点的线性组合，从而快速求解大量点。最后，使用诸如 Levenberg-Marquardt 算法之类的非线性优化方法来细化结果。

  • 如果异常值混入对应关系中，整个位姿估计可能会崩溃，因此需要使用 RANSAC-PnP、正深度检查和帧间一致性进行验证。

  • 当点几乎共面、视差较小、内参错误或场景中存在卷帘快门或动态物体时，退化和发散现象很常见。

1. 投影模型

Diagram 1 · Use the button to switch views
PnP流程，利用位姿假设投影已知的3D点，并通过RANSAC和非线性细化，利用残差更新位姿

图1 — 匹配的ID定义了3D-2D对应关系。PnP形成位姿假设，通过重投影残差剔除异常值，并细化R,t，后者将世界坐标映射到相机坐标。

设相机坐标系中的一个点为\mathbf X_c=R\mathbf X_w+t。在针孔模型中，归一化图像坐标为

x=\frac{X_c}{Z_c},\qquad y=\frac{Y_c}{Z_c}

像素坐标通过内参矩阵获得

K=\begin{bmatrix}f_x&0&c_x\\0&f_y&c_y\\0&0&1\end{bmatrix}

\mathbf u\sim K\mathbf X_c 和 R\in SO(3) 分别表示旋转和位移。如果存在镜头畸变，则需要在投影前后进行畸变校正。

未知量为 6 个自由度，包括 3 个旋转自由度和 3 个位移自由度。给定 3D 点 \mathbf X_i 与观测值 \mathbf u_i 之间的对应关系 n，重投影误差

E(R,t)=\sum_{i=1}^{n}\rho\left(\left\|\mathbf u_i-\pi(K(R\mathbf X_i+t))\right\|^2\right)

被最小化。\pi 表示透视除法，\rho 表示鲁棒损失，例如 Huber 损失。

不要将变换与相机位置混淆

OpenCV 的 solvePnP 返回 rvec, tvec，表示将物体/世界坐标系中的点映射到相机坐标系的变换。要获取世界坐标系中的相机中心，请使用对于相机位姿，取反 \mathbf C_w=-R^Tt 即可得到 T_{cw}。将 tvec 本身视为相机的世界位置是一个常见错误。此外，如果输入图像点已经过畸变校正，则应避免重复进行畸变校正。

2. P3P、AP3P 和 EPnP

P3P（透视三点法）利用三个点的图像角度和三维点之间的距离来恢复到相机中心的距离，最多有 4 个解。通过与第四个点或地图的已知位姿进行比较来选择正确的解。AP3P 是一个快速变体，它以代数方式重新组织解。

当存在大量点时，EPnP（高效点对点法）将每个三维点表示为 4 个虚拟控制点的加权和。

\mathbf X_i=\sum_{j=1}^{4}\alpha_{ij}\mathbf C_j,\qquad \sum_j\alpha_{ij}=1

控制点的相机坐标由线性方程组求解，并由此恢复旋转和平移。由于其计算成本几乎与点数呈线性关系，因此非常适合利用SLAM的众多地标构建初始位姿。初始解之后，使用Levenberg-Marquardt算法迭代地修正重投影误差。

3. RANSAC-PnP

特征点对应关系会与相似的模式、移动物体和错误的地图ID混杂在一起。标准方法是RANSAC：从最小点集构建一个暂定位姿，重投影每个对应关系，并统计有多少内点落在阈值范围内。给定异常值率\epsilon、最小样本量s和成功概率p，所需的迭代次数N由以下因素决定：

N\ge\frac{\log(1-p)}{\log(1-(1-\epsilon)^s)}

由于所需的迭代次数会随着异常值率的增加而急剧增长，因此需要预先使用特征点比率测试、基于网格的离散度或动态对象掩码来降低 \epsilon 的值。OpenCV 的 solvePnPRansac 需要显式指定点数、标志（EPNP、P3P、SQPNP 等）、重投影阈值和置信度。

4. 识别退化

共面点集

如果所有 3D 点都位于同一平面上，则 PnP 的深度和姿态信息会变得模糊，并且几何形状同样可以用单应性矩阵来解释。棋盘格标定特意使用平面，但您需要选择合适的视角和点布局，以充分约束姿态的自由度。正面观察单个 AR 标记时，深度信息变得不稳定也是同样的现象。

图像覆盖范围窄且距离远

PnP 直接使用单张图像。已知三维点，因此帧间视差本身并非必要输入。然而，当对应点仅占据图像的一小部分区域、目标距离较远且看起来很小，或者三维点的深度变化很小时，PnP 的条件数较差。不要仅凭内点计数就接受结果：检查图像覆盖率、重投影均方根误差 (RMSE) 以及位姿协方差或对扰动的敏感性，然后在需要时融合 IMU 或深度传感器的数据。

校准和时序

焦距、主点和畸变的误差会转化为每个点的系统性重投影误差。镜头的固有矩阵会随变焦、温度或对焦而变化，因此需要重新校准。在车辆和无人机中，如果卷帘快门的行时序与 IMU 不同步，PnP 将返回“弯曲”的相机位姿。

5. PnP 在视觉 SLAM 中的作用

在视觉 SLAM 中，随着先前三角测量的地图点数量的增加，可以逐帧跟踪相机位姿。使用 PnP 进行帧校正。在姿态固定的情况下，对新点进行三角测量，一旦积累了足够多的关键帧，捆绑调整 (Bundle Adjustment) 就会联合优化姿态和地图。最简单的理解方式是分工：PnP 是轻量级的前端，而捆绑调整负责全局一致性。

6. 实现清单

  1. 使用棋盘格或其他类似方法校准 K 和畸变，并记录重投影误差。

  2. 将 3D 点的单位（米/毫米）和坐标系与图像点的畸变校正状态对齐。

  3. 通过比率测试、最近邻和时间跟踪来缩小对应范围。

  4. 使用 RANSAC-PnP 去除异常值，并保存内点分布和重投影误差。

  5. 检查深度是否为正值，姿态变化是否符合物理规律，以及与前一帧的差异是否合理。

  6. 如果条件较差，则回退到其他方法。到 IMU、深度、单应性矩阵或重新初始化。

最小实现步骤

使用 OpenCV，首先从 solvePnPRansac 获取 rvec, tvec, inliers，仅将内点传递给 solvePnPRefineLM，最后使用 projectPoints 自行计算内点 RMSE 和误差分布。仅凭 API 返回成功无法检测到过多的不匹配、点簇或物理上不可能的位姿。

ok, rvec, tvec, inliers = cv2.solvePnPRansac(
    object_points, image_points, K, dist,
    flags=cv2.SOLVEPNP_EPNP,
    reprojectionError=3.0, confidence=0.999, iterationsCount=200,
)
if not ok or inliers is None or len(inliers) < 6:
    raise RuntimeError("PnP failed or has too few inliers")

idx = inliers.ravel()
rvec, tvec = cv2.solvePnPRefineLM(
    object_points[idx], image_points[idx], K, dist, rvec, tvec
)
projected, _ = cv2.projectPoints(object_points[idx], rvec, tvec, K, dist)
rmse = np.sqrt(np.mean(np.sum(
    (projected.reshape(-1, 2) - image_points[idx].reshape(-1, 2)) ** 2,
    axis=1,
)))
R, _ = cv2.Rodrigues(rvec)
camera_center_world = -R.T @ tvec.reshape(3, 1)

3.0 px 和六个内点都不是通用的接受阈值；它们只是本示例的初始值。阈值应根据图像分辨率、特征精度和应用程序允许的位姿误差来确定。此外，还要验证内点是否聚集在图像的某个角落，以及每个点是否具有相机帧深度 Z_c>0。

7. 总结

PnP 是连接将 3D 地图和 2D 图像之间的对应关系转换为具有 6 个自由度的相机位姿。使用 P3P/EPnP 构建初始解，使用 RANSAC 去除异常值，并使用非线性优化进行细化。只有同时管理点阵布局、标定、视差和时间同步，才能获得用于视觉 SLAM 或 AR 的稳定位姿估计。

检查你的理解
PnP 只需要两张图像吗？

它的基本输入是已知的 3D 点、它们在 2D 图像中的对应关系以及相机内参。这与从 2D 到 2D 的匹配中估计运动不同。

参考资料

What to read next

Review the background极线几何——从两幅图像中读取深度和相机运动Continue the series运动结构重建入门——从无序照片集中同时恢复3D结构和相机位置Explore another aspect of this field图像亮度 Lab — 曝光、伽马与裁剪
#PnP #Perspective-n-Point #Camera Pose #3D Reconstruction #RANSAC #OpenCV