双目视觉离线测量空间三维坐标带详细注释_matlab双目测量坐标

直接上代码：

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)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114

代码中的示例图片和参数详见链接。