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)
