'''
完成相机外参矩阵的标定

'''


import cv2
import numpy as np


# 标定板参数
pattern_size = (11, 8)  # 标定板格点数
square_size = 0.020  # 每个格子的边长（单位：米）

# 准备标定板在实际世界中的坐标
objp = np.zeros((np.prod(pattern_size), 3), dtype=np.float32)
objp[:, :2] = np.indices(pattern_size).T.reshape(-1, 2)
objp *= square_size

# 存储实际世界中的点坐标和图像中的点坐标
obj_points = []
img_points = []

# 读取标定板图像
images = ["1723983285.6120198.jpg"]  # 替换为实际的图像文件

for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

    # 找到棋盘格角点
    ret, corners = cv2.findChessboardCorners(gray, pattern_size)
    if ret:
        obj_points.append(objp)
        img_points.append(corners)

        # 可视化角点
        cv2.drawChessboardCorners(img, pattern_size, corners, ret)
        # cv2.imshow('Corners', img)
        cv2.waitKey(500)

cv2.destroyAllWindows()

# 相机内参和畸变系数（通常先通过标定获取）
camera_matrix = np.array([[1461.09955384437, 0, 963.862411991572],
                          [0, 1461.89533915958, 545.053909593624],
                          [0, 0, 1]])
dist_coeffs = np.array([0.123539164198816, -0.224109122219162, 0, 0, 0])

# 使用solvePnP计算外参
retval, rvec, tvec = cv2.solvePnP(obj_points[0], img_points[0], camera_matrix, dist_coeffs)

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

# 平移向量
T = tvec

# 打印旋转矩阵和平移向量
print("旋转矩阵 R:\n", R)
print("平移向量 T:\n", T)


# 保存相机参数到文件
np.savez("camera_params.npz", camera_matrix=camera_matrix, dist_coeffs=dist_coeffs, R=R, T=T)