for p in points: q = project(camera, canvas_v, p[0], flag) c, r = topixel(camera, img_sz, q, scale, flag) picture[r, c, :] = p[1] picture = cv2.cvtColor(picture, cv2.COLOR_RGB2BGR) picture = drawAxis(picture, camera, canvas_v, scale, img_sz, 50.0, flag) cv2.imshow('3DPicture', picture) cv2.waitKey(0) cv2.destroyAllWindows() if __name__ == '__main__': #display('3DPoints.ply', (640, 480), 0) #display('_3DPoints.ply', (640, 480), 0) '''imgL = cv2.imread('L2.jpg') imgR = cv2.imread('R2.jpg') Q = np.float32([[1, 0, 0, -0.5*imgL.shape[1]], [0, 1, 0, -0.5*imgL.shape[0]], [0, 0, 0, imgL.shape[1] * 0.8], [0, 0, 1, 0]]) for i in [3, 4, 5, 6]: print i stereosgbm_match(imgL, imgR, '3DPoints.ply', Q, i) display('3DPoints.ply', (640, 480), 0)''' params = (0.04, 0.176, 0, 0, 0) #pitch, yaw, height, posx, posy = params imgL, imgR, Q = stereo_rectify('L4.jpg', 'R4.jpg', 'rectmap.npy', 'Q.npy') for h in [-24, -20, -18, -16]: print h, "-------------------------------------------" stereosgbm_match(imgL, imgR, '_3DPoints.ply', Q, 64, params, h) display('_3DPoints.ply', (640, 480), 0)
print 'Q\n', Q Q = np.float32([[1, 0, 0, -0.5*imgSize[0]], [0, 1, 0, -0.5*imgSize[1]], [0, 0, 0, cameraMatrix[0][0]], [0, 0, 0.07, 0]]) #Save the results np.save('rectmap.npy', urmaps) np.save('Q.npy', Q) #RECTIFY THE IMAGES AND FIND DISPARITY MAPS #Setup for finding correspondences img1r = cv2.remap(image1, urmaps[0], urmaps[1], cv2.INTER_LINEAR) img2r = cv2.remap(image2, urmaps[2], urmaps[3], cv2.INTER_LINEAR) cv2.imshow('Image 1', img1r); cv2.imshow('Image 2', img2r) cv2.waitKey(0) '''disp, points = stereoBMMatch(img1r, img2r, Q) colors = cv2.cvtColor(image1, cv2.COLOR_BGR2RGB) mask = disp > disp.min() points = points[mask]; colors = colors[mask] write_ply('_3DPoints.ply', points, colors)''' #stereosgbm_match(img1r, img2r, '_3DPoints.ply', Q) cv2.waitKey(0) cv2.destroyAllWindows() if __name__ == '__main__': #stereoCalibrate() imgL, imgR, Q = stereo_rectify('L4.jpg', 'R4.jpg', 'rectmap.npy', 'Q.npy') stereosgbm_match(imgL, imgR, '_3DPoints.ply', Q, -50)
def detect_obstacle(img1, img2, pos, angle, eps): params = (angle[0], angle[1], pos[2], pos[0], pos[1]) imgL, imgR, Q = stereo_rectify(img1, img2, REMAP_FILENAME, Q_MATRIX_FILENAME) Q[3][2] = 0.07 return stereosgbm_match(imgL, imgR, None, Q, 48, params, eps)