Python-OpenCV双目测距代码实现以及参数解读_stereoconfig

一、双目相机拍照后使用Matlab进行双目标定

必看： USB双目相机的具体标定过程：https://blog.csdn.net/qq_40700822/article/details/124251201?spm=1001.2014.3001.5501

主要参考：https://blog.csdn.net/dulingwen/article/details/98071584
感谢大佬的分享！！！(*≧ω≦)！！

二、标定后生成深度图，之后在进行测距（这里主要是对双目参数的解读）

1、导入相关库和相机的标定参数

首先导入需要的相关库以及双目相机标定的各项参数：

 # -*- coding: utf-8 -*-
import cv2
import numpy as np
import stereoconfig_040_2 #导入相机标定的参数
import pcl
import pcl.pcl_visualization
1
2
3
4
5
6

首先对导入的左右相机的图片进行预处理，一般可以削弱光照不均的影响，使得两个图片的曝光值差距缩小。
小知识：python-opencv读取的灰度图像是二维列表（数组）,彩色图像是三位列表（数组）。
.ndim返回的是数组的维度，返回的只有一个数，该数即表示数组的维度。
参考：https://blog.csdn.net/mokeding/article/details/17599585

#像素的访问和访问numpy中ndarray的方法完全一样
img[j,i] = 255  # 灰度图访问;j,i 分别表示图像的行和列 即 j * i 二维
 
#BGR 图像访问 彩色 BGR 图像 为 j * i * 3 三维
img[j,i,0] = 255   # 0 -- 为通道，指B通道
img[j,i,1] = 255   # 1 -- 为通道，指G通道
img[j,i,2] = 255   # 2 -- 为通道，指R通道
1
2
3
4
5
6
7

2、图像预处理

# 预处理
def preprocess(img1, img2):
    # 彩色图->灰度图
    if(img1.ndim == 3): #判断是否为三维数组
        img1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY)  # 通过OpenCV加载的图像通道顺序是BGR
    if(img2.ndim == 3):#判断是否为三维数组
        img2 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)

    # 直方图均衡
    # 当然这里也可以按需对图像进行处理
    img1 = cv2.equalizeHist(img1)
    img2 = cv2.equalizeHist(img2)

    return img1, img2
1
2
3
4
5
6
7
8
9
10
11
12
13
14

3、消除图像的畸变

cv.undistort()消除畸变函数参数含义
参考OpenCV官网：https://docs.opencv.org/master/d9/d0c/group__calib3d.html#ga69f2545a8b62a6b0fc2ee060dc30559d

cv.undistort(	src, cameraMatrix, distCoeffs[, dst[, newCameraMatrix]]	) ->	dst
1

各个参数的含义：

消除畸变函数的定义：

# 消除畸变
def undistortion(image, camera_matrix, dist_coeff):
    undistortion_image = cv2.undistort(image, camera_matrix, dist_coeff)

    return undistortion_image
1
2
3
4
5

调用示例，不能直接使用（stereoconfig_040_2.py完整代码见文末）：

 # 读取相机内参和外参
config = stereoconfig_040_2.stereoCamera()
i = 3
string = 'Val'
# 读取数据集的图片
iml = cv2.imread('./%sLeft%d.bmp' %(string,i) )  # 左图
imr = cv2.imread('./%sRight%d.bmp'%(string,i) )  # 右图
iml = undistortion(iml, config.cam_matrix_left, config.distortion_l)
imr = undistortion(imr, config.cam_matrix_right, config.distortion_r)

#cv2.undistort（）的dist_coeff参数的形式
# 左右相机畸变系数:[k1, k2, p1, p2, k3]
#config.distortion_l = np.array([[-0.0806, 0.3806, -0.0033, 0.0005148, -0.5229]])
#config.distortion_r = np.array([[-0.0485, 0.2200, -0.002,  0.0017,    -0.2876]])
1
2
3
4
5
6
7
8
9
10
11
12
13
14

4、立体校正

# 获取畸变校正和立体校正的映射变换矩阵、重投影矩阵
# @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


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

cv.stereoRectify()参数含义：
官网定义的形式：

cv.stereoRectify(
cameraMatrix1, 
distCoeffs1, 
cameraMatrix2, 
distCoeffs2, 
imageSize, 
R, 
T[, R1
 [, R2
 [, P1
 [, P2
 [, Q
 [, flags
 [, alpha
 [, newImageSize]]]]]]]]	
 ) ->	R1, R2, P1, P2, Q, validPixROI1, validPixROI2
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

cv.stereoRectify()参数的含义：

cv.initUndistortRectifyMap(	
cameraMatrix, 
distCoeffs, 
R, 
newCameraMatrix, 
size, 
m1type[, map1
	  [, map2]
	  ]	
  ) ->	map1, map2
1
2
3
4
5
6
7
8
9
10

cv.initUndistortRectifyMap()函数各个参数的含义：
参考OpenCV官网：
https://docs.opencv.org/master/d9/d0c/group__calib3d.html#ga7dfb72c9cf9780a347fbe3d1c47e5d5a

5、视差计算

# 视差计算
def stereoMatchSGBM(left_image, right_image, down_scale=False):
    # SGBM匹配参数设置
    if left_image.ndim == 2:
        img_channels = 1
    else:
        img_channels = 3
        
    blockSize = 3
    paraml = {'minDisparity': 0,
             'numDisparities': 128,
             'blockSize': blockSize,
             'P1': 8 * img_channels * blockSize ** 2,
             'P2': 32 * img_channels * blockSize ** 2,
             'disp12MaxDiff': 1,
             'preFilterCap': 63,
             'uniquenessRatio': 15,
             'speckleWindowSize': 100,
             'speckleRange': 1,
             'mode': cv2.STEREO_SGBM_MODE_SGBM_3WAY
             }

    # 构建SGBM对象
    left_matcher = cv2.StereoSGBM_create(**paraml)
    paramr = paraml
    paramr['minDisparity'] = -paraml['numDisparities']
    right_matcher = cv2.StereoSGBM_create(**paramr)

    # 计算视差图
    size = (left_image.shape[1], left_image.shape[0])
    if down_scale == False:
        disparity_left = left_matcher.compute(left_image, right_image)
        disparity_right = right_matcher.compute(right_image, left_image)

    else:
        left_image_down = cv2.pyrDown(left_image)
        right_image_down = cv2.pyrDown(right_image)
        factor = left_image.shape[1] / left_image_down.shape[1]

        disparity_left_half = left_matcher.compute(left_image_down, right_image_down)
        disparity_right_half = right_matcher.compute(right_image_down, left_image_down)
        disparity_left = cv2.resize(disparity_left_half, size, interpolation=cv2.INTER_AREA)
        disparity_right = cv2.resize(disparity_right_half, size, interpolation=cv2.INTER_AREA)
        disparity_left = factor * disparity_left
        disparity_right = factor * disparity_right

    # 真实视差（因为SGBM算法得到的视差是×16的）
    trueDisp_left = disparity_left.astype(np.float32) / 16.
    trueDisp_right = disparity_right.astype(np.float32) / 16.

    return trueDisp_left, trueDisp_right

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

Python-openCV 中cv2.StereoSGBM_create()参数的含义：

参考：https://docs.opencv.org/trunk/d2/d85/classcv_1_1StereoSGBM.html
参考OpenCV官网：https://docs.opencv.org/trunk/d2/d85/classcv_1_1StereoSGBM.html
cv2.StereoSGBM_create()的SGBM算法的定义：

cv2.StereoSGBM_create(
[，minDisparity 
[，numDisparities 
[，blockSize 
[，P1 
[，P2 
[，disp12MaxDiff 
[，preFilterCap 
[，uniquenessRatio 
[，speckleWindowSize 
[，speckleRange 
[，mode]]]]]]]]]]]]
)
1
2
3
4
5
6
7
8
9
10
11
12
13

各个参数的含义：

Python-openCV 中cv2.pyrDown()中的参数和含义：
参考OpenCV官网：https://docs.opencv.org/master/d4/d86/group__imgproc__filter.html#gaf9bba239dfca11654cb7f50f889fc2ff

cv.pyrDown(	src[, dst[, dstsize[, borderType]]]	) ->	dst
1

6、其他函数的参数以及含义

reprojectImageTo3D的定义：

cv.reprojectImageTo3D(	
disparity, 
Q
[, _3dImage
[, handleMissingValues
[, ddepth]]]	
 ) ->	_3dImage
1
2
3
4
5
6
7

cv2.reprojectImageTo3D()参数以及其含义：
参考官网：https://docs.opencv.org/master/d9/d0c/group__calib3d.html#ga1bc1152bd57d63bc524204f21fde6e02

三、双目测距代码的实现

1、stereoconfig_040_2.py–相机标定的参数

import numpy as np


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


# 双目相机参数
class stereoCamera(object):
    def __init__(self):
        # 左相机内参
        self.cam_matrix_left = np.array([   [830.5873,   -3.0662,  658.1007],
                                            [       0,  830.8116,  482.9859],
                                            [       0,         0,         1]
                                        ])
        # 右相机内参
        self.cam_matrix_right = np.array([  [830.4255,   -3.5852,  636.8418],
                                            [       0,  830.7571,  476.0664],
                                            [       0,         0,         1]
                                        ])

        # 左右相机畸变系数:[k1, k2, p1, p2, k3]
        self.distortion_l = np.array([[-0.0806, 0.3806, -0.0033, 0.0005148, -0.5229]])
        self.distortion_r = np.array([[-0.0485, 0.2200, -0.002,  0.0017,    -0.2876]])

        # 旋转矩阵
        self.R = np.array([ [      1,  0.0017, -0.0093],
                            [-0.0018,  1.0000, -0.0019],
                            [ 0.0093,  0.0019,  1.0000]   
                            ])

        # 平移矩阵
        self.T = np.array([[-119.9578], [0.1121], [-0.2134]])

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

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

       


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

2、dianyuntu.py–测距实现代码

 # -*- coding: utf-8 -*-
import cv2
import numpy as np
import stereoconfig_040_2   #导入相机标定的参数
import pcl
import pcl.pcl_visualization


# 预处理
def preprocess(img1, img2):
    # 彩色图->灰度图
    if(img1.ndim == 3):#判断为三维数组
        img1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY)  # 通过OpenCV加载的图像通道顺序是BGR
    if(img2.ndim == 3):
        img2 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)

    # 直方图均衡
    img1 = cv2.equalizeHist(img1)
    img2 = cv2.equalizeHist(img2)

    return img1, img2


# 消除畸变
def undistortion(image, camera_matrix, dist_coeff):
    undistortion_image = cv2.undistort(image, camera_matrix, dist_coeff)

    return undistortion_image


# 获取畸变校正和立体校正的映射变换矩阵、重投影矩阵
# @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)
    rectifyed_img2 = cv2.remap(image2, map2x, map2y, cv2.INTER_AREA)

    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 = 50  # 直线间隔：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


# 视差计算
def stereoMatchSGBM(left_image, right_image, down_scale=False):
    # SGBM匹配参数设置
    if left_image.ndim == 2:
        img_channels = 1
    else:
        img_channels = 3
    blockSize = 3
    paraml = {'minDisparity': 0,
             'numDisparities': 128,
             'blockSize': blockSize,
             'P1': 8 * img_channels * blockSize ** 2,
             'P2': 32 * img_channels * blockSize ** 2,
             'disp12MaxDiff': 1,
             'preFilterCap': 63,
             'uniquenessRatio': 15,
             'speckleWindowSize': 100,
             'speckleRange': 1,
             'mode': cv2.STEREO_SGBM_MODE_SGBM_3WAY
             }

    # 构建SGBM对象
    left_matcher = cv2.StereoSGBM_create(**paraml)
    paramr = paraml
    paramr['minDisparity'] = -paraml['numDisparities']
    right_matcher = cv2.StereoSGBM_create(**paramr)

    # 计算视差图
    size = (left_image.shape[1], left_image.shape[0])
    if down_scale == False:
        disparity_left = left_matcher.compute(left_image, right_image)
        disparity_right = right_matcher.compute(right_image, left_image)

    else:
        left_image_down = cv2.pyrDown(left_image)
        right_image_down = cv2.pyrDown(right_image)
        factor = left_image.shape[1] / left_image_down.shape[1]

        disparity_left_half = left_matcher.compute(left_image_down, right_image_down)
        disparity_right_half = right_matcher.compute(right_image_down, left_image_down)
        disparity_left = cv2.resize(disparity_left_half, size, interpolation=cv2.INTER_AREA)
        disparity_right = cv2.resize(disparity_right_half, size, interpolation=cv2.INTER_AREA)
        disparity_left = factor * disparity_left
        disparity_right = factor * disparity_right

    # 真实视差（因为SGBM算法得到的视差是×16的）
    trueDisp_left = disparity_left.astype(np.float32) / 16.
    trueDisp_right = disparity_right.astype(np.float32) / 16.

    return trueDisp_left, trueDisp_right


# 将h×w×3数组转换为N×3的数组
def hw3ToN3(points):
    height, width = points.shape[0:2]

    points_1 = points[:, :, 0].reshape(height * width, 1)
    points_2 = points[:, :, 1].reshape(height * width, 1)
    points_3 = points[:, :, 2].reshape(height * width, 1)

    points_ = np.hstack((points_1, points_2, points_3))

    return points_


# 深度、颜色转换为点云
def DepthColor2Cloud(points_3d, colors):
    rows, cols = points_3d.shape[0:2]
    size = rows * cols

    points_ = hw3ToN3(points_3d)
    colors_ = hw3ToN3(colors).astype(np.int64)

    # 颜色信息
    blue = colors_[:, 0].reshape(size, 1)
    green = colors_[:, 1].reshape(size, 1)
    red = colors_[:, 2].reshape(size, 1)

    rgb = np.left_shift(blue, 0) + np.left_shift(green, 8) + np.left_shift(red, 16)

    # 将坐标+颜色叠加为点云数组
    pointcloud = np.hstack((points_, rgb)).astype(np.float32)

    # 删掉一些不合适的点
    X = pointcloud[:, 0]
    Y = -pointcloud[:, 1]
    Z = -pointcloud[:, 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]))

    pointcloud_1 = np.delete(pointcloud, remove_idx, 0)

    return pointcloud_1


# 点云显示
def view_cloud(pointcloud):
    cloud = pcl.PointCloud_PointXYZRGBA()
    cloud.from_array(pointcloud)

    try:
        visual = pcl.pcl_visualization.CloudViewing()
        visual.ShowColorACloud(cloud)
        v = True
        while v:
            v = not (visual.WasStopped())
    except:
        pass


if __name__ == '__main__':

    i = 3
    string = 're'
    # 读取数据集的图片
    iml = cv2.imread('./%sLift%d.bmp' %(string,i) )  # 左图
    imr = cv2.imread('./%sRight%d.bmp'%(string,i) ) # 右图
    height, width = iml.shape[0:2]

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

    # 读取相机内参和外参
    config = stereoconfig_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_rectified, imr_rectified)
    cv2.imwrite('./%s检验%d.png' %(string,i), line)

    # 消除畸变
    iml = undistortion(iml, config.cam_matrix_left, config.distortion_l)
    imr = undistortion(imr, config.cam_matrix_right, config.distortion_r)

    # 立体匹配
    iml_, imr_ = preprocess(iml, imr)  # 预处理，一般可以削弱光照不均的影响，不做也可以

    iml_rectified_l, imr_rectified_r = rectifyImage(iml_, imr_, map1x, map1y, map2x, map2y)

    disp, _ = stereoMatchSGBM(iml_rectified_l, imr_rectified_r, True) 
    cv2.imwrite('./%s视差%d.png' %(string,i), disp)

    

    # 计算像素点的3D坐标（左相机坐标系下）
    points_3d = cv2.reprojectImageTo3D(disp, Q)  # 可以使用上文的stereo_config.py给出的参数

    #points_3d = points_3d

        # 鼠标点击事件
    def onMouse(event, x, y, flags, param):
        if event == cv2.EVENT_LBUTTONDOWN:
            print('点 (%d, %d) 的三维坐标 (%f, %f, %f)' % (x, y, points_3d[y, x, 0], points_3d[y, x, 1], points_3d[y, x, 2]))
            dis = ( (points_3d[y, x, 0] ** 2 + points_3d[y, x, 1] ** 2 + points_3d[y, x, 2] **2) ** 0.5) / 1000
            print('点 (%d, %d) 距离左摄像头的相对距离为 %0.3f m' %(x, y, dis) )

        # 显示图片
    cv2.namedWindow("disparity",0)
    cv2.imshow("disparity", disp)
    cv2.setMouseCallback("disparity", onMouse, 0)

    

    # 构建点云--Point_XYZRGBA格式
    pointcloud = DepthColor2Cloud(points_3d, iml)

    # 显示点云
    view_cloud(pointcloud)

    cv2.waitKey(0)
    cv2.destroyAllWindows()
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
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261

3、代码打包下载地址

本人瞎写的代码，如有错误，还请见谅！
https://download.csdn.net/download/qq_40700822/18378679

4、查看实现的效果

直接使用python运行上边打包的代码的dianyuntu.py即可，如果报错，就是环境没有搭建好，自行解决吧。

PS. 我的摄像头的分辨率是1920*720，所以效果一般。
得到的视差图：

得到的点云图：