PnP算法介绍（Perspective-n-Point)

紫金修道2026-07-14 15:15

PnP（Perspective-n-Point，透视n点）算法是AprilTag系统的核心，负责将检测到的二维图像标记，转换为相机在三维空间中的精确位姿。简单来说，它解决了"已知若干个3D空间点及其在2D图像上的投影位置，如何确定相机在空间中的位置和朝向"这一问题。

前言

在计算机视觉领域，有一个经典问题：给你一张照片，照片里有一个已知三维形状的物体（比如一个方形二维码），你能算出拍照时相机相对于这个物体的位置和朝向吗？这个问题就是 PnP（Perspective-n-Point，透视n点）问题。

PnP算法在增强现实（AR）、机器人定位、SLAM（即时定位与地图构建）、无人机自主导航等领域都有着广泛的应用。当你用手机AR应用把虚拟物体"放置"在现实世界的桌面上时，背后正是PnP算法在默默地计算着手机与桌面之间的相对位姿。

⚙️ 核心原理：从2D到3D的坐标转换

PnP算法的本质是求解一个坐标系变换问题。在AprilTag应用中，已知信息包括：

  1. 3D空间点：AprilTag四个角点在其自身坐标系中的精确三维坐标。例如，对于一个边长为1的方形Tag，其角点坐标可定义为 \[0,0,0, 1,0,0, 1,1,0, 0,1,0]。
  2. 2D投影点：通过图像处理，算法检测到的这四个角点在图像像素坐标系中的二维坐标。
  3. 相机内参：相机的焦距、光心等参数，可通过相机标定获得。

算法利用这些对应关系，计算出一个旋转矩阵（R） 和平移向量（t），这个变换矩阵描述了AprilTag坐标系与相机坐标系之间的相对位姿。最终输出通常是一个6自由度的位姿估计，包含沿X、Y、Z轴的平移（位置）和绕这三个轴的旋转（朝向）。

一、PnP问题的数学描述

1.1 问题定义

PnP问题的核心可以这样描述：已知n个三维空间点在世界坐标系中的坐标，以及它们在图像平面上的二维投影点坐标，在相机内参已知的前提下，求解相机在世界坐标系中的位姿（旋转矩阵R和平移向量t）。

这里的"位姿"包含6个自由度：3个平移参数（沿X、Y、Z轴的位移x,y,z）和3个旋转参数（绕三个轴的旋转角度rx,ry,rz）。

1.2 投影方程------PnP的基石

PnP问题的数学基础是针孔相机模型。设三维空间点在世界坐标系下的坐标为 (P_i = (X_i, Y_i, Z_i))，其在图像平面上的投影点为 (p_i = (u_i, v_i))，则投影关系可以表示为：

K 是相机内参矩阵，包含焦距 (f_x, f_y) 和主点坐标 (c_x, c_y)；

R 是旋转矩阵（3×3正交矩阵），描述相机的朝向；

t 是平移向量（3×1），描述相机的位置；

u 是归一化比例因子，来源于透视投影的深度信息。

展开后可以得到两个方程（每个3D-2D点对提供两个约束）:

问题的本质：未知量是旋转矩阵R（9个参数，但有正交约束）和平移向量 t（3个参数），每个3D-2D点对提供2个方程。理论上最少需要3个点对（6个方程对应6个未知数），但3个点对会产生多解，实际应用中通常需要至少4个点对来获得唯一解。

PnP算法家族

PnP不是一个单一的算法，而是一类问题的统称。研究者们提出了多种求解方法，各有优劣。下面介绍几种最具代表性的算法。

2.1 DLT（直接线性变换法）

DLT 是最直观的求解思路------既然投影方程是线性的，那就直接解线性方程组。

将投影方程改写为矩阵形式 ，其中包含了旋转矩阵和平移向量的所有未知元素。每组3D-2D匹配点可以提供2个方程，而未知数一共有12个（旋转矩阵9个 + 平移向量3个），因此至少需要6组匹配点才能直接求解。

DLT的优点是简单直接，计算速度快。缺点也很明显：它只考虑了线性意义下的最优解，没有利用旋转矩阵的正交性约束，因此求出的结果往往需要进一步优化。此外，当点数较多时，DLT对噪声比较敏感。

python 复制代码 
import cv2
import numpy as np

# ---------- 准备数据（6个3D点及其2D投影） ----------
object_pts = np.array([
    [0.0, 0.0, 0.0],
    [1.0, 0.0, 0.0],
    [1.0, 1.0, 0.0],
    [0.0, 1.0, 0.0],
    [0.5, 0.5, 1.0],
    [0.2, 0.8, 0.5]
], dtype=np.float32)

image_pts = np.array([
    [100.0, 150.0],
    [250.0, 160.0],
    [240.0, 300.0],
    [90.0, 290.0],
    [180.0, 200.0],
    [150.0, 240.0]
], dtype=np.float32)

camera_matrix = np.array([
    [800.0, 0.0, 320.0],
    [0.0, 800.0, 240.0],
    [0.0, 0.0, 1.0]
], dtype=np.float32)

# ---------- DLT 求解函数 ----------
def dlt_pnp(object_pts, image_pts, camera_matrix):
    n = object_pts.shape[0]
    if n < 6:
        raise ValueError("DLT需要至少6个点")
    A = []
    for i in range(n):
        X, Y, Z = object_pts[i]
        u, v = image_pts[i]
        A.append([X, Y, Z, 1, 0, 0, 0, 0, -u*X, -u*Y, -u*Z, -u])
        A.append([0, 0, 0, 0, X, Y, Z, 1, -v*X, -v*Y, -v*Z, -v])
    A = np.array(A)
    _, _, Vt = np.linalg.svd(A)
    P = Vt[-1].reshape(3, 4)                     # 投影矩阵
    K_inv = np.linalg.inv(camera_matrix)
    Rt = K_inv @ P                               # [R|t] 但带有尺度
    R = Rt[:, :3]
    t = Rt[:, 3]
    # 尺度恢复：使 R 的 Frobenius 范数为 sqrt(3)
    s = np.sqrt(3) / np.linalg.norm(R, 'fro')
    R = s * R
    t = s * t
    # 强制旋转矩阵正交
    U, _, Vt2 = np.linalg.svd(R)
    R = U @ Vt2
    return R, t

# ---------- 调用 DLT ----------
R, t = dlt_pnp(object_pts, image_pts, camera_matrix)
rvec, _ = cv2.Rodrigues(R)   # 转为旋转向量便于显示
print("DLT 求解结果：")
print(f"旋转向量 rvec = {rvec.ravel()}")
print(f"平移向量 t    = {t.ravel()}")

2.2 P3P（透视三点法）

P3P 是PnP问题中最经典的方法之一，它利用3对3D-2D点来估计相机位姿。

P3P的核心思路属于"先求3D点在相机坐标系下的坐标，再求位姿"这一类方法。具体流程如下：

  1. 构建三角锥：已知3个三维点A、B、C在世界坐标系下的坐标，以及它们在图像上的投影点a、b、c。光心O与a、b、c构成一个三角锥。

  2. 计算夹角：利用相机内参，可以计算出a、b、c在相机坐标系下的坐标，进而得到 (\angle aOb)、(\angle aOc)、(\angle bOc)。

  3. 列余弦定理方程：在三角形AOB、AOC、BOC中，已知AB、AC、BC的长度（从世界坐标算出）和三个夹角，未知量是OA、OB、OC的长度。通过余弦定理可以建立方程组。

  4. 求解与验证：这个方程组最终可以化为一个一元四次方程，最多有4组解析解。因此需要第4个点来验证哪一组解是正确的。

P3P的优势在于利用了三角形的几何约束，比DLT更精确。局限性在于：3个点不能共线；存在多解问题；当点配置不佳时可能出现数值不稳定的"缺解"问题。

python 复制代码 
import cv2
import numpy as np

# ---------- 准备数据（4个点，前3个用于求解，第4个用于验证） ----------
object_pts = np.array([
    [0.0, 0.0, 0.0],
    [1.0, 0.0, 0.0],
    [1.0, 1.0, 0.0],
    [0.0, 1.0, 0.0]
], dtype=np.float32)

image_pts = np.array([
    [100.0, 150.0],
    [250.0, 160.0],
    [240.0, 300.0],
    [90.0, 290.0]
], dtype=np.float32)

camera_matrix = np.array([
    [800.0, 0.0, 320.0],
    [0.0, 800.0, 240.0],
    [0.0, 0.0, 1.0]
], dtype=np.float32)
dist_coeffs = np.zeros((4, 1))

# ---------- 调用 P3P ----------
ret, rvec, tvec = cv2.solvePnP(object_pts, image_pts, camera_matrix,
                               dist_coeffs, flags=cv2.SOLVEPNP_P3P)
if ret:
    print("P3P 求解成功：")
    print(f"旋转向量 rvec = {rvec.ravel()}")
    print(f"平移向量 tvec = {tvec.ravel()}")
else:
    print("P3P 求解失败")

2.3 EPnP（高效PnP）

EPnP 由Lepetit等人在2008年提出，是PnP算法发展史上的一个重要里程碑。它的核心优势是计算复杂度为O(n)，即随着点数n的增加，计算时间线性增长。

EPnP的核心思想

EPnP的巧妙之处在于：不直接求解n个3D点在相机坐标系下的坐标，而是引入4个虚拟的"控制点"。

具体步骤如下：

第一步：用4个控制点表示所有参考点

在世界坐标系中选取4个控制点 CjwC_j^wCjw（j=1,2,3,4j=1,2,3,4j=1,2,3,4），通常选择所有点的质心作为第一个控制点，另外三个通过对数据进行PCA（主成分分析）得到。于是，任意一个3D点 PiwP_i^wPiw 都可以表示为4个控制点的加权和：

其中权重 (\alpha_{ij}) 满足 (\sum_{j=1}^{4} \alpha_{ij} = 1)。

第二步：利用刚体变换的权重不变性

从世界坐标系到相机坐标系的变换是刚体变换（旋转+平移），不改变点的重心坐标。因此在相机坐标系下，同一个点可以表示为：

第三步：建立投影方程求解控制点

将上述表达式代入针孔相机投影方程，可以得到关于4个控制点在相机坐标系下坐标（共12个未知数）的线性方程组。通过求解这个方程组，就能得到4个控制点在相机坐标系下的坐标。

第四步：恢复位姿

得到控制点的相机坐标后，所有n个点的相机坐标也随之恢复。最后通过3D-3D的点云配准（如SVD分解）计算出旋转矩阵R和平移向量t。

EPnP的优势在于速度快、精度高，适用于所有n≥4的情况，并且能正确处理平面和非平面配置。它常被用来为迭代优化算法提供高质量的初始值。

python 复制代码 
import cv2
import numpy as np

# ---------- 准备数据（4个点即可，也可用更多） ----------
object_pts = np.array([
    [0.0, 0.0, 0.0],
    [1.0, 0.0, 0.0],
    [1.0, 1.0, 0.0],
    [0.0, 1.0, 0.0]
], dtype=np.float32)

image_pts = np.array([
    [100.0, 150.0],
    [250.0, 160.0],
    [240.0, 300.0],
    [90.0, 290.0]
], dtype=np.float32)

camera_matrix = np.array([
    [800.0, 0.0, 320.0],
    [0.0, 800.0, 240.0],
    [0.0, 0.0, 1.0]
], dtype=np.float32)
dist_coeffs = np.zeros((4, 1))

# ---------- 调用 EPnP ----------
ret, rvec, tvec = cv2.solvePnP(object_pts, image_pts, camera_matrix,
                               dist_coeffs, flags=cv2.SOLVEPNP_EPNP)
if ret:
    print("EPnP 求解成功：")
    print(f"旋转向量 rvec = {rvec.ravel()}")
    print(f"平移向量 tvec = {tvec.ravel()}")
else:
    print("EPnP 求解失败")

2.4 UPnP（未标定PnP）

UPnP 是EPnP的一个扩展版本，全称是Uncalibrated PnP

与EPnP最大的不同在于：UPnP在求解位姿的同时，还能估计相机的焦距，因此适用于相机内参未知的场景。

UPnP的问题建模方式与EPnP类似，都是通过控制点将问题降维，但在求解过程中将焦距也作为未知量一同优化。研究表明，UPnP在大焦距图像的姿态估计中表现尤其出色。

python 复制代码 
import cv2
import numpy as np

# ---------- 准备数据（至少6个点） ----------
object_pts = np.array([
    [0.0, 0.0, 0.0],
    [1.0, 0.0, 0.0],
    [1.0, 1.0, 0.0],
    [0.0, 1.0, 0.0],
    [0.5, 0.5, 1.0],
    [0.2, 0.8, 0.5]
], dtype=np.float32)

image_pts = np.array([
    [100.0, 150.0],
    [250.0, 160.0],
    [240.0, 300.0],
    [90.0, 290.0],
    [180.0, 200.0],
    [150.0, 240.0]
], dtype=np.float32)

camera_matrix = np.array([
    [800.0, 0.0, 320.0],
    [0.0, 800.0, 240.0],
    [0.0, 0.0, 1.0]
], dtype=np.float32)
dist_coeffs = np.zeros((4, 1))

# ---------- 调用 UPnP ----------
ret, rvec, tvec = cv2.solvePnP(object_pts, image_pts, camera_matrix,
                               dist_coeffs, flags=cv2.SOLVEPNP_UPNP)
if ret:
    print("UPnP 求解成功：")
    print(f"旋转向量 rvec = {rvec.ravel()}")
    print(f"平移向量 tvec = {tvec.ravel()}")
else:
    print("UPnP 求解失败")

2.5 IPPE（基于无穷小平面的位姿估计）

IPPE 是一种专门针对共面点设计的PnP算法。在AprilTag、ArUco等二维码标记检测场景中，所有角点都位于同一个平面上，IPPE正是为此而生。

IPPE通过单应性矩阵的雅可比矩阵来求解位姿。由于采用了解析方法（而非迭代），IPPE速度极快，同时精度非常高。对于方形标记（如AprilTag），OpenCV提供了专门的 SOLVEPNP_IPPE_SQUARE 标志，输入4个共面点即可快速求解。

值得注意的是，IPPE会返回2个可能的解，需要根据实际情况选择正确的一个。

python 复制代码 
import cv2
import numpy as np

# ---------- 准备数据（必须共面，所有点的 Z 为 0） ----------
object_pts = np.array([
    [0.0, 0.0, 0.0],
    [1.0, 0.0, 0.0],
    [1.0, 1.0, 0.0],
    [0.0, 1.0, 0.0]
], dtype=np.float32)

image_pts = np.array([
    [100.0, 150.0],
    [250.0, 160.0],
    [240.0, 300.0],
    [90.0, 290.0]
], dtype=np.float32)

camera_matrix = np.array([
    [800.0, 0.0, 320.0],
    [0.0, 800.0, 240.0],
    [0.0, 0.0, 1.0]
], dtype=np.float32)
dist_coeffs = np.zeros((4, 1))

# ---------- 调用 IPPE ----------
ret, rvec, tvec = cv2.solvePnP(object_pts, image_pts, camera_matrix,
                               dist_coeffs, flags=cv2.SOLVEPNP_IPPE)
if ret:
    print("IPPE 求解成功：")
    print(f"旋转向量 rvec = {rvec.ravel()}")
    print(f"平移向量 tvec = {tvec.ravel()}")
else:
    print("IPPE 求解失败")

PnP（Perspective-n-Point）要解的问题是：已知一组 3D 点 和它们在图像上对应的 2D 像素点，反求相机相对这些 3D 点的位姿（R, t）。

参数 含义

objp（objectPoints）

4 个角点在 marker 自身坐标系下的 3D 坐标（单位米），即模型坐标

imgp（imagePoints）

同样 4 个角点在图像上的 2D 像素坐标（ArUco 检测出来的、亚像素细化后的角点）

K（cameraMatrix）

相机内参矩阵 \[fx,0,cx,0,fy,cy,0,0,1]

dist（distCoeffs）

畸变系数 k1,k2,p1,p2,k3,...

flags

求解算法，这里选 SOLVEPNP_IPPE_SQUARE（方形平面专用）

三、算法对比与选择指南

算法 所需点数 适用场景 特点
DLT ≥6 通用 简单快速，但对噪声敏感
P3P 3（+1验证） 点数少 几何约束强，但有多解
EPnP ≥4 通用 O(n)复杂度，速度快精度高
UPnP ≥6 内参未知 同时估计焦距
IPPE ≥4（共面） 平面标记 极快，专为二维码设计

在实际应用中，没有哪一种算法是万能的。通常的做法是组合使用：先用EPnP或IPPE快速得到一个初始位姿，再用高斯-牛顿法或列文伯格-马夸尔特法进行迭代优化，最小化重投影误差。

四、总结

PnP算法是连接2D图像与3D世界的桥梁。从最朴素的DLT线性求解，到利用几何约束的P3P，再到高效优雅的EPnP、适应未标定场景的UPnP，以及专为平面标记设计的IPPE------每一种算法都在精度、速度、鲁棒性之间做着不同的权衡。

理解这些算法的原理，不仅有助于我们在实际项目中做出正确的技术选型，更能让我们在面对新的视觉定位问题时，知道从何处着手设计和优化。

相关推荐
froyoisle3 小时前
CSP 真题解析：[CSP-J 2019-T4] 加工零件
c++·算法·bfs·csp-j·算法竞赛·信息学·信奥赛
兰令水3 小时前
hot100【acm版】【2026.7.13打卡-java版本】
java·开发语言·数据结构·算法·leetcode·面试
Tisfy3 小时前
LeetCode 1291.顺次数：打表/枚举
算法·leetcode·题解·枚举·遍历
ysa0510303 小时前
【板子】拓扑排序
c++·算法·图论·板子
算法备案代理4 小时前
宽带自动连接保姆级教程
算法
罗超驿4 小时前
双指针算法详解：从入门到精通（Java版）
算法·leetcode·职场和发展
m0_377062924 小时前
电池电量计库伦算法计算公式
算法
patrickpdx4 小时前
高联预赛中的高斯函数问题
算法
tkevinjd4 小时前
力扣300-最长递增子序列
算法·leetcode·职场和发展·动态规划·贪心
热门推荐
01GitHub 镜像站点02AI科技热点日报 | 2026年07月01日03幻兽帕鲁 - 服务器管理员权限与 GM 命令完全指南042026年7月AI圈大地震：GPT-5.6被政府限制、Claude入驻Slack、Anthropic自研芯片05GPT-5.5 对比 GPT-5.6 Sol、Terra、Luna：官方性能数据与选型分析062026 国产 AI 大模型横评：DeepSeek、通义千问、Kimi、文心一言、星火、豆包谁更能打？07AI 编程 IDE 全景解析 2026：Agent 全面接管开发链路082026 年 AI 编程工具终极横评：Cursor vs Claude Code vs Copilot vs Windsurf092026 AI 编程工具终极实战指南：Cursor vs Claude Code vs Copilot，开发者该怎么选？102026 年 AI 大模型 & AI 编程工具实战全总结