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

引言

相机校准与姿态估计是计算机视觉领域的核心任务，广泛应用于三维重建、机器人导航、增强现实等场景。通过校准相机参数（内参、畸变系数）和估计物体相对于相机的空间位置（外参），可实现高精度的几何测量与空间定位。本文基于OpenCV库，系统讲解相机校准与姿态估计的完整流程，结合理论推导与代码实现，帮助读者掌握关键技术。

一、相机校准基础

1.1 相机模型与参数

相机成像过程可用针孔模型描述，其数学表达为：
[ s \begin{bmatrix} u \ v \ 1 \end{bmatrix} = \mathbf{K} \begin{bmatrix} \mathbf{R} & \mathbf{t} \end{bmatrix} \begin{bmatrix} X \ Y \ Z \ 1 \end{bmatrix} ]
其中：

• 内参矩阵（(\mathbf{K})）：描述相机内部特性，包含焦距（(f_x, f_y)）、主点坐标（(c_x, c_y)）和畸变系数（(k_1, k_2, p_1, p_2, k_3)）。
• 外参矩阵（([\mathbf{R}|\mathbf{t}])）：表示世界坐标系到相机坐标系的旋转（(\mathbf{R})）和平移（(\mathbf{t})）。

1.2 校准流程

OpenCV提供cv2.calibrateCamera()函数实现自动校准，步骤如下：

1. 准备标定板：使用棋盘格或圆点网格，已知角点物理尺寸（如25mm×25mm）。
2. 采集图像：从不同角度拍摄10-20张标定板图像。
3. 检测角点：使用cv2.findChessboardCorners()定位角点。
4. 执行校准：输入角点世界坐标与图像坐标，计算内参和畸变系数。

代码示例：

import cv2
import numpy as np
import glob
# 定义标定板参数
square_size = 25.0  # mm
pattern_size = (9, 6)  # 内部角点数量
# 准备对象点（世界坐标系）
objp = np.zeros((pattern_size[0] * pattern_size[1], 3), np.float32)
objp[:, :2] = np.mgrid[0:pattern_size[0], 0:pattern_size[1]].T.reshape(-1, 2) * square_size
# 存储对象点和图像点
objpoints = []  # 3D点
imgpoints = []  # 2D点
# 读取标定图像
images = glob.glob('calibration_images/*.jpg')
for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray, pattern_size, None)
    if ret:
        objpoints.append(objp)
        # 亚像素级角点优化
        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
        corners_refined = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
        imgpoints.append(corners_refined)
# 执行校准
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
print("内参矩阵:\n", mtx)
print("畸变系数:\n", dist)

1.3 畸变校正

径向畸变（桶形/枕形）和切向畸变可通过cv2.undistort()校正：

img_undistorted = cv2.undistort(img, mtx, dist, None, mtx)

二、姿态估计原理

2.1 PnP问题

给定3D-2D点对应关系，求解相机外参（(\mathbf{R}, \mathbf{t})）的问题称为Perspective-n-Point（PnP）。OpenCV提供多种解法：

• SOLVEPNP_ITERATIVE：基于Levenberg-Marquardt优化，适用于通用场景。
• SOLVEPNP_P3P：仅需3个点对，但需无噪声数据。
• SOLVEPNP_EPNP：高效算法，适用于大量点对。

2.2 姿态解算流程

1. 检测目标特征点：如ArUco标记或自定义特征。
2. 匹配3D模型点：建立3D-2D对应关系。
3. 调用PnP求解器：计算旋转向量和平移向量。
4. 转换为旋转矩阵：使用cv2.Rodrigues()。

代码示例：

# 假设已获取3D点（model_points）和2D点（image_points）
ret, rvec, tvec = cv2.solvePnP(model_points, image_points, mtx, dist, flags=cv2.SOLVEPNP_ITERATIVE)
# 旋转向量转矩阵
rmat, _ = cv2.Rodrigues(rvec)
print("旋转矩阵:\n", rmat)
print("平移向量:\n", tvec)

三、实战案例：ArUco标记姿态估计

3.1 ArUco标记优势

• 编码唯一性，避免误匹配。
• 内置3D模型点，简化流程。

3.2 实现步骤

1. 生成标记：使用cv2.aruco.Dictionary_get()。
2. 检测标记：cv2.aruco.detectMarkers()。
3. 估计姿态：cv2.aruco.estimatePoseSingleMarkers()。

完整代码：

# 初始化ArUco字典
dictionary = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
parameters = cv2.aruco.DetectorParameters()
# 检测标记
corners, ids, rejected = cv2.aruco.detectMarkers(img, dictionary, parameters=parameters)
# 估计姿态（标记边长为50mm）
marker_length = 50.0
if ids is not None:
    rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(corners, marker_length, mtx, dist)
    for i in range(len(rvecs)):
        cv2.aruco.drawAxis(img, mtx, dist, rvecs[i], tvecs[i], 0.1)  # 绘制坐标轴

四、优化与调试技巧

4.1 校准精度提升

• 增加标定图像数量：建议≥15张，覆盖不同角度和距离。
• 均匀分布视角：避免所有图像集中在同一区域。
• 检查重投影误差：cv2.calibrateCamera()返回的ret值应小于0.5像素。

4.2 姿态估计稳定性

• 滤波处理：对连续帧的rvec和tvec应用卡尔曼滤波。
• 多标记融合：同时检测多个ArUco标记，通过最小二乘法优化结果。
• 异常值剔除：使用RANSAC算法排除误匹配点。

五、应用场景与扩展

5.1 典型应用

• 机器人抓取：通过姿态估计确定物体位置。
• 增强现实：将虚拟对象准确叠加到现实场景。
• 三维重建：结合多视角姿态估计生成点云。

5.2 扩展方向

• 深度学习集成：用神经网络预测关键点，替代传统特征检测。
• 动态校准：实时更新相机参数以适应环境变化。
• 多相机系统：扩展至立体视觉和结构光扫描。

结论

相机校准与姿态估计是计算机视觉的基石技术。通过OpenCV提供的工具链，开发者可高效实现从标定到姿态解算的全流程。本文结合理论推导与代码实践，覆盖了关键算法和优化策略，为机器人、AR等领域的实际应用提供了完整解决方案。建议读者通过实际项目巩固知识，并探索深度学习等前沿技术的融合应用。