PnP（Perspective-n-Point）算法 | 用于求解已知n个3D点及其对应2D投影点的相机位姿_pnp算法

什么是PnP算法？

• PnP 全称是 Perspective-n-Point，中文叫“n点透视问题”。
• 它的目标是：
  已知一些空间中已知3D点的位置（世界坐标）和它们对应的2D图像像素坐标，求解摄像机的姿态（位置和平移）和朝向（旋转）。

简单来说，就是通过3D点和它们在照片上的投影点，算出相机“站在哪儿”和“看向哪儿”。

PnP解决的问题

你有一堆现实世界的点（比如3D模型上的标志点），也知道这些点在照片里的像素位置，PnP就是帮你找出“相机相对于这些点的位置和角度”。

PnP算法的原理（简单版）

1. 已知条件：

   • $n$ 个已知3D点 Pi=(Xi,Yi,Zi)P_i = (X_i, Y_i, Z_i)Pi​=(Xi​,Yi​,Zi​)，在世界坐标系里。
   • 它们对应的2D像素点 pi=(ui,vi)p_i = (u_i, v_i)pi​=(ui​,vi​)，在图像中。

2. 目标：

   • 找到相机的旋转矩阵 $R$ 和平移向量 $t$，使得所有3D点经过变换后投影到图像上的位置，尽可能接近对应的2D点。

3. 数学关系：

   s[uivi1]=K×(R×Pi+t)s \\begin{bmatrix} u_i \\\\ v_i \\\\ 1 \\end{bmatrix} = K \\times \\big( R \\times P_i + t \\big)s​ui​vi​1​​=K×(R×Pi​+t)

   其中：

   • $K$ 是相机内参矩阵，已知。
   • $R,t$ 是未知（我们要解的）。
   • $s$ 是尺度因子（用于齐次坐标）。

4. 求解过程：

   • 利用多组点的对应关系，建立方程组。
   • 通过最小化投影误差（图像点和投影点的距离），用优化算法求出最佳的 $R$ 和 $t$。

你可以这样理解：

• PnP就像“反向投影”：你知道点在现实和照片的位置，求相机的位置和朝向。

额外说明：

• 如果点的数量较少（最少4个点），算法能工作，但更多点通常能得到更稳定准确的结果。
• 经典的PnP算法有很多变体，比如EPnP、UPnP、RPnP等，都旨在更快或更鲁棒地求解。

值得注意的是PnP的前提：对应关系必须先确定

• **PnP算法本身不负责找到哪些3D点对应哪些2D点，**它的输入是：

  • 一组3D点（已知位置）
  • 它们在图像中的对应2D点

• 所以，在用PnP之前，必须先解决“匹配”问题：
  怎样找到图像中哪个像素点对应哪个3D世界点？

对应关系的获得途径举例：

1. 人工标定/标记：
   在物体或场景上贴标记点，知道标记点的3D坐标，手动或者半自动找到图像中的对应点。

2. 特征匹配：
   利用特征点检测（如SIFT、ORB）在图像中提取关键点，并用描述子匹配它们与3D点（如用激光扫描得到的点云中提取特征点）。

3. 结构光或投影仪：
   通过主动投影图案产生已知对应关系。

4. 深度相机（RGB-D）或激光雷达数据：
   直接获得3D点和2D图像对齐。

没有对应关系，PnP算法就无从下手

所以在做定位、姿态估计、SLAM等任务时，建立稳定准确的“3D点 ↔ 2D点”对应关系是关键的第一步。