OpenCV Tutorials 26 - 相机校准与姿态估计

一、相机校准基础理论

1.1 针孔相机模型

针孔相机模型是计算机视觉的基础，其成像公式为：
[
s \begin{bmatrix} u \ v \ 1 \end{bmatrix} =
\begin{bmatrix} f_x & 0 & c_x \ 0 & f_y & c_y \ 0 & 0 & 1 \end{bmatrix}
\begin{bmatrix} X \ Y \ Z \end{bmatrix}
]
其中( (u,v) )为像素坐标，( (X,Y,Z) )为世界坐标，( f_x,f_y )为焦距，( c_x,c_y )为主点偏移。OpenCV通过cv2.calibrateCamera()实现参数求解，需准备至少10组不同视角的棋盘格图像。

1.2 畸变矫正模型

实际镜头存在径向畸变（桶形/枕形）和切向畸变，数学模型为：
[
\begin{cases}
x{corrected} = x(1 + k_1r^2 + k_2r^4 + k_3r^6) + 2p_1xy + p_2(r^2+2x^2) \
y{corrected} = y(1 + k_1r^2 + k_2r^4 + k_3r^6) + p_1(r^2+2y^2) + 2p_2xy
\end{cases}
]
其中( k_1,k_2,k_3 )为径向畸变系数，( p_1,p_2 )为切向畸变系数。OpenCV使用cv2.undistort()进行图像矫正，示例代码如下：

import cv2
import numpy as np
# 假设已获得相机矩阵和畸变系数
camera_matrix = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
dist_coeffs = np.array([k1, k2, p1, p2, k3])
# 读取畸变图像
img = cv2.imread('distorted.jpg')
# 矫正图像
undistorted_img = cv2.undistort(img, camera_matrix, dist_coeffs)

二、单目相机姿态估计

2.1 PnP问题求解

给定3D-2D点对应关系，通过Perspective-n-Point算法求解相机位姿。OpenCV提供cv2.solvePnP()实现，支持SOLVEPNP_ITERATIVE（默认）、SOLVEPNP_P3P等5种方法。典型流程：

# 假设已知3D点坐标和对应的2D投影点
object_points = np.array([[0,0,0], [1,0,0], [0,1,0], [0,0,1]], dtype=np.float32)
image_points = np.array([[100,200], [300,200], [100,400], [300,400]], dtype=np.float32)
# 使用EPNP方法求解
success, rotation_vector, translation_vector = cv2.solvePnP(
    object_points, image_points, camera_matrix, dist_coeffs, flags=cv2.SOLVEPNP_EPNP)

2.2 位姿可视化

将旋转向量转换为旋转矩阵后，可使用cv2.aruco.drawAxis()绘制坐标轴：

# 旋转向量转旋转矩阵
rotation_matrix, _ = cv2.Rodrigues(rotation_vector)
# 绘制坐标轴（长度50单位）
img_axis = cv2.aruco.drawAxis(img, camera_matrix, dist_coeffs, 
                             rotation_vector, translation_vector, 50)

三、双目视觉姿态估计

3.1 立体校正

双目系统需先进行立体校正，使两幅图像行对齐。关键步骤：

# 假设已完成单目校准
left_camera_matrix, left_dist = ...  # 左相机参数
right_camera_matrix, right_dist = ...  # 右相机参数
R, T = ...  # 左右相机相对位姿（通过stereoCalibrate获得）
# 计算立体校正参数
R1, R2, P1, P2, Q, _, _ = cv2.stereoRectify(
    left_camera_matrix, left_dist, right_camera_matrix, right_dist,
    (640,480), R, T, alpha=0.9)
# 生成校正映射
left_map1, left_map2 = cv2.initUndistortRectifyMap(
    left_camera_matrix, left_dist, R1, P1, (640,480), cv2.CV_16SC2)
right_map1, right_map2 = cv2.initUndistortRectifyMap(
    right_camera_matrix, right_dist, R2, P2, (640,480), cv2.CV_16SC2)
# 应用校正
left_rectified = cv2.remap(left_img, left_map1, left_map2, cv2.INTER_LINEAR)
right_rectified = cv2.remap(right_img, right_map1, right_map2, cv2.INTER_LINEAR)

3.2 深度图计算

校正后使用cv2.StereoSGBM计算视差图：

stereo = cv2.StereoSGBM_create(
    minDisparity=0,
    numDisparities=64,
    blockSize=5,
    P1=8*3*3**2,
    P2=32*3*3**2,
    disp12MaxDiff=1,
    uniquenessRatio=10,
    speckleWindowSize=100,
    speckleRange=32
)
disparity = stereo.compute(left_rectified, right_rectified).astype(np.float32)/16.0

四、实战建议与优化

4.1 标定板选择

• 棋盘格：适用于高精度场景，推荐7x10内角点，方格尺寸20-30mm
• 圆形网格：抗模糊能力更强，需检测椭圆中心
• ChArUco板：结合棋盘格和ArUco标记，适合部分遮挡情况

4.2 精度提升技巧

1. 图像多样性：覆盖整个视场，包含不同距离（近/中/远）和倾斜角度
2. 重投影误差监控：目标应小于0.5像素

   mean_error = 0
   for i in range(len(obj_points)):
    img_points2, _ = cv2.projectPoints(obj_points[i], rvecs[i], tvecs[i], 
                                      camera_matrix, dist_coeffs)
    error = cv2.norm(img_points[i], img_points2, cv2.NORM_L2)/len(img_points2)
    mean_error += error
   print("Mean reprojection error: {}".format(mean_error/len(obj_points)))

3. 温度控制：工业相机需预热30分钟达到热稳定

4.3 实时性优化

• 使用cv2.UMat启用OpenCL加速
• 对PnP求解采用RANSAC迭代（设置useExtrinsicGuess=True）
• 双目匹配时限制搜索范围（setDisparityRange()）

五、典型应用场景

1. AR导航：结合SLAM实现厘米级定位
2. 工业检测：通过位姿估计引导机械臂抓取
3. 三维重建：多视角位姿融合生成点云
4. 无人机定高：利用地面特征点估计飞行高度

本教程完整代码包含标定工具、位姿解算演示和可视化界面，可在GitHub获取。建议初学者先完成单目标定，再逐步过渡到双目系统，最后实现动态追踪应用。