python+openCV實現雙目視差圖及測距

通過matlab標定得到相機參數放到stereoconfig.py

import numpy as np
import cv2

#雙目相機參數
class stereoCameral(object):
    def __init__(self):

        #左相機內參數
        self.cam_matrix_left = np.array([[249.82379, 0., 156.38459], [0., 249.07678, 122.46872], [0., 0., 1.]])
        #右相機內參數
        self.cam_matrix_right = np.array([[242.77875, 0., 153.22330], [0., 242.27426, 117.63536], [0., 0., 1.]])

        #左右相機畸變系數:[k1, k2, p1, p2, k3]
        self.distortion_l = np.array([[-0.02712, -0.03795, -0.00409, 0.00526, 0.00000]])
        self.distortion_r = np.array([[-0.03348, 0.08901, -0.00327, 0.00330, 0.00000]])

        #旋轉矩陣
        om = np.array([-0.00320, -0.00163, -0.00069])
        self.R = cv2.Rodrigues(om)[0]  # 使用Rodrigues變換將om變換為R
        #平移矩陣
        self.T = np.array([-90.24602, 3.17981, -19.44558])

視差圖及三維坐標

import cv2
import numpy as np
import stereoconfig

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

    #計算校正變換
    if type(height) != "int" or type(width) != "int":
        height = int(height)
        width = int(width)
    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)
    rectifyed_img2 = cv2.remap(image2, map2x, map2y, cv2.INTER_AREA)
    return rectifyed_img1, rectifyed_img2

#視差計算
def sgbm(imgL, imgR):
    #SGBM參數設置
    blockSize = 8
    img_channels = 3
    stereo = cv2.StereoSGBM_create(minDisparity = 1,
                                   numDisparities = 64,
                                   blockSize = blockSize,
                                   P1 = 8 * img_channels * blockSize * blockSize,
                                   P2 = 32 * img_channels * blockSize * blockSize,
                                   disp12MaxDiff = -1,
                                   preFilterCap = 1,
                                   uniquenessRatio = 10,
                                   speckleWindowSize = 100,
                                   speckleRange = 100,
                                   mode = cv2.STEREO_SGBM_MODE_HH)
    # 計算視差圖
    disp = stereo.compute(imgL, imgR)
    disp = np.divide(disp.astype(np.float32), 16.)#除以16得到真實視差圖
    return disp
#計算三維坐標，並刪除錯誤點
def threeD(disp, Q):
    # 計算像素點的3D坐標（左相機坐標系下）
    points_3d = cv2.reprojectImageTo3D(disp, Q) 

    points_3d = points_3d.reshape(points_3d.shape[0] * points_3d.shape[1], 3)

    X = points_3d[:, 0]
    Y = points_3d[:, 1]
    Z = points_3d[:, 2]

    #選擇並刪除錯誤的點
    remove_idx1 = np.where(Z <= 0)
    remove_idx2 = np.where(Z > 15000)
    remove_idx3 = np.where(X > 10000)
    remove_idx4 = np.where(X < -10000)
    remove_idx5 = np.where(Y > 10000)
    remove_idx6 = np.where(Y < -10000)
    remove_idx = np.hstack(
        (remove_idx1[0], remove_idx2[0], remove_idx3[0], remove_idx4[0], remove_idx5[0], remove_idx6[0]))

    points_3d = np.delete(points_3d, remove_idx, 0)

    #計算目標點（這里我選擇的是目標區域的中位數，可根據實際情況選取）
    if points_3d.any():
        x = np.median(points_3d[:, 0])
        y = np.median(points_3d[:, 1])
        z = np.median(points_3d[:, 2])
        targetPoint = [x, y, z]
    else:
        targetPoint = [0, 0, -1]#無法識別目標區域

    return targetPoint


imgL = cv2.imread("_left.jpg")
imgR = cv2.imread("_right.jpg")

height, width = imgL.shape[0:2]
# 讀取相機內參和外參
config = stereoconfig.stereoCameral()

map1x, map1y, map2x, map2y, Q = getRectifyTransform(height, width, config)
iml_rectified, imr_rectified = rectifyImage(imgL, imgR, map1x, map1y, map2x, map2y)

disp = sgbm(iml_rectified, imr_rectified)
cv2.imshow("disp", disp)
target_point = threeD(disp, Q)#計算目標點的3D坐標（左相機坐標系下）
print(target_point)