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

测距公式： 

这篇博文也比较详细：

使用OpenCV/python进行双目测距 - 走看看

通过matlab标定得到相机参数放到stereoconfig.py

  

   

    

     

    
    

     

      import numpy as np
     
    
   

    

     

    
    

     

      import cv2
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     

      #双目相机参数
     
    
   

    

     

    
    

     

      class stereoCameral(object):
     
    
   

    

     

    
    

     
    def __init__(self):
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     
        #左相机内参数
     
    
   

    

     

    
    

     

              self.cam_matrix_left = np.array([[249.82379, 0., 156.38459], [0., 249.07678, 122.46872], [0., 0., 1.]])
     
    
   

    

     

    
    

     
        #右相机内参数
     
    
   

    

     

    
    

     

              self.cam_matrix_right = np.array([[242.77875, 0., 153.22330], [0., 242.27426, 117.63536], [0., 0., 1.]])
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     
        #左右相机畸变系数:[k1, k2, p1, p2, k3]
     
    
   

    

     

    
    

     

              self.distortion_l = np.array([[-0.02712, -0.03795, -0.00409, 0.00526, 0.00000]])
     
    
   

    

     

    
    

     

              self.distortion_r = np.array([[-0.03348, 0.08901, -0.00327, 0.00330, 0.00000]])
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     
        #旋转矩阵
     
    
   

    

     

    
    

     

              om = np.array([-0.00320, -0.00163, -0.00069])
     
    
   

    

     

    
    

     

              self.R = cv2.Rodrigues(om)[0]  # 使用Rodrigues变换将om变换为R
     
    
   

    

     

    
    

     
        #平移矩阵
     
    
   

    

     

    
    

     

              self.T = np.array([-90.24602, 3.17981, -19.44558])
     
    
  
 

视差图及三维坐标

  

   

    

     

    
    

     

      import cv2
     
    
   

    

     

    
    

     

      import numpy as np
     
    
   

    

     

    
    

     

      import stereoconfig
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     

      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
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     
    #计算校正变换
     
    
   

    

     

    
    

     
    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)
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     
    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 sgbm(imgL, imgR):
     
    
   

    

     

    
    

     
    #SGBM参数设置
     
    
   

    

     

    
    

     

          blockSize = 8
     
    
   

    

     

    
    

     

          img_channels = 3
     
    
   

    

     

    
    

     

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

    

     

    
    

     
    return disp
     
    
   

    

     

    
    

     

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

    

     

    
    

     

      def threeD(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)
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     
    #计算目标点（这里我选择的是目标区域的中位数，可根据实际情况选取）
     
    
   

    

     

    
    

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

    

     

    
    

     
 
     
    
   

    

     

    
    

     
    return targetPoint
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     

      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)
     
    
  
 

结果：

文章来源: blog.csdn.net，作者：AI视觉网奇，版权归原作者所有，如需转载，请联系作者。

原文链接：blog.csdn.net/jacke121/article/details/124928816