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

一、相机校准：从畸变图像到几何精准

1.1 相机模型的数学基础

相机成像本质是将三维空间点投影到二维图像平面的过程，其数学模型由内参矩阵和外参矩阵共同描述。内参矩阵包含焦距（fx, fy）、主点坐标（cx, cy）和畸变系数（k1, k2, p1, p2, k3），决定了相机内部的几何特性；外参矩阵则通过旋转向量（rvec）和平移向量（tvec）描述相机在世界坐标系中的位姿。

OpenCV中，cv2.calibrateCamera()函数通过多组三维-二维点对计算这些参数。例如，使用棋盘格标定时，需提取角点的图像坐标（objPoints）和对应的世界坐标（imgPoints），通过迭代优化最小化重投影误差。

1.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])
# 读取畸变图像
distorted_img = cv2.imread('distorted.jpg')
# 校正图像
undistorted_img = cv2.undistort(distorted_img, camera_matrix, dist_coeffs)

对于性能敏感场景，可预先计算校正映射（cv2.initUndistortRectifyMap()）并应用cv2.remap()，避免重复计算。

1.3 标定板选择与数据采集

棋盘格因其规则性和易检测性成为最常用标定物。采集时需注意：

多角度覆盖：从不同距离、倾斜角度拍摄（建议15-20组），确保涵盖相机视野的各个区域。
分辨率一致性：所有图像应保持相同分辨率，避免内参矩阵尺度变化。
角点检测精度：使用cv2.findChessboardCorners()时，可启用亚像素级优化（cv2.cornerSubPix()）提升精度。

二、姿态估计：从二维到三维的空间定位

2.1 单应性变换（Homography）的应用

当目标平面与相机成像平面平行时（如地面、墙面），可通过单应性矩阵直接建立二维-二维对应关系。OpenCV中，cv2.findHomography()使用RANSAC算法排除误匹配，适用于AR标记追踪、图像拼接等场景：

# 假设已提取两组对应点
src_pts = np.float32([[x1, y1], [x2, y2], ...])
dst_pts = np.float32([[x1', y1'], [x2', y2'], ...])
H, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0)
# 应用变换
warped_img = cv2.warpPerspective(img, H, (width, height))

2.2 PnP算法：三维姿态求解的核心

对于非平面目标或需精确三维定位的场景，PnP（Perspective-n-Point）问题通过已知三维点及其二维投影求解相机位姿。OpenCV实现包括：

SOLVEPNP_ITERATIVE：基于Levenberg-Marquardt优化，适合高精度需求。
SOLVEPNP_P3P：仅需3个点对，但易受噪声影响。
SOLVEPNP_EPNP：快速近似解法，适用于实时系统。

示例代码：

# 假设已知3D点（obj_pts）和2D投影（img_pts）
obj_pts = np.array([[0,0,0], [1,0,0], [0,1,0], [0,0,1]], dtype=np.float32)
img_pts = np.array([[x1,y1], [x2,y2], [x3,y3], [x4,y4]], dtype=np.float32)
# 使用EPnP算法求解
success, rvec, tvec = cv2.solvePnP(obj_pts, img_pts, camera_matrix, dist_coeffs, flags=cv2.SOLVEPNP_EPNP)
# 将旋转向量转换为旋转矩阵
rot_matrix, _ = cv2.Rodrigues(rvec)

2.3 位姿优化的关键技巧

重投影误差监控：通过cv2.projectPoints()将3D点重新投影到图像，计算与实际检测点的误差，一般需控制在0.5像素以内。
多帧融合：对视频序列中的位姿结果进行卡尔曼滤波或滑动平均，抑制单帧噪声。
退化场景处理：当特征点共面或数量不足时，结合IMU数据或引入先验约束。

三、实战案例：基于ArUco标记的实时姿态估计

3.1 ArUco标记的优势

ArUco是OpenCV内置的二进制方块标记库，具有：

高鲁棒性：每个标记有唯一ID，支持部分遮挡。
快速检测：cv2.aruco.detectMarkers()可在毫秒级完成检测。
内置位姿估计：直接调用cv2.aruco.estimatePoseSingleMarkers()获取位姿。

3.2 完整代码实现

import cv2
import cv2.aruco as aruco
# 初始化标记字典（6x6标记，250个ID）
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
parameters = aruco.DetectorParameters_create()
# 假设已校准相机
camera_matrix = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
dist_coeffs = np.array([k1, k2, p1, p2, k3])
cap = cv2.VideoCapture(0)
while True:
    ret, frame = cap.read()
    if not ret:
        break
    # 检测标记
    corners, ids, rejected = aruco.detectMarkers(frame, aruco_dict, parameters=parameters)
    if ids is not None:
        # 估计每个标记的位姿（标记边长0.05m）
        rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(corners, 0.05, camera_matrix, dist_coeffs)
        # 绘制坐标轴（红色X，绿色Y，蓝色Z）
        for i in range(len(ids)):
            aruco.drawAxis(frame, camera_matrix, dist_coeffs, rvecs[i], tvecs[i], 0.1)
    cv2.imshow('Pose Estimation', frame)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break
cap.release()
cv2.destroyAllWindows()

3.3 性能优化建议

降低分辨率：在保证精度的前提下，将图像缩放至640x480可提升帧率30%-50%。
硬件加速：对支持OpenCL的设备，启用cv2.UMat进行GPU加速。
标记布局设计：避免标记间相互遮挡，对于大场景，采用分布式标记群。

四、常见问题与解决方案

4.1 校准结果不稳定

原因：标定板检测误差、光照不均、样本不足。
解决：增加标定图像数量（建议30组以上），使用均匀光照，手动检查角点检测结果。

4.2 姿态估计跳变

原因：特征点误匹配、PnP算法收敛失败。
解决：引入RANSAC阈值调整（如cv2.SOLVEPNP_RANSAC），结合时间滤波。

4.3 实时性不足

原因：高分辨率图像、复杂算法。
解决：采用轻量级标记（如AprilTag），优化代码（如C++重写关键部分）。

五、总结与展望

相机校准与姿态估计是计算机视觉从二维分析迈向三维感知的关键桥梁。通过OpenCV提供的工具链，开发者可高效实现从毫米级精度标定到实时六自由度位姿跟踪的完整流程。未来，随着深度学习与几何方法的融合（如Learning-based PnP），系统鲁棒性将进一步提升，为机器人导航、增强现实等领域带来更广阔的应用空间。

OpenCV相机校准与姿态估计全解析：从理论到实践