[学习记录]双目相机 极线校正python代码

写个文章以免忘记

参考https://blog.csdn.net/qq_40700822/article/details/115765728?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522167509054916800215088614%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fblog.%2522%257D&request_id=167509054916800215088614&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~blog~first_rank_ecpm_v1~rank_v31_ecpm-1-115765728-null-null.blog_rank_default&utm_term=%E5%8F%8C%E7%9B%AE%E8%A7%86%E8%A7%89&spm=1018.2226.3001.4450

ZED双目相机，matlab工具箱棋盘格标定后的参数代码如下

mystereoconfig_040_2

import numpy as np


####################仅仅是一个示例###################################


# 双目相机参数
class stereoCamera(object):
    def __init__(self):
        # 左相机内参
        self.cam_matrix_left = np.array([   [427.5903,         0,  336.0246],
                                            [       0,  421.7347,  196.9884],
                                            [       0,         0,         1]
                                        ])
        # 右相机内参
        self.cam_matrix_right = np.array([  [424.6221,         0,  325.2774],
                                            [       0,  418.6604,  193.3607],
                                            [       0,         0,         1]
                                        ])

        # 左右相机畸变系数:[k1, k2, p1, p2, k3]
        self.distortion_l = np.array([[-0.1438, -0.0389, 0, 0, 0]])
        self.distortion_r = np.array([[-0.1627,  0.0218, 0, 0, 0]])

        # 旋转矩阵
        self.R = np.array([ [1.0000  , -0.00014492, -0.00015   ],
                           [0.0001438, 1.0000     , -0.00075485],
                           [0.0015   , 0.00075464 , 1.0000     ]
                            ])

        # 平移矩阵
        self.T = np.array([[-119.7388], [0.8975], [-3.8558]])

        # 焦距
        #self.focal_length = 312.0959 # 默认值，一般取立体校正后的重投影矩阵Q中的 Q[2,3]

        # 基线距离
        self.baseline = 119.7388 # 单位：mm， 为平移向量的第一个参数（取绝对值）

        

极线校正代码如下

mylitijiaozheng

# -*- coding: utf-8 -*-

import cv2
import numpy as np
import mystereoconfig_040_2  # 导入相机标定的参数
import matplotlib.pyplot as plt
from PIL import Image

# # 用arrays存储所有图片的object points 和 image points
# objpoints = []  # 3d points in real world space
# imgpointsR = []  # 2d points in image plane
# imgpointsL = []

image1 = cv2.imread(r'D:\dht\left_8', 0)  # 右视图
image2 = cv2.imread(r'D:\dht\right_8', 0)  # 左视图

# # 获取畸变校正和立体校正的映射变换矩阵、重投影矩阵
# # @param：config是一个类，存储着双目标定的参数:config = stereoconfig.stereoCamera()
def getRectifyTransform(height, width, config):
    # 读取内参和外参
    left_K = config.cam_matrix_left
    right_K = config.cam_matrix_right
    left_distortion = config.distortion_l
    right_distortion = config.distortion_r
    R = config.R
    T = config.T

    #    # 计算校正变换
    R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(left_K, left_distortion, right_K, right_distortion,
                                                      (width, height), R, T, alpha=0)

    map1x, map1y = cv2.initUndistortRectifyMap(left_K, left_distortion, R1, P1, (width, height), cv2.CV_32FC1)
    map2x, map2y = cv2.initUndistortRectifyMap(right_K, right_distortion, R2, P2, (width, height), cv2.CV_32FC1)

    return map1x, map1y, map2x, map2y, Q


# # 畸变校正和立体校正
def rectifyImage(image1, image2, map1x, map1y, map2x, map2y):
    rectifyed_img1 = cv2.remap(image1, map1x, map1y, cv2.INTER_AREA)
    im_L = Image.fromarray(rectifyed_img1)  # numpy 转 image类
    rectifyed_img2 = cv2.remap(image2, map2x, map2y, cv2.INTER_AREA)
    im_R = Image.fromarray(rectifyed_img2)  # numpy 转 image 类

    return rectifyed_img1, rectifyed_img2


# 立体校正检验----画线
def draw_line(image1, image2):
    # 建立输出图像
    height = max(image1.shape[0], image2.shape[0])
    width = image1.shape[1] + image2.shape[1]

    output = np.zeros((height, width, 3), dtype=np.uint8)
    output[0:image1.shape[0], 0:image1.shape[1]] = image1
    output[0:image2.shape[0], image1.shape[1]:] = image2

    # 绘制等间距平行线
    line_interval = 40  # 直线间隔：50
    for k in range(height // line_interval):
        cv2.line(output, (0, line_interval * (k + 1)), (2 * width, line_interval * (k + 1)), (0, 255, 0), thickness=2,
                 lineType=cv2.LINE_AA)

    return output

    #ght = disparity_right.astype(np.float32) / 16.

    #return trueDisp_left, trueDisp_right

if __name__ == '__main__':

    i = 8
    string = 're'
    # 读取数据集的图片
    t = str(i)
    iml = cv2.imread(r'D:\dht\left_' + t + '.jpg', 1)  # 左图
    imr = cv2.imread(r'D:\dht\right_' + t + '.jpg', 1)  # 右图
    height, width = iml.shape[0:2]

    print("width = %d \n" % width)
    print("height = %d \n" % height)

    # 读取相机内参和外参
    config = mystereoconfig_040_2.stereoCamera()

    # 立体校正
    map1x, map1y, map2x, map2y, Q = getRectifyTransform(height, width, config)  # 获取用于畸变校正和立体校正的映射矩阵以及用于计算像素空间坐标的重投影矩阵
    iml_rectified, imr_rectified = rectifyImage(iml, imr, map1x, map1y, map2x, map2y)

    print("Print Q!")
    print(Q)

    # 绘制等间距平行线，检查立体校正的效果
    line = draw_line(iml, imr)
    cv2.imwrite('aaa.jpg',line)
    #cv2.imwrite('./%s检验%d.png' % (string, i), line)
    # points_3d = points_3d

    # 显示图片
    cv2.waitKey(0)
    cv2.destroyAllWindows()

    plt.savefig(r'D:\dht\check' + t + '.jpg')
    plt.show()

原图 左目右目

极限校正后