Карта глубины из 2 снимков с использованием openCV для airSim

Question

Я хочу получить карту глубины из 2 фотографий для моего беспилотника airSim. Я сделал 2 снимка один за другим и попытался сделать из них карту глубины, используя openCV. Сначала я сделал карту несоответствия, а затем я сделал карту глубины. Я думаю, что проблема с картой несоответствия.

Я попробовал этот код:

   import numpy as np
   from sklearn.preprocessing import normalize
   import cv2
   import time
   from PIL import Image

   print('loading images...')
   imgL = cv2.imread('10.png')  # downscale images for faster processing
   imgR = cv2.imread('12.png')

   # SGBM Parameters -----------------
   window_size = 5                     # wsize default 3; 5; 7 for SGBM reduced size image; 15 for SGBM full size image (1300px and above); 5 Works nicely

   left_matcher = cv2.StereoSGBM_create(
       minDisparity=0,
       numDisparities=16,             # max_disp has to be dividable by 16 f. E. HH 192, 256
       blockSize=5,
       P1=8 * 3 * window_size ** 2,    # wsize default 3; 5; 7 for SGBM reduced size image; 15 for SGBM full size image (1300px and above); 5 Works nicely
       P2=32 * 3 * window_size ** 2,
       disp12MaxDiff=1,
       uniquenessRatio=1,
       speckleWindowSize=0,
       speckleRange=5000,
       preFilterCap=63,
       mode=cv2.STEREO_SGBM_MODE_SGBM_3WAY
   )

   right_matcher = cv2.ximgproc.createRightMatcher(left_matcher)

   # FILTER Parameters
   lmbda = 1000
   sigma = 1.2
   visual_multiplier = 2.0

   wls_filter = cv2.ximgproc.createDisparityWLSFilter(matcher_left=left_matcher)
   wls_filter.setLambda(lmbda)
   wls_filter.setSigmaColor(sigma)

   print('computing disparity...')
   displ = left_matcher.compute(imgL, imgR)  # .astype(np.float32)/16
   dispr = right_matcher.compute(imgR, imgL)  # .astype(np.float32)/16
   displ = np.int16(displ)
   dispr = np.int16(dispr)
   filteredImg = wls_filter.filter(displ, imgL, None, dispr)  # important to put "imgL" here!!!

   filteredImg = cv2.normalize(src=filteredImg, dst=filteredImg, beta=0, alpha=255, norm_type=cv2.NORM_MINMAX);
   filteredImg = np.uint8(filteredImg)
   cv2.imshow('Disparity Map', filteredImg)

   q = np.zeros((4, 4, 1), dtype = "uint8")
   q[0][0] = 1
   q[0][1] = 0
   q[0][2] = 0
   q[0][3] = -10
   q[1][0] = 0
   q[1][1] = 1
   q[1][2] = 0
   q[1][3] = -10
   q[2][0] = 0
   q[2][1] = 0
   q[2][2] = 0
   q[2][3] = 0
   q[3][0] = 0
   q[3][1] = 0
   q[3][2] = 4
   q[3][3] = 0

   threeDImage = cv2.reprojectImageTo3D(filteredImg, q)
   cv2.imshow('depth', threeDImage)
   cv2.waitKey(1)
   time.sleep(1)
   #print(threeDImage[0][16])
   cv2.destroyAllWindows()

   cv2.imwrite("3dimage.png", np.float32(threeDImage)*256)

   ```
and it didn't work.
I also tried this: 

   ```



   ```#!/usr/bin/env python

   '''
   Simple example of stereo image matching and point cloud generation.

   Resulting .ply file cam be easily viewed using MeshLab ( http://meshlab.sourceforge.net/ )
   '''

   # Python 2/3 compatibility
   from __future__ import print_function

   import numpy as np
   import cv2 as cv

   ply_header = '''ply
   format ascii 1.0
   element vertex %(vert_num)d
   property float x
   property float y
   property float z
   property uchar red
   property uchar green
   property uchar blue
   end_header
   '''

   def write_ply(fn, verts, colors):
       verts = verts.reshape(-1, 3)
       colors = colors.reshape(-1, 3)
       verts = np.hstack([verts, colors])
       with open(fn, 'wb') as f:
           f.write((ply_header % dict(vert_num=len(verts))).encode('utf-8'))
           np.savetxt(f, verts, fmt='%f %f %f %d %d %d ')


   def main():
       print('loading images...')
       imgL = cv.pyrDown(cv.imread('10.png'))  # downscale images for faster processing
       imgR = cv.pyrDown(cv.imread('12.png'))

       # disparity range is tuned for 'aloe' image pair
       window_size = 5
       min_disp = 0
       num_disp = 32
       stereo = cv.StereoSGBM_create(minDisparity = min_disp,
           numDisparities = num_disp,
           blockSize = 16,
           P1 = 8*3*window_size**2,
           P2 = 32*3*window_size**2,
           disp12MaxDiff = 1,
           uniquenessRatio = 10,
           speckleWindowSize = 100,
           speckleRange = 32
       )

       print('computing disparity...')
       disp = stereo.compute(imgL, imgR).astype(np.float32) / 16.0

       print('generating 3d point cloud...',)
       h, w = imgL.shape[:2]
       f = 0.8*w                          # guess for focal length
       Q = np.float32([[1, 0, 0, -0.5*w],
                       [0,-1, 0,  0.5*h], # turn points 180 deg around x-axis,
                       [0, 0, 0,     -f], # so that y-axis looks up
                       [0, 0, 1,      0]])
       points = cv.reprojectImageTo3D(disp, Q)
       colors = cv.cvtColor(imgL, cv.COLOR_BGR2RGB)
       mask = disp > disp.min()
       out_points = points[mask]
       out_colors = colors[mask]
       out_fn = 'out.ply'
       write_ply(out_fn, out_points, out_colors)
       print('%s saved' % out_fn)

       cv.imshow('left', imgL)
       cv.imshow('disparity', (disp-min_disp)/num_disp)
       cv.waitKey()

       print('Done')


   if __name__ == '__main__':
       print(__doc__)
       main()
       cv.destroyAllWindows()

Кто-нибудь знает, как решить эту проблему?

Спасибо