双目视觉离线测量空间三维坐标带详细注释-Toy模板网

这篇具有很好参考价值的文章主要介绍了双目视觉离线测量空间三维坐标带详细注释。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

直接上代码：

import numpy as np
import cv2
import glob


# 函数功能：通过双目相机的内外参数和畸变系数进行立体校正，获得去畸变后的双目参数
def get_stereo_rectify_image_from_camera_parameters(P, gray_imageL, gray_imageR):

    # 左、右相机内参
    mtx_l = np.array([[P[0], 0, P[1], 0],
                      [0, P[2], P[3], 0],
                      [0, 0, 1, 0]])
    mtx_r = np.array([[P[4], 0, P[5], 0],
                      [0, P[6], P[7], 0],
                      [0, 0, 1, 0]])
    # 右相机到左相机的旋转矩阵、平移矩阵
    R_lr = np.array([[P[8], P[9], P[10]],
                     [P[11], P[12], P[13]],
                     [P[14], P[15], P[16]]])
    T_lr = np.array([[P[17]],
                     [P[18]],
                     [P[19]]])
    cameraMatrixL = mtx_l[:, 0:3]
    cameraMatrixR = mtx_r[:, 0:3]
    # 左、右相机畸变
    distCoeffL = np.array([P[20], P[21], P[22], P[23], P[24]])
    distCoeffR = np.array([P[25], P[26], P[27], P[28], P[29]])
    # 左相机到左相机的投影矩阵
    R_ll = ([[1, 0, 0],
             [0, 1, 0],
             [0, 0, 1]])
    T_ll = ([[0], [0], [0]])
    temp_R_ll = np.append(R_ll, T_ll, axis=1)
    _temp_R_ll = np.row_stack((temp_R_ll, [0, 0, 0, 1]))
    P0 = np.dot(mtx_l, _temp_R_ll)
    # 左相机到右相机的投影矩阵
    temp_R_lr = np.append(R_lr, T_lr, axis=1)
    _temp_R_lr = np.row_stack((temp_R_lr, [0, 0, 0, 1]))
    P1 = np.dot(mtx_r, _temp_R_lr)

    # 图像的分辨率
    imageSize = (gray_imageL.shape[1], gray_imageL.shape[0])
    # # 立体校正
    Rl, Rr, Pl, Pr, Q, validROIL, validROIR = cv2.stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, R_lr, T_lr, flags=0, alpha=0, newImageSize=(0, 0))
    # 计算更正remap
    mapLx, mapLy = cv2.initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pl, imageSize, cv2.CV_32FC1)
    mapRx, mapRy = cv2.initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, cv2.CV_32FC1)
    # 经过remap之后，左右相机的图像已经共面并且行对齐
    rectifyImageL = cv2.remap(gray_imageL, mapLx, mapLy, cv2.INTER_LINEAR)
    rectifyImageR = cv2.remap(gray_imageR, mapRx, mapRy, cv2.INTER_LINEAR)

    # 返回值分别为：左、右相机投影矩阵，校正后的左、右相机投影矩阵，校正后的左、右相机图像
    return P0, P1, Pl, Pr, rectifyImageL, rectifyImageR


# 函数功能：通过双目相机的参数和相机拍摄的光斑的左、右相机图像，计算出光斑三维坐标
def cal_coordinate_from_spot_centroid(cameraParameters, imageL, imageR):

    # 读取相机拍摄的光斑图像，及相机内外参的txt文件
    imagesL = glob.glob(imageL)
    imagesR = glob.glob(imageR)
    file = open(cameraParameters, 'r')
    P = (np.array([x.strip() for x in file.readlines()])).astype(np.float64)

    for imgL in imagesL:
        listL = []
        listR = []

        grayImageL = cv2.imread(imgL, 0)
        imgR = imgL.replace('L', 'R')
        grayImageR = cv2.imread(imgR, 0)

        # 对左右相机拍摄的图像进行立体校正处理
        P0, P1, Pl, Pr, rectifyimgL, rectifyimgR = get_stereo_rectify_image_from_camera_parameters(P, grayImageL, grayImageR)
        # 阈值分割
        ret, thr = cv2.threshold(rectifyimgL, 175, 255, cv2.THRESH_BINARY)
        # 找到图像轮廓并画出来
        contoursL, hie = cv2.findContours(thr, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
        cv2.drawContours(rectifyimgL, contours=contoursL, contourIdx=-1, color=[0, 0, 255], thickness=2)
        contoursL = [cnt for cnt in contoursL if cv2.contourArea(cnt) > 30]
        # 计算图像质心坐标
        for index in range(len(contoursL)):
            ML = cv2.moments(contoursL[index])
            cxL = round(ML['m10'] / ML['m00'], 3)
            cyL = round(ML['m01'] / ML['m00'], 3)
            centerlistL = [cxL, cyL]
            listL.append(centerlistL)

        ret, thr = cv2.threshold(rectifyimgR, 175, 255, cv2.THRESH_BINARY)
        contoursR, hie = cv2.findContours(thr, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
        cv2.drawContours(rectifyimgR, contours=contoursR, contourIdx=-1, color=[0, 0, 255], thickness=2)
        contoursR = [cnt for cnt in contoursR if cv2.contourArea(cnt) > 30]
        for index in range(len(contoursR)):
            MR = cv2.moments(contoursR[index])
            cxR = round(MR['m10'] / MR['m00'], 3)
            cyR = round(MR['m01'] / MR['m00'], 3)
            centerlistR = [cxR, cyR]
            listR.append(centerlistR)

        # 计算光斑的三维坐标
        for i in range(len(listL)):
            s1 = np.array(cv2.triangulatePoints(Pl, Pr, np.array(listL[i]), np.array(listR[i]))).T
            point_3D = s1[0][:-1] / np.max(s1[0][-1])
            point_3D = ("%.3f" % float(point_3D[0]), "%.3f" % float(point_3D[1]), "%.3f" % float(point_3D[2]))
            print('光斑{}的三维空间坐标为：{}'.format(i + 1, point_3D))


if __name__ == "__main__":
    # 读取相机内外参的txt文件，包括双目相机的内外参和畸变参数
    camera_parameters = '0524.txt'
    # 读取相机拍摄的光斑图像
    image_L = 'L1.bmp'
    image_R = 'R1.bmp'
    cal_coordinate_from_spot_centroid(camera_parameters, image_L, image_R)