单目及双目代码.zip

import sys import cv2 import numpy as np import stereoconfig import doublecam_calibration import open3d as o3d # 获取畸变校正和立体校正的映射变换矩阵、重投影矩阵 # 该函数的作用是为每个摄像头计算立体校正的映射矩阵，所以其运行结果并不是直接将图片进行立体矫正， # 而是得出进行立体矫正所需要的映射矩阵 # 立体极线校正 # @param：config是一个类，存储着双目标定的参数:config = stereoconfig.stereoCamera() def getRectifyTransform(height, width, config): # 读取内参和外参 cam_matrix_left = config.cam_matrix_left cam_matrix_right = config.cam_matrix_right distortion_l = config.distortion_l distortion_r = config.distortion_r R = config.R T = config.T # 计算校正变换 R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(cam_matrix_left, distortion_l, cam_matrix_right, distortion_r, (width, height), R, T, alpha=0) # 对整幅图像去除畸变，生成映射矩阵 maplx, maply = cv2.initUndistortRectifyMap(cam_matrix_left, distortion_l, R1, P1, (width, height), cv2.CV_16SC2) maprx, mapry = cv2.initUndistortRectifyMap(cam_matrix_right, distortion_r, R2, P2, (width, height), cv2.CV_16SC2) return maplx, maply, maprx, mapry, Q def compute_disp(img_L, img_r, maplx, maply, maprx, mapry): # 立体校正 rectifyed_img_l = cv2.remap(img_L, maplx, maply, cv2.INTER_LANCZOS4, cv2.BORDER_CONSTANT, 0) rectifyed_img_r = cv2.remap(img_r, maprx, mapry, cv2.INTER_LANCZOS4, cv2.BORDER_CONSTANT, 0) # 立体校正检验----画线 def draw_line(rectifyed_img_l, rectifyed_img_r): # 建立输出图像 height = max(rectifyed_img_l.shape[0], rectifyed_img_r.shape[0]) width = rectifyed_img_l.shape[1] + rectifyed_img_r.shape[1] output = np.zeros((height, width, 3), dtype=np.uint8) output[0:rectifyed_img_l.shape[0], 0:rectifyed_img_l.shape[1]] = rectifyed_img_l output[0:rectifyed_img_r.shape[0], rectifyed_img_l.shape[1]:] = rectifyed_img_r # 绘制等间距平行线 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 # 不想画可以不画 # line = draw_line(rectifyed_img1, rectifyed_img2) # cv2.imshow("rectify", line) # # 创建SGBM对象和准备参数 window_size = 3 # 正常来说，min_disp应该是0 min_disp = 0 num_disp = 128 - min_disp # 用SGBM算法获取视差图，即景深图 # StereoSGBM的速度比StereoBM慢，但是精度更高，准确性更好 # 下面的这些参数都是可以调节的，都是超参数，要做实验，以便确定最佳参数，根据具体的摄像机来确定 # numDisparities必须要能被16整除 # blockSize是matched block size，它应该为一个奇数，大部分情况下，它在3到11之间 # P1和P2控制disparity smoothness # speckleRange一般来说，1或者2就足够好了 stereoL = cv2.StereoSGBM_create(minDisparity=min_disp, numDisparities=num_disp, blockSize=window_size, uniquenessRatio=15, speckleWindowSize=100, speckleRange=1, disp12MaxDiff=1, P1=8 * 3 * window_size ** 2, P2=32 * 3 * window_size ** 2, mode=cv2.STEREO_SGBM_MODE_SGBM_3WAY) stereoR = cv2.ximgproc.createRightMatcher(stereoL) # Create another stereo for right this time wls_filter = cv2.ximgproc.createDisparityWLSFilter(matcher_left=stereoL) # WLS FILTER Parameters # 较大的lambda使得filter_img和原图的轮廓更为一致，通常值为8000 # 较小的sigma使得视差对图片的纹理、噪音更为敏感，通常在0.8到2.0之间 lmbda = 8000 sigma = 2.0 wls_filter.setLambda(lmbda) wls_filter.setSigmaColor(sigma) # 上面校正完之后，再计算视差 grayR = cv2.cvtColor(rectifyed_img_r, cv2.COLOR_BGR2GRAY) grayL = cv2.cvtColor(rectifyed_img_l, cv2.COLOR_BGR2GRAY) dispL = stereoL.compute(grayL, grayR) true_disp = dispL.astype(np.float32) / 16 dispL = np.int16(dispL) dispR = stereoR.compute(grayR, grayL) dispR = np.int16(dispR) filteredImg = wls_filter.filter(dispL, grayL, None, dispR) filteredImg = cv2.normalize(src=filteredImg, dst=filteredImg, beta=0, alpha=255, norm_type=cv2.NORM_MINMAX) filteredImg = np.uint8(filteredImg) # 视差图上色，仅用于展示 # filt_Color = cv2.applyColorMap(filteredImg, cv2.COLORMAP_OCEAN) # cv2.imshow('Filtered Color Depth', filt_Color) return filteredImg # 返回视差图 def getDepthMapWithQ(disparityMap : np.ndarray, Q : np.ndarray) -> np.ndarray: points_3d = cv2.reprojectImageTo3D(disparityMap, Q) depthMap = points_3d[:, :, 2] reset_index = np.where(np.logical_or(depthMap < 0.0, depthMap > 65535.0)) depthMap[reset_index] = 0 return depthMap.astype(np.float32) if __name__ == '__main__': img_l = cv2.imread("left.jpg", 1) # 左图 img_r = cv2.imread("right.jpg", 1) # 右图 if (img_l is None) or (img_r is None): print("Error: Images are empty, please check your image's path!") sys.exit(0) height, width = img_l.shape[0:2] # 读取相机内参和外参 # 使用之前先将标定得到的内外参数填写到stereoconfig.py中的StereoCamera类中 # config = doublecam_calibration.get_config() config = stereoconfig.stereoCamera() # 立体校正 maplx, maply, maprx, mapry, Q = getRectifyTransform(height, width, config) # 获取用于畸变校正和立体校正的映射矩阵以及用于计算像素空间坐标的重投影矩阵 # 计算视差 disp = compute_disp(img_l, img_r, maplx, maply, maprx, mapry) cv2.imshow('Disparity Map', disp) # 视差图 # 计算像素点的3D坐标（左相机坐标系下） points_3d = cv2.reprojectImageTo3D(disp, Q) # 参数中的Q就是由getRectifyTransform()函数得到的重投影矩阵 # print(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("distance", img_l) # 点击测距 cv2.setMouseCallback("disparity", onMouse, 0) # 计算深度图 depthMap = getDepthMapWithQ(disp, Q) # depthMap = getDepthMapWithConfig(disp, config) minDepth = np.min(depthMap) maxDepth = np.max(depthMap) # print(minDepth, maxDepth) depthMapVis = (255.0 *(depthMap - minDepth)) / (maxDepth - minDepth) depthMapVis = depthMapVis.astype(np.uint8) cv2.imshow("DepthMap", depthMapVis) # 深度图 # 使用open3d库绘制点云 colorImage = o3d.geometry.Image(img_l) depthImage = o3d.geometry.Image(depthMap) rgbdImage = o3d.geometry.RGBDImage().create_from_color_and_depth(colorImage, depthImage, depth_scale=1000.0, depth_trunc=np.inf) int