2D-3D投影变换（PnP算法）

PnP算法详解

概述

PnP（Perspective-n-Point）算法的核心目标是通过已知3D点和对应的2D图像点，计算相机的旋转矩阵（R）和平移向量（t），从而确定相机相对于3D点的位姿。

基本概念

旋转矩阵（R）：描述相机的姿态，3×3维度，用于表示3D世界坐标系到相机坐标系的旋转关系
平移向量（t）：描述相机的位置，3×1维度，用于表示3D世界坐标系原点到相机坐标系原点的平移关系

输入数据要求

计算PnP需要准备以下3组核心数据：

1. 3D点坐标

世界坐标系下的点：\(P_i = (X_i, Y_i, Z_i)^T\)，至少需要4个非共线的点（P3P需3个点）

2. 2D图像点坐标

3D点在相机图像平面上的投影：\(p_i = (u_i, v_i)^T\)，需与3D点一一对应

3. 相机内参矩阵

由相机自身参数决定，通过相机标定获得：

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

其中：

\(f_x, f_y\)：相机在x、y轴方向的焦距
\(c_x, c_y\)：图像坐标系的主点坐标（通常在图像中心）

PnP算法类型

1. P3P算法

适用场景：仅有3个不共线3D点的情况

原理：通过三角几何关系推导位姿，但可能存在多解（最多4个），需要额外点验证

优点：

计算速度快
对点数量要求最低

缺点：

存在多解问题
对噪声敏感
鲁棒性较差

2. EPnP算法（Efficient PnP）

适用场景：4个及以上3D点的情况

原理：将3D点分解为"虚拟控制点"，通过控制点求解位姿

优点：

鲁棒性强
计算效率高
解唯一

缺点：

需要至少4个点

3. DLT算法（Direct Linear Transform）

适用场景：点数量较多的情况

原理：通过直接线性变换求解相机的投影矩阵，再分解出R和t

优点：

实现简单
适合大量点的场景

缺点：

没有考虑旋转矩阵的正交约束
对噪声敏感

4. RANSAC PnP算法

适用场景：存在错误匹配点对（外点）的实际工程应用

详细原理和工程应用将在下一章节详细讨论

坐标变换与投影模型

坐标系转换

3D点从世界坐标系到图像坐标系的完整转换过程：

步骤1：世界坐标系→相机坐标系

通过旋转\(\mathbf{R}\)和平移\(\mathbf{t}\)变换：

\[\mathbf{P}'_i = \mathbf{R} \cdot \mathbf{P}_i + \mathbf{t} \]

其中\(\mathbf{P}'_i = (X'_i, Y'_i, Z'_i)^T\)是3D点在相机坐标系下的坐标

步骤2：相机坐标系→图像坐标系

通过相机内参\(\mathbf{K}\)进行透视变换：

\[\begin{bmatrix} \hat{u}_i \\ \hat{v}_i \\ 1 \end{bmatrix} = \frac{1}{Z'_i} \cdot \mathbf{K} \cdot \begin{bmatrix} X'_i \\ Y'_i \\ Z'_i \end{bmatrix} \]

简化为分量形式：

\[\hat{u}_i = f_x \cdot \frac{X'_i}{Z'_i} + c_x, \quad \hat{v}_i = f_y \cdot \frac{Y'_i}{Z'_i} + c_y \]

坐标变换计算实例

示例场景

假设一个简单的PnP场景，包含以下数据：

已知条件：

相机内参：\(f_x = 800\), \(f_y = 800\), \(c_x = 320\), \(c_y = 240\)
真实位姿：相机绕Y轴旋转30°，平移\(t = [0, 0, 5]^T\)
世界坐标系中的3D点：
- \(P_1 = (1, 0, 0)^T\) (前方1米)
- \(P_2 = (0, 1, 0)^T\) (左侧1米)
- \(P_3 = (0, 0, 1)^T\) (上方1米)

步骤1：构建旋转矩阵

绕Y轴旋转30°的旋转矩阵：

\[\mathbf{R} = \begin{bmatrix} \cos 30° & 0 & \sin 30° \\ 0 & 1 & 0 \\ -\sin 30° & 0 & \cos 30° \end{bmatrix} = \begin{bmatrix} 0.866 & 0 & 0.5 \\ 0 & 1 & 0 \\ -0.5 & 0 & 0.866 \end{bmatrix}\]

步骤2：坐标变换计算

对于点\(P_1 = (1, 0, 0)^T\)：

世界坐标系→相机坐标系：

\[\mathbf{P}'_1 = \mathbf{R} \cdot P_1 + \mathbf{t} = \begin{bmatrix} 0.866 & 0 & 0.5 \\ 0 & 1 & 0 \\ -0.5 & 0 & 0.866 \end{bmatrix} \begin{bmatrix} 1 \\ 0 \\ 0 \end{bmatrix} + \begin{bmatrix} 0 \\ 0 \\ 5 \end{bmatrix} = \begin{bmatrix} 0.866 \\ 0 \\ -0.5 \end{bmatrix} + \begin{bmatrix} 0 \\ 0 \\ 5 \end{bmatrix} = \begin{bmatrix} 0.866 \\ 0 \\ 4.5 \end{bmatrix}\]
相机坐标系→图像坐标系：

\[\hat{u}_1 = 800 \cdot \frac{0.866}{4.5} + 320 = 800 \cdot 0.192 + 320 = 153.6 + 320 = 473.6 \]
\[\hat{v}_1 = 800 \cdot \frac{0}{4.5} + 240 = 0 + 240 = 240 \]
预测图像点：\(\hat{p}_1 = (473.6, 240)\)

对于点\(P_2 = (0, 1, 0)^T\)：

世界坐标系→相机坐标系：

\[\mathbf{P}'_2 = \mathbf{R} \cdot P_2 + \mathbf{t} = \begin{bmatrix} 0.866 & 0 & 0.5 \\ 0 & 1 & 0 \\ -0.5 & 0 & 0.866 \end{bmatrix} \begin{bmatrix} 0 \\ 1 \\ 0 \end{bmatrix} + \begin{bmatrix} 0 \\ 0 \\ 5 \end{bmatrix} = \begin{bmatrix} 0 \\ 1 \\ 0 \end{bmatrix} + \begin{bmatrix} 0 \\ 0 \\ 5 \end{bmatrix} = \begin{bmatrix} 0 \\ 1 \\ 5 \end{bmatrix}\]
相机坐标系→图像坐标系：

\[\hat{u}_2 = 800 \cdot \frac{0}{5} + 320 = 0 + 320 = 320 \]
\[\hat{v}_2 = 800 \cdot \frac{1}{5} + 240 = 800 \cdot 0.2 + 240 = 160 + 240 = 400 \]
预测图像点：\(\hat{p}_2 = (320, 400)\)

对于点\(P_3 = (0, 0, 1)^T\)：

世界坐标系→相机坐标系：

\[\mathbf{P}'_3 = \mathbf{R} \cdot P_3 + \mathbf{t} = \begin{bmatrix} 0.866 & 0 & 0.5 \\ 0 & 1 & 0 \\ -0.5 & 0 & 0.866 \end{bmatrix} \begin{bmatrix} 0 \\ 0 \\ 1 \end{bmatrix} + \begin{bmatrix} 0 \\ 0 \\ 5 \end{bmatrix} = \begin{bmatrix} 0.5 \\ 0 \\ 0.866 \end{bmatrix} + \begin{bmatrix} 0 \\ 0 \\ 5 \end{bmatrix} = \begin{bmatrix} 0.5 \\ 0 \\ 5.866 \end{bmatrix}\]
相机坐标系→图像坐标系：

\[\hat{u}_3 = 800 \cdot \frac{0.5}{5.866} + 320 = 800 \cdot 0.0852 + 320 = 68.2 + 320 = 388.2 \]
\[\hat{v}_3 = 800 \cdot \frac{0}{5.866} + 240 = 0 + 240 = 240 \]
预测图像点：\(\hat{p}_3 = (388.2, 240)\)

重投影误差计算实例

实际观测与预测对比

假设实际检测到的图像点（含有一些噪声）为：

实际点：\(p_1 = (475, 242)\)
实际点：\(p_2 = (318, 398)\)
实际点：\(p_3 = (390, 238)\)

重投影误差计算（欧式距离）

点1的误差：

\[e_1 = \sqrt{(473.6 - 475)^2 + (240 - 242)^2} = \sqrt{(-1.4)^2 + (-2)^2} = \sqrt{1.96 + 4} = \sqrt{5.96} = 2.44 \text{像素} \]

点2的误差：

\[e_2 = \sqrt{(320 - 318)^2 + (400 - 398)^2} = \sqrt{2^2 + 2^2} = \sqrt{4 + 4} = \sqrt{8} = 2.83 \text{像素} \]

点3的误差：

\[e_3 = \sqrt{(388.2 - 390)^2 + (240 - 238)^2} = \sqrt{(-1.8)^2 + 2^2} = \sqrt{3.24 + 4} = \sqrt{7.24} = 2.69 \text{像素} \]

总体误差评估

总误差平方和：

\[\sum_{i=1}^3 e_i^2 = 2.44^2 + 2.83^2 + 2.69^2 = 5.95 + 8.01 + 7.24 = 21.2 \]

均方根误差（RMSE）：

\[\text{RMSE} = \sqrt{\frac{1}{3} \times 21.2} = \sqrt{7.07} = 2.66 \text{像素} \]

结果分析

平均重投影误差约为2.7像素，属于可接受范围
点2误差最大，可能是特征检测精度问题
总体拟合效果良好，说明位姿估计准确

PnP求解过程演示

反向求解位姿

现在假设我们只知道3D点和对应的2D观测点，要求解相机位姿：

输入数据：

3D点：\(P_1 = (1, 0, 0)^T\), \(P_2 = (0, 1, 0)^T\), \(P_3 = (0, 0, 1)^T\)
观测2D点：\(p_1 = (475, 242)\), \(p_2 = (318, 398)\), \(p_3 = (390, 238)\)
相机内参：同上

RANSAC PnP求解过程：

随机采样：从3个点中选择所有3个点（最小子集）
EPnP求解：计算初始位姿估计
一致性验证：计算所有点的重投影误差
结果：得到最优位姿估计
- 旋转向量：\(\mathbf{r} = (0, 0.524, 0)\)（约30°）
- 平移向量：\(\mathbf{t} = (0, 0, 5)\)

验证：将求解的位姿用于重新投影，验证重投影误差是否最小。

重投影误差与优化

重投影误差定义

单点重投影误差：\(e_i = \sqrt{(\hat{u}_i - u_i)^2 + (\hat{v}_i - v_i)^2}\)

优化目标

最小化所有点的总重投影误差：

\[\min_{\mathbf{R}, \mathbf{t}} \sum_{i=1}^n e_i^2 \]

精度评估

使用均方根误差（RMSE）评估求解精度：

\[\text{RMSE} = \sqrt{\frac{1}{n} \sum_{i=1}^n e_i^2} \]

误差来源分析

特征点检测误差：角点提取精度通常为0.1-1像素
相机标定误差：内参矩阵的不准确性
镜头畸变：未完全校正的径向和切向畸变
数值计算误差：浮点运算精度限制
运动模糊：相机运动或物体运动造成的图像模糊

误差优化策略

亚像素精度：使用亚像素角点检测算法
多帧融合：利用时间序列信息提高精度
非线性优化：使用Levenberg-Marquardt算法进行Bundle Adjustment
外点剔除：通过RANSAC等鲁棒方法去除异常点

RANSAC PnP算法详解（工程应用重点）

算法原理

RANSAC（Random Sample Consensus，随机采样一致性）是一种用于处理含有外点（outliers）数据的鲁棒估计算法。在PnP问题中，RANSAC用于处理错误的3D-2D点对匹配。

数学基础

外点概率模型：

假设数据集中外点的比例为\(\epsilon\)
单次采样得到纯内点集的概率为\((1-\epsilon)^m\)，其中\(m\)为最小采样点数

迭代次数计算：

\[N = \frac{\log(1-p)}{\log(1-(1-\epsilon)^m)} \]

其中：

\(N\)：需要的迭代次数
\(p\)：期望的成功概率（通常取0.99或0.995）
\(\epsilon\)：估计的外点比例
\(m\)：最小采样点数（PnP中为4）

详细算法流程

1. 随机采样阶段

从所有\(n\)个3D-2D点对中随机选择\(m=4\)个非共线点作为最小子集

2. 模型拟合阶段

使用选中的4个点计算初始位姿\((\mathbf{R}, \mathbf{t})\)：

通常使用EPnP算法进行快速求解
验证旋转矩阵的正交性：\(\mathbf{R}^T\mathbf{R} = \mathbf{I}\)

3. 一致性验证阶段

计算所有\(n\)个点对的重投影误差：

\[e_i = \sqrt{(\hat{u}_i - u_i)^2 + (\hat{v}_i - v_i)^2} \]

将误差小于阈值\(\tau\)的点标记为内点：

\[\text{inlier}_i = \begin{cases} 1 & \text{if } e_i < \tau \\ 0 & \text{otherwise} \end{cases}\]

4. 最优模型选择

统计内点数量，保留内点最多的模型作为候选

5. 全局优化

使用所有内点重新优化位姿，通常采用Levenberg-Marquardt算法最小化总重投影误差

工程应用要点

参数设置策略

迭代次数选择：

// OpenCV典型参数设置
int iterationsCount = 300;      // 最大迭代次数
float reprojectionError = 2.0;   // 重投影误差阈值（像素）
double confidence = 0.99;       // 期望置信度

阈值设定指导原则：

重投影误差阈值：通常设为1-3像素
- 高精度场景：1.0像素
- 一般场景：2.0像素
- 噪声较大场景：3.0像素

外点比例估计：

特征匹配场景：外点比例通常为20-50%
SLAM回环检测：外点比例可能高达60-80%
标定场景：外点比例通常较低，约5-15%

性能优化技巧

早期终止策略：

当内点数量超过设定比例时提前终止
动态调整迭代次数：\(N_{adaptive} = \min(N_{max}, N_{theoretical})\)

预处理优化：

使用归一化坐标减少数值误差
预先剔除明显异常的点对
使用分层RANSAC提高效率

计算复杂度分析

时间复杂度：\(O(N \cdot m \cdot n)\)

\(N\)：迭代次数
\(m\)：最小采样点数（固定为4）
\(n\)：总点数

内存复杂度：\(O(n)\)

适用场景分析

高适用性场景

视觉SLAM：处理动态环境中的外点
AR应用：快速鲁棒位姿估计
机器人导航：传感器噪声处理

不适用场景

外点比例过高（>80%）的情况
实时性要求极高的嵌入式系统
点数极少（<4个）的场景

与其他算法对比

算法	外点处理能力	计算速度	精度	适用场景
RANSAC PnP	优秀	中等	高	含外点的实际应用
EPnP	无	快	中	理想数据
DLT	无	快	低	大量点数据
P3P	无	最快	低	最少点要求

代码实现示例

// RANSAC PnP典型实现
cv::Mat rvec, tvec;
cv::Mat inliers;

// 设置RANSAC参数
int iterationsCount = 300;
float reprojectionError = 2.0;
double confidence = 0.99;

// 执行RANSAC PnP
bool success = cv::solvePnPRansac(
    objectPoints,           // 3D点
    imagePoints,            // 2D点
    cameraMatrix,           // 相机内参
    distCoeffs,             // 畸变系数
    rvec, tvec,             // 输出位姿
    false,                  // 使用初始估计
    iterationsCount,        // 最大迭代次数
    reprojectionError,      // 重投影误差阈值
    confidence,             // 置信度
    inliers,                // 内点标记
    cv::SOLVEPNP_EPNP       // 内部使用EPnP
);

// 评估结果
double inlier_ratio = (double)inliers.total() / objectPoints.size();
std::cout << "Inlier ratio: " << inlier_ratio << std::endl;

实际应用案例分析

案例1：AR标记检测

场景描述：

检测平面AR标记的6个角点
相机分辨率：640×480
标记尺寸：10cm×10cm

参数设置：

int iterationsCount = 100;
float reprojectionError = 1.5;
double confidence = 0.99;

典型结果：

内点比例：85-95%
RMSE：< 1像素
处理时间：5-15ms

案例2：机器人导航