王正威5分钟实战：用OpenCV的solvePnP快速搞定相机位姿估计
在增强现实、机器人导航和三维重建等领域，我们经常需要快速估算相机在空间中的位置和方向。传统PnP（Perspective-n-Point）算法的理论推导往往让开发者望而生畏——那些复杂的矩阵运算和数学公式就像一堵高墙，挡住了许多想要快速应用的工程师。但今天，我要告诉你一个秘密：利用OpenCV的solvePnP函数，你完全可以在5分钟内实现高精度的相机位姿估计，而无需深入那些令人头疼的数学推导。
1. 准备工作：理解基础概念
在开始编码前，我们需要明确几个关键概念：
- 3D-2D点对：这是PnP问题的核心输入数据。3D点指的是物体在世界坐标系中的坐标，2D点则是这些点在相机成像平面上的投影位置。
- 相机内参：包括焦距(fx, fy)和主点坐标(cx, cy)，通常用一个3×3的矩阵K表示。
- 位姿估计：即求取相机坐标系相对于世界坐标系的旋转矩阵R和平移向量t。
关键点：你至少需要4组3D-2D匹配点才能获得稳定的解（3点解法存在多解情况，需要额外验证点）。
需要模型API调用？ 免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. OpenCV的solvePnP函数详解
OpenCV提供了多种PnP求解方法，下面是最常用的几种：
| 方法类型 | 特点 | 适用场景 |
|---|---|---|
| SOLVEPNP_ITERATIVE | 基于Levenberg-Marquardt优化的重投影误差最小化 | 通用场景，精度高 |
| SOLVEPNP_EPNP | 高效线性解法，复杂度O(n) | 点数较多时(>50) |
| SOLVEPNP_P3P | 仅需3个点，但可能有多个解 | 点数极少时 |
| SOLVEPNP_DLS | 直接最小二乘法 | 中等规模点集 |
| SOLVEPNP_UPNP | 通用无标定PnP | 相机内参未知时 |
基本函数调用形式如下：
python复制retval, rvec, tvec = cv2.solvePnP(
objectPoints, # 世界坐标系中的3D点(Nx3)
imagePoints, # 对应的2D图像点(Nx2)
cameraMatrix, # 相机内参矩阵(3x