3D 位姿估计基础：PnP、EPnP 与 ICP 算法原理

各位同学，欢迎来到第三章。前两章我们聊了目标检测的基础，但检测出物体还不够——机器人要抓取、无人机要降落、AR 要叠加虚拟物体，这些场景都需要知道物体在三维空间中的精确位置和朝向。说白了，就是要把 2D 图像里的检测框，映射到 3D 世界坐标系里去。

这一章，我们就来啃下 3D 位姿估计的三块硬骨头：PnP 问题、EPnP 算法和迭代最近点（ICP）算法。这三兄弟是视觉 SLAM 和机器人抓取的基石，搞懂了它们，你才算真正入了 3D 视觉的门。

核心认知： 2D 检测告诉你“是什么”，3D 位姿告诉你“在哪、怎么摆”。两者协同，才是完整的感知闭环。
3D 位姿估计核心方法 3D 位姿估计 PnP 问题 EPnP 算法 ICP 算法 2D-3D 对应点 最少 3 组点求解 O(n) 复杂度 4 个控制点加权 3D-3D 对应点 迭代优化位姿

3.1 PnP 问题：从 2D 到 3D 的桥梁

PnP，全称 Perspective-n-Point。它要解决什么问题呢？给你一组 3D 空间点（比如物体上的特征点），以及它们在相机图像上的 2D 投影位置，求相机相对于这组 3D 点的位姿（旋转 R 和平移 t）。

我当年第一次接触 PnP 时，觉得这简直像变魔术。你想想看，一张 2D 照片，怎么就能算出相机在三维空间里的位置？其实原理不复杂——利用的是透视投影模型和三角形相似性。

数学上，PnP 可以写成：

s * [u, v, 1]^T = K * [R | t] * [X, Y, Z, 1]^T

其中 (u,v) 是像素坐标，(X,Y,Z) 是世界坐标，K 是相机内参，s 是尺度因子。我们要解的就是 R 和 t。

我的经验： 实际项目中，PnP 对特征点匹配质量极其敏感。我曾经在弱纹理场景下用 ORB 特征做 PnP，结果位姿抖得像帕金森。后来换成 AKAZE 特征 + RANSAC 滤除外点，才稳定下来。

PnP 的求解方法有很多，经典的有：

  • P3P：只用 3 组点，解出 4 组候选解，需要第 4 组点验证。速度快但容易受噪声影响。
  • DLT（直接线性变换）：用 6 组点线性求解，简单粗暴，但精度一般。
  • EPnP：我们下一节重点讲，效率高且精度不错。
  • 非线性优化：比如 BA（Bundle Adjustment），精度最高但计算量大。
方法 最少点数 复杂度 精度 适用场景
P3P 3 O(1) 中 实时性要求高，点少
DLT 6 O(n) 低 快速初始化
EPnP 4 O(n) 高 通用场景，推荐首选
BA 任意 O(n²) 最高 离线优化，精度优先

3.2 EPnP 算法：高效且优雅的解法

EPnP（Efficient PnP）是 2008 年由 Lepetit 等人提出的。说实话，这算法让我眼前一亮——它把 PnP 问题的复杂度从 O(n³) 降到了 O(n)，而且精度还很高。

EPnP 的核心思想很巧妙：用 4 个虚拟控制点来代表所有 3D 点。每个 3D 点都可以表示为这 4 个控制点的加权和，权重是固定的。这样一来，未知数就从 n 个点的位姿，变成了 4 个控制点在相机坐标系下的坐标。

具体步骤是这样的：

  1. 选择 4 个控制点：通常选 3D 点集的质心和一个主成分方向上的点。
  2. 计算权重：每个 3D 点用 4 个控制点的重心坐标表示。
  3. 建立线性方程：利用投影关系，构建关于控制点坐标的线性方程组。
  4. 求解位姿：解出控制点在相机坐标系下的坐标，再恢复出 R 和 t。
// EPnP 核心伪代码
function solveEPnP(worldPoints, imagePoints, K):
    // 1. 选择 4 个控制点
    cw = selectControlPoints(worldPoints)
    
    // 2. 计算重心坐标权重
    alphas = computeBarycentricWeights(worldPoints, cw)
    
    // 3. 构建 M 矩阵 (2n x 12)
    M = buildM(imagePoints, alphas, K)
    
    // 4. 求解 M * v = 0
    [~, ~, V] = svd(M)
    v = V[:, end]
    
    // 5. 恢复控制点在相机坐标系下的坐标
    cc = reshape(v, [3, 4])
    
    // 6. 用 ICP 或 SVD 分解求 R, t
    [R, t] = alignPointSets(cw, cc)
    
    return R, t
避坑指南： 我曾经在 EPnP 实现中踩过一个坑——控制点选择不当会导致数值不稳定。建议控制点尽量覆盖 3D 点集的凸包，不要选得太集中。另外，当点集共面时，EPnP 会退化，需要特殊处理。

EPnP 的优势很明显：

  • 计算复杂度 O(n)，适合实时系统
  • 不需要迭代，直接线性求解
  • 对噪声有一定鲁棒性
  • 可以作为非线性优化的初始值

3.3 迭代最近点（ICP）算法：3D-3D 的精确配准

如果说 PnP 解决的是 2D-3D 问题，那 ICP 解决的就是3D-3D 问题。给你两组 3D 点云，求它们之间的旋转和平移变换。

ICP 的思路非常直观：

  1. 找对应点：对源点云中的每个点，在目标点云中找到最近邻点。
  2. 计算变换：用 SVD 分解求使对应点距离最小的 R 和 t。
  3. 迭代：用求得的变换更新源点云，重复步骤 1-2，直到收敛。

数学上，ICP 的目标函数是：

min_{R,t} Σ || R * p_i + t - q_i ||²

其中 p_i 是源点云中的点，q_i 是目标点云中的对应点。

关键点： ICP 的成功与否，很大程度上取决于初始位姿的好坏。如果初始位姿差得太远，ICP 很容易陷入局部最优。这就是为什么很多系统先用 PnP 或 EPnP 做粗估计，再用 ICP 做精配准。

ICP 有很多变体，我整理了一下：

变体名称 对应点策略 特点
Point-to-Point 最近邻点 经典方法，简单但受噪声影响大
Point-to-Plane 点到切平面距离 收敛更快，适合平滑表面
Generalized ICP 概率模型 结合了点-点和点-面的优点
Sparse ICP 稀疏对应 对离群点鲁棒
我的建议： 在机器人抓取项目中，我通常这样搭配：先用 EPnP 从 2D 检测框和 3D 模型点得到初始位姿，然后用 Point-to-Plane ICP 做精配准。这样既保证了实时性，又拿到了亚毫米级的精度。

3.4 三者协同：从粗到精的位姿估计流水线

在实际系统中，PnP、EPnP 和 ICP 很少单独使用。我习惯把它们串成一条流水线：

  1. 目标检测：YOLO 或 Faster R-CNN 输出 2D 检测框
  2. 特征匹配：在检测框内提取特征点，与 3D 模型匹配
  3. EPnP 粗估计：用匹配的 2D-3D 点对快速计算初始位姿
  4. ICP 精配准：用深度图或 3D 点云做精细对齐
  5. 非线性优化：最后用 BA 微调，得到最终位姿

这套流程我在多个项目中验证过，效果很稳定。你想想看，如果只用 EPnP，精度受限于特征点匹配质量；如果只用 ICP，又容易陷入局部最优。两者结合，正好互补。

注意： 当物体表面缺乏纹理时，特征点匹配会失败。这时候可以考虑用边缘特征或深度信息来做 PnP。我在一个工业零件抓取项目中就遇到过这种情况——零件全是金属反光面，最后改用边缘 PnP 才搞定。
← 上一章 📖 返回目录 下一章 →