SHARE
TWEET

Untitled

a guest Feb 21st, 2020 81 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. left = cv.imread("/home/maria/PycharmProjects/reprojection/BFS1_test.pgm", cv.IMREAD_GRAYSCALE)
  2. right = cv.imread("/home/maria/PycharmProjects/reprojection/BFS2_test.pgm", cv.IMREAD_GRAYSCALE)
  3.  
  4. C = camera_calibrate.StereoCalibration("/home/maria/PycharmProjects/reprojection/calibrationData/")
  5.  
  6. rectData = cv.stereoRectify(cameraMatrix1=C.camera_model['M1'], distCoeffs1=C.camera_model['dist1'],
  7.                             cameraMatrix2=C.camera_model['M2'], distCoeffs2=C.camera_model['dist2'],
  8.                             imageSize=(1440, 1080), R=C.camera_model['R'], T=C.camera_model['T'])
  9.  
  10. uRectData = list(rectData)
  11. Q = uRectData[4]
  12. R1 = uRectData[0]
  13. R2 = uRectData[1]
  14. P1 = uRectData[2]
  15.  
  16. mapx1, mapy1 = cv.initUndistortRectifyMap(C.camera_model["M1"], C.camera_model["dist1"],R1,
  17.                                           C.camera_model["M1"],
  18.                                           ((1440, 1080)), cv.CV_32F)
  19.  
  20. mapx2, mapy2 = cv.initUndistortRectifyMap(C.camera_model["M2"], C.camera_model["dist2"], R2 ,
  21.                                           C.camera_model["M2"],
  22.                                           ((1440, 1080)), cv.CV_32F)
  23.  
  24. img_left_rect = cv.remap(left, mapx1, mapy1, cv.INTER_LINEAR)
  25. img_right_rect = cv.remap(right, mapx2, mapy2, cv.INTER_LINEAR)
  26.  
  27.  
  28.  
  29. stereoSGBMobj = cv.StereoSGBM_create(numDisparities=16 * 4)
  30. disparity = stereoSGBMobj.compute(img_left_rect, img_right_rect)
  31.  
  32. PtCloud = cv.reprojectImageTo3D(disparity, Q)
  33. PtCloud1 = np.reshape(PtCloud, (PtCloud.shape[0] * PtCloud.shape[1], 3))
  34. PtCloud1 = np.matmul(PtCloud1,np.linalg.inv(R1))
  35.  
  36. rvec,j = list(cv.Rodrigues(np.eye(3)))
  37.  
  38. Res = cv.projectPoints(objectPoints=PtCloud1,
  39.                        rvec=rvec, tvec=np.array([[0],[0],[0]],dtype=float),
  40.                        cameraMatrix=C.camera_model['M1'],
  41.                        distCoeffs=np.asarray(C.camera_model['dist1']))
  42.  
  43. Points = np.squeeze(np.array(np.array(Res[0])))
  44.  
  45. for i in range(len(Points)):
  46.     if PtCloud1[i][2]<0:
  47.         Points[i] = [np.nan,np.nan]
  48.         continue
  49.     if PtCloud1[i][2]>400:
  50.         Points[i] = [np.nan,np.nan]
  51.         continue
  52.  
  53.  
  54. plt.figure(1)
  55. plt.imshow(left, cmap='gray')
  56.  
  57. plt.scatter(Points[:, 0], Points[:, 1], s=0.1, alpha=0.1)
  58.  
  59. plt.show()
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Top