单目相机实战：用OpenCV的solvePnP实现物体位姿估计（附Python代码）

单目相机实战：用OpenCV的solvePnP实现物体位姿估计（附Python代码）

在机器人导航、增强现实和工业检测等领域，精确获取物体相对于相机的位置和姿态是关键挑战。单目相机因其成本优势和轻量化特点，成为许多视觉系统的首选传感器。本文将手把手带您实现一个完整的位姿估计流程，从坐标系关系到代码落地，最后还能计算出目标物体的欧拉角和实际距离。

1. 理解坐标系：从三维世界到二维像素

任何视觉系统的第一步都是建立坐标系间的数学关系。我们需要明确四个坐标系及其转换：

• 世界坐标系：物体在真实空间中的绝对坐标，通常以检测目标的某个角点为原点
• 相机坐标系：以相机光心为原点，Z轴指向拍摄方向
• 图像坐标系：成像平面上的二维坐标系，原点在图像中心
• 像素坐标系：OpenCV等库处理的图像坐标系，原点在左上角

它们之间的转换通过以下矩阵实现：

# 相机内参矩阵示例 camera_matrix = np.array([ [fx, 0, cx], [ 0, fy, cy], [ 0, 0, 1] ])

其中fx/fy是焦距与像素尺寸的比值，cx/cy是主点坐标

2. solvePnP核心参数详解

OpenCV的solvePnP函数是位姿估计的核心工具，其参数配置直接影响结果精度：

推荐算法选择：

• SOLVEPNP_ITERATIVE：默认方法，需要至少4个点
• SOLVEPNP_EPNP：适用于点数较多(≥4)的场景
• SOLVEPNP_IPPE：平面物体定位专用算法

3. 完整Python实现流程

下面是一个检测矩形物体的实战示例，假设我们已知物体尺寸为20cm×15cm：

import cv2 import numpy as np # 定义物体3D坐标 (单位：厘米) object_pts = np.float32([ [0, 0, 0], # 左下角 [20, 0, 0], # 右下角 [20, 15, 0], # 右上角 [0, 15, 0] # 左上角 ]) # 假设检测到的图像坐标 image_pts = np.float32([ [325, 420], # 左下角 [480, 410], # 右下角 [475, 300], # 右上角 [330, 310] # 左上角 ]) # 相机内参 (需要实际标定) camera_matrix = np.array([ [800, 0, 320], [0, 800, 240], [0, 0, 1] ]) # 执行位姿求解 success, rvec, tvec = cv2.solvePnP( object_pts, image_pts, camera_matrix, None, # 假设无畸变 flags=cv2.SOLVEPNP_ITERATIVE ) # 转换为旋转矩阵 rotation_mat, _ = cv2.Rodrigues(rvec) print("旋转矩阵:\n", rotation_mat) print("平移向量:\n", tvec)

4. 欧拉角与距离计算实战

获得旋转矩阵后，可以进一步提取更直观的欧拉角：

# 计算俯仰角(pitch)、偏航角(yaw)、滚转角(roll) def rotation_matrix_to_euler_angles(R): sy = np.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0]) x = np.arctan2(R[2,1], R[2,2]) y = np.arctan2(-R[2,0], sy) z = np.arctan2(R[1,0], R[0,0]) return np.rad2deg(np.array([x, y, z])) euler_angles = rotation_matrix_to_euler_angles(rotation_mat) print(f"欧拉角(度): Pitch={euler_angles[0]:.2f}, Yaw={euler_angles[1]:.2f}, Roll={euler_angles[2]:.2f}") # 计算物体到相机的距离(单位：厘米) distance = np.linalg.norm(tvec) print(f"目标距离: {distance:.2f} cm")

常见问题处理：

1. 当出现nan结果时，检查：
   • 点对应关系是否正确
   • 相机内参是否合理
   • 3D点是否共面
2. 精度提升技巧：
   • 使用更多特征点(6-8个为佳)
   • 采用亚像素级角点检测
   • 进行相机标定获取精确内参

5. 实际应用中的优化策略

在真实场景中，还需要考虑以下增强措施：

• 鲁棒性处理：

  # 使用RANSAC剔除异常点 _, rvec, tvec, inliers = cv2.solvePnPRansac( object_pts, image_pts, camera_matrix, distCoeffs=None, iterationsCount=100, reprojectionError=8.0 )

• 运动平滑：

  # 使用卡尔曼滤波平滑位姿变化 kalman = cv2.KalmanFilter(6, 3) kalman.measurementMatrix = np.eye(3, 6, dtype=np.float32) kalman.transitionMatrix = np.eye(6, 6, dtype=np.float32) # 更新步骤 measurement = np.concatenate([rvec.flatten(), tvec.flatten()]) kalman.correct(measurement) predicted = kalman.predict()

• 性能优化：

  • 对静态场景可缓存位姿结果
  • 使用C++扩展处理高频数据
  • 采用多线程并行计算

6. 可视化与调试技巧

良好的可视化能极大提升开发效率：

# 绘制坐标系轴 def draw_axes(img, rvec, tvec, camera_matrix, length=5): axis = np.float32([[length,0,0], [0,length,0], [0,0,-length], [0,0,0]]) imgpts, _ = cv2.projectPoints(axis, rvec, tvec, camera_matrix, None) img = cv2.line(img, tuple(imgpts[3].ravel()), tuple(imgpts[0].ravel()), (255,0,0), 3) # X轴(红) img = cv2.line(img, tuple(imgpts[3].ravel()), tuple(imgpts[1].ravel()), (0,255,0), 3) # Y轴(绿) img = cv2.line(img, tuple(imgpts[3].ravel()), tuple(imgpts[2].ravel()), (0,0,255), 3) # Z轴(蓝) return img # 在图像上显示结果 result_img = draw_axes(input_img, rvec, tvec, camera_matrix) cv2.putText(result_img, f"Distance: {distance:.1f}cm", (10,30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,255,0), 2)

对于需要持续跟踪的场景，建议记录时间序列数据并绘制变化曲线，这能帮助发现潜在的抖动或漂移问题。