def load_disparity_points(drive, frame, color=False, **kwargs): calib = Calib(color=color, **kwargs) # read velodyne points points = load_velodyne_points(drive, frame, **kwargs) # remove all points behind image plane (approximation) points = points[points[:, 0] >= 1, :] # convert points to each camera xyd = calib.velo2disp(points) # take only points that fall in the first image xyd = calib.filter_disps(xyd) return xyd
def test_disp2rect(): drive = 11 frame = 0 color = True img0, img1 = load_stereo_frame(drive, frame, color=color) calib = Calib(color=color) # get calibration # get points vpts = load_velodyne_points(drive, frame) # remove invalid points # m = (vpts[:, 0] >= 5) # m = (vpts[:, 0] >= 5) & (np.abs(vpts[:, 1]) < 5) m = (vpts[:, 0] >= 5) & (vpts[:, 2] >= -3) vpts = vpts[m, :] rpts = calib.velo2rect(vpts) # get disparities xyd = calib.rect2disp(rpts) xyd, valid_rpts = calib.filter_disps(xyd, return_mask=True) if 1: # plot disparities disp = np.zeros(image_shape, dtype=np.uint8) for x, y, d in np.round(xyd): disp[y, x] = d plt.figure(101) plt.clf() plt.subplot(211) plt.imshow(img0, cmap='gray') plt.subplot(212) plt.imshow(disp) plt.show() # assert False # convert back to rect rpts2 = calib.disp2rect(xyd) assert np.allclose(rpts[valid_rpts], rpts2) # plotting if 0: plt.figure(101) plt.clf() img0, img1 = load_stereo_frame(drive, frame, color=color) plt.imshow(img0, cmap='gray') from mpl_toolkits.mplot3d import Axes3D fig = plt.figure(1) fig.clf() ax = fig.add_subplot(111, projection='3d') ax.plot3D(rpts[:, 0], rpts[:, 1], rpts[:, 2], '.') ax.set_xlabel('x') ax.set_ylabel('y') ax.set_zlabel('z') plt.show()
def test_disp2rect(): drive = 11 frame = 0 color = True img0, img1 = load_stereo_frame(drive, frame, color=color) calib = Calib(color=color) # get calibration # get points vpts = load_velodyne_points(drive, frame) # remove invalid points # m = (vpts[:, 0] >= 5) # m = (vpts[:, 0] >= 5) & (np.abs(vpts[:, 1]) < 5) m = (vpts[:, 0] >= 5) & (vpts[:, 2] >= -3) vpts = vpts[m, :] rpts = calib.velo2rect(vpts) # get disparities xyd = calib.rect2disp(rpts) xyd, valid_rpts = calib.filter_disps(xyd, return_mask=True) if 1: # plot disparities disp = np.zeros(image_shape, dtype=np.uint8) for x, y, d in np.round(xyd): disp[y, x] = d plt.figure(101) plt.clf() plt.subplot(211) plt.imshow(img0, cmap='gray') plt.subplot(212) plt.imshow(disp) plt.show() # assert False # convert back to rect rpts2 = calib.disp2rect(xyd) assert np.allclose(rpts[valid_rpts], rpts2) # plotting if 0: plt.figure(101) plt.clf() img0, img1 = load_stereo_frame(drive, frame, color=color) plt.imshow(img0, cmap='gray') from mpl_toolkits.mplot3d import Axes3D fig = plt.figure(1) fig.clf() ax = fig.add_subplot(111, projection='3d') ax.plot3D(rpts[:, 0], rpts[:, 1], rpts[:, 2], '.') ax.set_xlabel('x') ax.set_ylabel('y') ax.set_zlabel('z') plt.show()