import numpy as np
import cv2
import glob

# 1. 准备标定板参数
chessboard_size = (7, 10)  # 内角点数量
square_size = 13.0  # 毫米

# 2. 生成世界坐标系中的3D点
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) * square_size

# 3. 存储对象点和图像点
objpoints = []  # 3D点
imgpoints = []  # 2D点

# 4. 读取所有标定图像
images = glob.glob('D:/DeepLearning/datasets/clibration/rect/*.png')
image_size = None

for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    
    if image_size is None:
        image_size = gray.shape[::-1]
    
    # 查找棋盘格角点
    ret, corners = cv2.findChessboardCorners(gray, chessboard_size, None)
    
    if ret:
        objpoints.append(objp)
        
        # 亚像素精确化
        corners2 = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1),
                                   (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001))
        imgpoints.append(corners2)
        
        # 可视化角点
        cv2.drawChessboardCorners(img, chessboard_size, corners2, ret)
        cv2.imshow('Corners', img)
        cv2.imwrite("this.png",img)
        cv2.waitKey(500)

cv2.destroyAllWindows()

# 5. 相机标定
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, 
                                                   image_size, None, None)

print("相机内参矩阵:\n", mtx)
print("\n畸变系数:", dist.ravel())

# 6. 评估标定误差
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("\n平均重投影误差: {} 像素".format(mean_error/len(objpoints)))

# 7. 保存标定结果
np.savez('calibration_result.npz', mtx=mtx, dist=dist)