双目测距python_python+openCV实现双目视差图及测距

importcv2importnumpy as npimportstereoconfigdefgetRectifyTransform(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)returnmap1x, map1y, map2x, map2y, Q#畸变校正和立体校正

defrectifyImage(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)returnrectifyed_img1, rectifyed_img2#视差计算

defsgbm(imgL, imgR):#SGBM参数设置

blockSize = 8img_channels= 3stereo= 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得到真实视差图

returndisp#计算三维坐标，并删除错误点

defthreeD(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)#计算目标点(这里我选择的是目标区域的中位数，可根据实际情况选取)

ifpoints_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]#无法识别目标区域

returntargetPoint

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)