import cv2
import numpy as np
import glob

# 棋盘格尺寸：内角点数量（宽度，高度）
chessboard_size = (9, 6)

# 创建棋盘格世界坐标系下的三维点阵
# 生成形如(0,0,0), (1,0,0)...(8,5,0)的三维坐标
objp = np.zeros((chessboard_size[0] * chessboard_size[1], 3), np.float32)
objp[:, :2] = np.mgrid[0:chessboard_size[0], 0:chessboard_size[1]].T.reshape(-1, 2)

# 存储对象点和图像点的列表
objpoints = []  # 世界坐标系中的3D点
imgpoints = []  # 图像坐标系中的2D点

# 加载所有棋盘格图像路径
images = glob.glob('images/*.jpg')

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

    # 寻找棋盘格角点
    ret, corners = cv2.findChessboardCorners(gray, chessboard_size, None)

    if ret:
        objpoints.append(objp)
        imgpoints.append(corners)

        # 绘制并显示检测到的角点
        cv2.drawChessboardCorners(img, chessboard_size, corners, ret)
        cv2.imshow('Corners', img)
        cv2.waitKey(0)  # 修改此处：改为等待用户按键关闭窗口

# 销毁所有OpenCV创建的窗口
cv2.destroyAllWindows()

if len(objpoints) > 0:
    # 执行相机标定，获取相机矩阵、畸变系数等参数
    ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)

    # 输出相机矩阵和畸变系数
    print("相机矩阵:\n", mtx)
    print("畸变系数:\n", dist)

    # 计算重投影误差
    mean_error = 0
    for i in range(len(objpoints)):
        imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist)
        error = cv2.norm(imgpoints[i], imgpoints2, cv2.NORM_L2) / len(imgpoints2)
        mean_error += error

    print("平均重投影误差: ", mean_error / len(objpoints))
else:
    print("未找到有效的棋盘格图像。")