binocularVisionHandover/calibration.py

import cv2
import os
import numpy as np

leftpath = r'.\cap\left\cal'
rightpath = r'.\cap\right\cal'
CHECKERBOARD = (4, 4)  # 棋盘格内角点数
square_size = (30, 30)   # 棋盘格大小，单位mm

criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)  # termination criteria 结束条件
imgpoints_l = []    # 存放左图像坐标系下角点位置
imgpoints_r = []    # 存放右图像坐标系下角点位置
objpoints = []   # 存放世界坐标系下角点位置
objp = np.zeros((1, CHECKERBOARD[0]*CHECKERBOARD[1], 3), np.float32)
objp[0, :, :2] = np.mgrid[0:CHECKERBOARD[0], 0:CHECKERBOARD[1]].T.reshape(-1, 2)
objp[0, :, 0] *= square_size[0]
objp[0, :, 1] *= square_size[1]


for ii in range(len(os.listdir(leftpath))):
    img_l = cv2.imread(os.path.join(leftpath, os.listdir(leftpath)[ii]))
    gray_l = cv2.cvtColor(img_l, cv2.COLOR_BGR2GRAY)
    img_r = cv2.imread(os.path.join(rightpath, os.listdir(rightpath)[ii]))
    gray_r = cv2.cvtColor(img_r, cv2.COLOR_BGR2GRAY)
    ret_l, corners_l = cv2.findChessboardCorners(gray_l, CHECKERBOARD)  # 检测棋盘格内角点。cv2.findChessboardCorners()函数是OpenCV中的一个函数，用于找到棋盘格的所有内角点，返回所有检测到的内角点的2D坐标。
    ret_r, corners_r = cv2.findChessboardCorners(gray_r, CHECKERBOARD)
    if ret_l and ret_r:
        print("计算第"+str(ii)+"对图像")
        objpoints.append(objp)
        corners2_l = cv2.cornerSubPix(gray_l, corners_l, (11, 11), (-1, -1), criteria)
        imgpoints_l.append(corners2_l)
        corners2_r = cv2.cornerSubPix(gray_r, corners_r, (11, 11), (-1, -1), criteria)
        imgpoints_r.append(corners2_r)
        img = cv2.drawChessboardCorners(img_l, CHECKERBOARD, corners2_l, ret_l)
        cv2.imwrite('./ChessboardCorners_img_l.jpg', img)

ret, mtx_l, dist_l, rvecs_l, tvecs_l = cv2.calibrateCamera(objpoints, imgpoints_l, gray_l.shape[::-1], None, None)  # 先分别做单目标定，利用单目校正函数实现相机内参初始化
ret, mtx_r, dist_r, rvecs_r, tvecs_r = cv2.calibrateCamera(objpoints, imgpoints_r, gray_r.shape[::-1], None, None)
retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = cv2.stereoCalibrate(objpoints, imgpoints_l, imgpoints_r, mtx_l, dist_l, mtx_r, dist_r, gray_l.shape[::-1])  # 再做双目标定

print("stereoCalibrate : \n")
print("Camera matrix left : ")  # Camera matrix 相机内参矩阵
print(cameraMatrix1)
print("distCoeffs left  : ")  # distCoeffs 畸变矩阵
print(distCoeffs1)
print("cameraMatrix right : ")
print(cameraMatrix2)
print("distCoeffs right : ")
print(distCoeffs2)
print("R : ")  # 第一和第二个摄像机之间的旋转矩阵
print(R)
print("T : ")  # 第一和第二个摄像机之间的平移矩阵
print(T)
print("E : ")  # 本质矩阵
print(E)
print("F : ")  # 基础矩阵
print(F)
np.save('Camera matrix left.npy', cameraMatrix1)
np.save('distCoeffs left.npy', distCoeffs1)
np.save('cameraMatrix right.npy', cameraMatrix2)
np.save('distCoeffs right.npy', distCoeffs2)
np.save('R.npy', R)
np.save('T.npy', T)
np.save('F.npy', F)
np.save('E.npy', E)