柯雨恒OpenCV实战：手把手教你用solvePnP搞定相机位姿估计（附Python代码）
在计算机视觉和机器人领域，相机位姿估计是一个基础而关键的任务。无论是SLAM系统、增强现实应用，还是物体跟踪场景，我们都需要准确地知道相机在三维空间中的位置和朝向。OpenCV作为计算机视觉领域的瑞士军刀，提供了cv2.solvePnP这个强大的函数来解决这个问题。本文将带你从零开始，通过一个完整的Python示例，掌握如何在实际项目中使用solvePnP进行相机位姿估计。
1. 准备工作与环境配置
在开始之前，我们需要确保开发环境已经正确配置。推荐使用Python 3.8+和OpenCV 4.5+版本。可以通过以下命令安装必要的库：
bash复制pip install opencv-python numpy matplotlib
对于更完整的计算机视觉开发环境，还可以安装：
bash复制pip install opencv-contrib-python
关键依赖说明：
- OpenCV：提供solvePnP等核心计算机视觉功能
- NumPy：处理矩阵运算和数值计算
- Matplotlib：用于结果可视化和调试
提示：如果在虚拟环境中工作，建议先创建并激活虚拟环境，再安装上述包。
需要模型API调用？ 免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 理解PnP问题的本质
Perspective-n-Point（PnP）问题是指：已知一组3D空间点及其在2D图像上的投影，以及相机的内参，求解相机相对于这些3D点的位姿（旋转和平移）。
典型应用场景：
- AR/VR中的虚拟物体定位
- 机器人导航中的定位与建图
- 工业检测中的相机标定
- 自动驾驶中的环境感知
PnP问题的数学表述可以简化为：
code复制s * [u, v, 1]^T = K * [R|t] * [X, Y, Z, 1]^T
其中：
(u,v)是2D图像坐标(X,Y,Z)是对应的3D世界坐标K是相机内参矩阵[R|t]是待求的相机位姿s是比例因子（深度）
3. solvePnP函数详解
OpenCV的solvePnP函数封装了多种PnP求解算法，其基本调用形式为：
python复制retval, rvec, tvec = cv2.solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, flags=SOLVEPNP_ITERATIVE)
参数解析：
| 参数 | 类型 | 说明 |
|---|---|---|
| objectPoints | np.array | 3D点集，形状为(N,3) |
| imagePoints | np.array | 对应的2D点集，形状为(N,2) |
| cameraMatrix | np.array | 3x3相机内参矩阵 |
| distCoeffs | np.array | 畸变系数，可为None |
| flags | int | 算法选择标志 |
| rvec | np.array | 输出的旋转向量 |
| t |