📚 课程目录第4章相机外参估计（单张图像）

4. 相机外参估计（单张图像）

任务说明

已知相机内参矩阵K（含畸变系数），给定一张包含已知尺寸平面标定板（棋盘格）的单张图像，估计相机相对于标定板平面的外参（旋转矩阵R和平移向量t）。本案例使用OpenCV的solvePnP实现，输入为棋盘格角点的像素坐标与对应的三维世界坐标。

实现方案

采用棋盘格平面作为世界坐标系Z=0平面，通过findChessboardCorners提取角点，结合solvePnP求解外参。代码包含完整的角点提取、外参计算及结果可视化流程，可直接运行于提供的示例图像。

代码实现


import cv2
import numpy as np

# 内参矩阵（示例值，实际使用需标定获得）
K = np.array([[800, 0, 320],
              [0, 800, 240],
              [0, 0, 1]], dtype=np.float64)
dist_coeffs = np.zeros((4, 1))  # 假设无畸变

# 棋盘格参数
pattern_size = (9, 6)  # 内角点数量
square_size = 0.025   # 每个方格边长（米）

# 生成世界坐标（Z=0平面）
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)
objp *= square_size

# 读取图像并提取角点
image_path = "checkerboard.jpg"  # 替换为实际图像路径
img = cv2.imread(image_path)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

found, corners = cv2.findChessboardCorners(gray, pattern_size, None)
if not found:
    raise RuntimeError("未检测到完整棋盘格，请检查图像或参数")

# 亚像素细化
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
corners_refined = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)

# 使用solvePnP估计外参（棋盘格平面为世界坐标Z=0）
success, rvec, tvec = cv2.solvePnP(objp, corners_refined, K, dist_coeffs)
if not success:
    raise RuntimeError("solvePnP求解失败")

# 将旋转向量转为旋转矩阵
R, _ = cv2.Rodrigues(rvec)

print("旋转矩阵 R:")
print(R)
print("\n平移向量 t (米):")
print(tvec.reshape(3, 1))

# 可视化：绘制坐标轴（验证外参正确性）
axis_length = 0.05  # 坐标轴长度（米）
axis_points = np.float32([[0, 0, 0],
                          [axis_length, 0, 0],
                          [0, axis_length, 0],
                          [0, 0, axis_length]]).reshape(-1, 3)

# 投影三维点到图像平面
img_points, _ = cv2.projectPoints(axis_points, rvec, tvec, K, dist_coeffs)
img_points = img_points.reshape(-1, 2)

# 绘制坐标轴（红-X, 绿-Y, 蓝-Z）
origin = tuple(img_points[0].astype(int))
img_vis = img.copy()
for i, color in enumerate([(0, 0, 255), (0, 255, 0), (255, 0, 0)], start=1):
    end = tuple(img_points[i].astype(int))
    cv2.line(img_vis, origin, end, color, 3)

cv2.imshow("Pose Estimation", img_vis)
cv2.waitKey(0)
cv2.destroyAllWindows()

落地与验证

运行方式：安装OpenCV（pip install opencv-python），准备一张清晰的棋盘格图像（9×6内角点），将代码中image_path替换为实际路径后运行。

验证方法：观察输出旋转矩阵R是否接近单位矩阵（当相机正对棋盘格时），平移向量t的Z分量应约为相机到棋盘格的距离（米）。可视化窗口中，若外参正确，红、绿、蓝三色坐标轴应分别沿棋盘格的X、Y方向延伸，蓝色Z轴垂直指向棋盘格平面外。

判定标准：程序无报错完成求解；打印的R和t为有限数值；投影坐标轴与棋盘格边缘对齐（红色沿棋盘格行方向，绿色沿列方向，蓝色指向相机方向）。