python-opencv双目图像矫正

python-opencv双目图像矫正

最近在搞双目视觉矫正，采用的是张征友标定法。主要步骤包括：获取相机1和相机2的标定图片，对标定图片进行预处理 （裁剪、分辨率匹配）、然后利用opencv的函数库对图像进行矫正
核心代码主要是参考这篇博文 ，关于张征友标定法的理论大家可以去看刚才上面那篇博文，讲的很详细
本人在原有的基础根据自己的需求进行了一些改动以及注释的补充，直接上代码：

import numpy as np
import cv2
import os
import datetime

def cam_calib_find_corners(img,col_num, row_num):
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ret, corners2 = cv2.findChessboardCorners(gray ,(row_num ,col_num), None)
    # 为了得到稍微精确一点的角点坐标，进一步对角点进行亚像素寻找
    # corners2 = cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1), (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 10, 0.001))
    return (ret, corners2)


def stereo_calib_calibrate(img_dir_L ,img_dir_R ,row_num ,col_num , square_sz):
    """
    对图片进行处理并返回双目相机相关参数
    :param img_dir_L: 相机1图像路径 
    :param img_dir_R: 相机2图像路径
    :param row_num:   行数
    :param col_num:   列数
    :param square_sz: 单个格子实际物理尺寸 单位：mm
    :param square_sz: 单个格子实际物理尺寸 单位：mm
    :return: 以字典形式返回双目相机矫正参数以及处理图像文件保存路径
    retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F
    """
    w = 640
    h = 480
    all_cornersL = []
    all_cornersR = []
    patterns = []

    # 标定相机,先搞这么一个假想的板子，标定就是把图像中的板子往假想的板子上靠，靠的过程就是计算参数的过程
    # 比如说(0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
    pattern_points = np.zeros(( row_num * col_num, 3), np.float32)
    pattern_points[:, :2] = np.mgrid[0:row_num, 0:col_num].T.reshape(-1, 2)
    pattern_points *= square_sz

    png_list_r = os.listdir(img_dir_R)
    png_list_l = os.listdir(img_dir_L)
    png_list_l = sorted(png_list_l)
    png_list_r = sorted(png_list_r)

    folder = "./out/{}".format(datetime.datetime.now().strftime("%Y%m%d_%H%M%S"))

    os.makedirs(folder, exist_ok=True)
    for i in range(len(png_list_l)):
        png_path_r = img_dir_R + "/" + png_list_r[i]
        png_path_l = img_dir_L + "/" + png_list_l[i]
        print(png_path_l,png_path_r)
        img_r = cv2.imread(png_path_r)
        img_l = cv2.imread(png_path_l)
        img_l = img_l[135:413, 169:498]
        img_l = cv2.resize(img_l, (w,h))
        # 保存预处理后的图像
        pre_folder = folder +"/pre"
        os.makedirs(pre_folder, exist_ok=True)
        pre_png = pre_folder + "/"  + png_list_l[i]
        cv2.imwrite(pre_png,img_l)


        # 获取角点
        ret_r, corners_r = cam_calib_find_corners(img_r, col_num, row_num)
        ret_l, corners_l = cam_calib_find_corners(img_l, col_num, row_num)

        # 合并所有角点
        all_cornersL.append(corners_l)
        all_cornersR.append(corners_r)
        patterns.append(pattern_points)

    # rms表示的是重投影误差；
    # cameraMatrix是相机的内参矩阵；
    # distCoeffs表述的相机畸变参数；
    # rvecs表示标定棋盘格世界坐标系到相机坐标系的旋转参数：rotation vectors，需要进行罗德里格斯转换；
    # tvecs表示translation vectors，主要是平移参数。
    # https://blog.csdn.net/m0_49332456/article/details/121011500
    rmsL, cameraMatrixL, distCoeffsL, rvecsL, tvecsL = cv2.calibrateCamera(patterns, all_cornersL, (w, h), None, None)
    rmsR, cameraMatrixR, distCoeffsR, rvecsR, tvecsR = cv2.calibrateCamera(patterns, all_cornersR, (w, h), None, None)

    # 计算两个相机之间的关系矩阵模型
    flags = 0
    flags |= cv2.CALIB_FIX_FOCAL_LENGTH

    """
          flag
     "    CV_CALIB_FIX_INTRINSIC            如果该标志被设置，那么就会固定输入的cameraMatrix和distCoeffs不变，只求解R,T,E,F\n"
     "    CV_CALIB_USE_INTRINSIC_GUESS      根据用户提供的cameraMatrix和distCoeffs为初始值开始迭代\n"
     "    CV_CALIB_FIX_PRINCIPAL_POINT      迭代过程中不会改变主点的位置\n"
     "    CV_CALIB_FIX_FOCAL_LENGTH         迭代过程中不会改变焦距 \n"
     "    CV_CALIB_SAME_FOCAL_LENGTH        强制保持两个摄像机的焦距相同 CV_CALIB_ZERO_TANGENT_DIST 切向畸变保持为零\n"
     "    CV_CALIB_FIX_K1,…,CV_CALIB_FIX_K6 迭代过程中不改变相应的值。如果设置了CV_CALIB_USE_INTRINSIC_GUESS 将会使用用户提供的初始值，否则设置为零\n"
     "    CV_CALIB_RATIONAL_MODEL           畸变模型的选择，如果设置了该参数，将会使用更精确的畸变模型，distCoeffs的长度就会变成8\n"
     "    原文链接：https://blog.csdn.net/weixin_51229250/article/details/120218209\n"
    """
    stereocalib_criteria = (cv2.TERM_CRITERIA_MAX_ITER + cv2.TERM_CRITERIA_EPS, 1, 1e-5)
    # return ：retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F
    ret, Ml, dl, Mr, dr, R, T, E, F = cv2.stereoCalibrate(  patterns,
                                                            all_cornersL,
                                                            all_cornersR,
                                                            cameraMatrixL,
                                                            distCoeffsL,
                                                            cameraMatrixR,
                                                            distCoeffsR,
                                                            (w, h),
                                                            criteria=stereocalib_criteria,
                                                            flags=flags)

    print('Intrinsic_mtx_l', Ml)
    print('dist_l', dl)
    print('Intrinsic_mtx_r', Mr)
    print('dist_r', dr)
    print('R', R)
    print('T', T)
    print('E', E)
    print('F', F)

    camera_model = dict([('Ml', Ml), ('Mr', Mr), ('dl', dl),
                         ('dr', dr), ('rl', rvecsL),
                         ('rr', rvecsR), ('R', R), ('T', T),
                         ('E', E), ('F', F)])
    return camera_model,folder


def stereo_calib_correct(img_dir_L, new_img_dir_R, cameraModel):
    """
    :param crct_img_L_dir: 待矫正左相机图片路径
    :param crct_img_R_dir: 待矫正右相机图片路径
    :param cameraModel:    已获取相机参数
    :return:
    """
    w = 640
    h = 480
    png_list_r = os.listdir(new_img_dir_R)
    png_list_l = os.listdir(img_dir_L)


    png_list_l = sorted(png_list_l)
    png_list_r = sorted(png_list_r)

    for i in range(len(png_list_l)):
        png_path_r = new_img_dir_R + "/" + png_list_r[i]
        png_path_l = img_dir_L + "/" + png_list_l[i]
        print(png_path_l,png_path_r)
        img_r = cv2.imread(png_path_r)
        img_l = cv2.imread(png_path_l)
        grayL = cv2.cvtColor(img_l, cv2.COLOR_BGR2GRAY)
        grayR = cv2.cvtColor(img_r, cv2.COLOR_BGR2GRAY)

    # 极线矫正，也就是把两幅图的极线搞成水平，就像在博客中描述的那样，把任意位置的像平面，搞成两个平行的像平面
    """
        用于双目相机的立体校正环节
        https://blog.csdn.net/qq_41685265/article/details/105777229
        stereoRectify(cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize, R, T[, R1[, R2[, P1[, P2[, Q
        [,  flags[, alpha[, newImageSize]]]]]]]])
         -> R1, R2, P1, P2, Q, validPixROI1, validPixROI2
        cameraMatrix1 第一个摄像机的摄像机矩阵，即左相机相机内参矩阵，矩阵第三行格式应该为 0 0 1
        distCoeffs1    第一个摄像机的畸变向量
        cameraMatrix2  第一个摄像机的摄像机矩阵，即右相机相机内参矩阵，矩阵第三行格式应该为 0 0 1
        distCoeffs2    第二个摄像机的畸变向量
        imageSize      图像大小
        R-             相机之间的旋转矩阵，这里R的意义是：相机1通过变换R到达相机2的位姿 划重点！！！
        T-             左相机到右相机的平移矩阵
        R1             输出矩阵，第一个摄像机的校正变换矩阵（旋转变换）
        R2             输出矩阵，第二个摄像机的校正变换矩阵（旋转矩阵）
        P1             输出矩阵，第一个摄像机在新坐标系下的投影矩阵
        P2             输出矩阵，第二个摄像机在想坐标系下的投影矩阵
        Q              4*4的深度差异映射矩阵
        flags          可选的标志有两种零或者 CV_CALIB_ZERO_DISPARITY ,如果设置 CV_CALIB_ZERO_DISPARITY 的话，该函数会让两幅校正后的图像的主点有相同的像素坐标。否则该函数会水平或垂直的移动图像，以使得其有用的范围最大
        alpha          拉伸参数。如果设置为负或忽略，将不进行拉伸。如果设置为0，那么校正后图像只有有效的部分会被显示（没有黑色的部分），如果设置为1，那么就会显示整个图像。设置为0~1之间的某个值，其效果也居于两者之间。
        newImageSize   校正后的图像分辨率，默认为原分辨率大小。
        validPixROI1   可选的输出参数，Rect型数据。其内部的所有像素都有效
        validPixROI2   可选的输出参数，Rect型数据。其内部的所有像素都有效
    """

    '''
        Rl, Rr, Pl, Pr, Q, validPixROIl, validPixROIr =  cv2.stereoRectify(cameraModel['Ml'], cameraModel['dl'], cameraModel['Mr'], cameraModel['dr'], (w ,h), cameraModel['R'], cameraModel['T'])
        mapl_1, mapl_2 = cv2.initUndistortRectifyMap(cameraModel['Ml'], cameraModel['dl'], Rl, Pl, (w,h), cv2.CV_32FC1)
        mapr_1, mapr_2 = cv2.initUndistortRectifyMap(cameraModel['Mr'], cameraModel['dr'], Rr, Pr, (w,h), cv2.CV_32FC1)
        rltl = cv2.remap(grayL, mapl_1, mapl_2, cv2.INTER_LINEAR)
        rltr = cv2.remap(grayR, mapr_1, mapr_2, cv2.INTER_LINEAR)
    '''

    """
        getOptimalNewCameraMatrix() 用于去除畸变矫正后图像四周黑色的区域
        getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, alpha[, newImgSize[, centerPrincipalPoint]]) -> retval, validPixROI
        InputArray 	cameraMatrix,                     // 原相机内参矩阵
        InputArray 	distCoeffs,                       // 原相机畸变参数
        Size 	        imageSize,                    // 图像尺寸
        double 	        alpha,                        // 缩放比例  //当alpha=1时，所有像素均保留，但存在黑色边框; //当alpha=0时，损失最多的像素，没有黑色边框。		            
        Size 	        newImgSize = Size(),          // 校正后的图像尺寸
        Rect * 	        validPixROI = 0,              // 输出感兴趣区域设置
        bool 	        centerPrincipalPoint = false  // 可选标志
        https://blog.csdn.net/weixin_48592526/article/details/120393764
        ------------------------------------------------------------------------------------------------------------------------------------------
        undistort(src, cameraMatrix, distCoeffs[, dst[, newCameraMatrix]]) -> dst
        对图像进行变换，以补偿径向和切向的镜头失真,该函数是 initUndistortRectifyMap 和 remap 函数的简单组合
        @INPUT
        @param src 输入（扭曲的）图像。
        @param cameraMatrix 输入相机矩阵A
        @param distCoeffs 输入失真系数的向量的4、5、8、12或14个元素。如果该向量为NULL/空，则假定失真系数为零。
        @param newCameraMatrix 扭曲图像的相机矩阵。默认情况下，它与cameraMatrix相同，但你可以通过使用不同的矩阵来对结果进行额外的缩放和移动。
        @OUTPUT
        @param dst 输出（修正的）图像，其大小和类型与 src 相同 .
        -------------------------------------------------------------------------------------------------------------------------------------------
        concatenate((a1, a2, ...), axis=0, out=None, dtype=None, casting="same_kind")
        沿着一个现有的轴连接一个数组序列
        a1, a2, ...    :   类似数组的序列，这些数组必须具有相同的形状，除了在对应的 "轴"（默认是第一个）
        axis （int, optional）      数组拼接方向选择 0-按列拼接 1-按行拼接 NONE-压缩成1×n的array，n为元素数量
        out  （ndarray, 可选）      如果提供，是放置结果的目的地。其形状必须是形状必须正确，与没有指定out参数时concatenate将返回的结果相匹配的out参数
        dtype : str或dtype如果提供，目标数组将具有该dtype。不能和`out`一起提供。
        casting : {'no', 'equiv', 'safe', 'same_kind', 'unsafe'}, optional    控制可能发生的数据转换。默认为'same_kind'。
    """
        # 图像输出输出尺寸保持不变 获取新左右相机的内参矩阵
        newcameramtxL ,roiL = cv2.getOptimalNewCameraMatrix(cameraModel['Ml'], cameraModel['dl'] ,(w ,h) ,1 ,(w ,h))
        newcameramtxR ,roiR = cv2.getOptimalNewCameraMatrix(cameraModel['Mr'], cameraModel['dr'] ,(w ,h) ,1 ,(w ,h))

		dstL = cv2.undistort(grayL, cameraModel['Ml'], cameraModel['dl'], None, newcameramtxL)
        dstR = cv2.undistort(grayR, cameraModel['Mr'], cameraModel['dr'], None, newcameramtxR)

        # 保存矫正图像
        recPath = folder + "/rec"
        os.makedirs(recPath, exist_ok=True)
        # 按行（行拓展）拼接两个数组
        rlt = np.concatenate((dstL, dstR), axis=1)
        # rlt[::40, :] = 0
        cv2.imwrite(recPath + "/" + png_list_l[i], rlt)
        # 生成深度图
        # stereo = cv2.StereoBM_create(numDisparities=16, blockSize=15)
        # disparity = stereo.compute(grayL, grayR)  # 用原始图像

        # disp = cv2.normalize(disparity, disparity, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U)
        # 将图片扩展至3d空间中，其z方向的值则为当前的距离
        # threeD = cv2.reprojectImageTo3D(disparity.astype(np.float32 ) /16., Q)
        # cv2.imwrite(rlt_dir + "\\depth.jpg", disp)

    return 1



if __name__ == "__main__":

    row_num = 7
    col_num = 7
    # 相机1标定图片路径
    img_dir_L = r"./INR"
    # 相机2标定图片路径
    img_dir_R = r"./RGB"
    # 实际单个棋盘格物理尺寸
    square_sz = 15
    # 标定相机
    # 将相机1姿态变化为相机2
    cameraModel ,folder = stereo_calib_calibrate(img_dir_R ,img_dir_L ,row_num ,col_num , square_sz)
    new_img_dir_R = folder + "/pre"
    # 矫正图片
    ret = stereo_calib_correct(new_img_dir_R , img_dir_L, cameraModel,folder)