def test_disparity_with_bm(drive=11, frame=0): import cv2 xyd = load_disparity_points(drive, frame, color=True) disp = np.zeros(image_shape, dtype=np.uint8) for x, y, d in np.round(xyd): disp[y, x] = d # compare with block matching left, right = load_stereo_frame(drive, frame, color=False) ndisp = 128 bm = cv2.createStereoBM(numDisparities=ndisp, blockSize=9) bm.setPreFilterSize(41) bm.setPreFilterCap(31) bm.setTextureThreshold(20) bm.setUniquenessRatio(10) bm.setSpeckleWindowSize(100) bm.setSpeckleRange(32) bm_disp = bm.compute(left, right) / 16 # compute difference diff = np.zeros(image_shape) for x, y, d in np.round(xyd): if bm_disp[y, x] >= 0: diff[y, x] = bm_disp[y, x] - d # downsample for visualization purposes diff2 = np.zeros((image_shape[0] / 2, image_shape[1] / 2)) for i in range(diff2.shape[0]): for j in range(diff2.shape[1]): r = diff[2 * i:2 * i + 2, 2 * j:2 * j + 2].flatten() ind = np.argmax(np.abs(r)) diff2[i, j] = r[ind] # custom colormap from matplotlib.colors import LinearSegmentedColormap g = 1.0 cdict1 = { 'red': ((0.0, 0.0, 0.0), (0.5, 0.0, 0.0), (1.0, 1.0, 1.0)), 'green': ((0.0, g, g), (0.5, 0.0, 0.0), (1.0, g, g)), 'blue': ((0.0, 1.0, 1.0), (0.5, 0.0, 0.0), (1.0, 0.0, 0.0)), } cmap = LinearSegmentedColormap('BlueRed', cdict1) plt.figure(1) plt.clf() plt.subplot(311) plt.imshow(disp) plt.colorbar() plt.subplot(312) plt.imshow(bm_disp) plt.colorbar() plt.subplot(313) plt.imshow(diff2, cmap=cmap, interpolation='nearest') plt.colorbar() plt.show()
def test_clr(drive=11, frame=0): xyd = load_disparity_points(drive, frame, color=False) disp = np.zeros(image_shape) for j, i, d in np.round(xyd): disp[i, j] = d pair = load_stereo_frame(drive, frame, color=False) bp_disp2 = bp_stereo_interp(pair[0], pair[1], xyd) cpair = load_stereo_frame(drive, frame, color=True) plt.figure() plt.clf() plt.subplot(411) plt.imshow(cpair[0]) plt.subplot(412) plt.imshow(cpair[1]) plt.subplot(413) plt.imshow(disp) plt.subplot(414) plt.imshow(bp_disp2) plt.show(block=False)
def test_load_disparity_points(drive=11, frame=0): xyd = load_disparity_points(drive, frame, color=True) disp = np.zeros(image_shape, dtype=np.uint8) for x, y, d in np.round(xyd): disp[y, x] = d stf = load_stereo_frame(drive, 0) plt.figure(1) plt.clf() plt.subplot(2,1,1) plt.imshow(disp) plt.subplot(2,1,2) plt.imshow(stf[0],cmap=plt.cm.gray) plt.show()
def test_bp_interp(drive=11, frame=0): xyd = load_disparity_points(drive, frame, color=False) disp = np.zeros(image_shape) for j, i, d in np.round(xyd): disp[i, j] = d bp_disp = bp_interp(image_shape, xyd) pair = load_stereo_frame(drive, frame) bp_disp2 = bp_stereo_interp(pair[0], pair[1], xyd) plt.figure() plt.clf() plt.subplot(411) plt.imshow(pair[0], cmap='gray') plt.subplot(412) plt.imshow(disp) plt.subplot(413) plt.imshow(bp_disp) plt.subplot(414) plt.imshow(bp_disp2) plt.show()
import cv2 import numpy as np import matplotlib.pyplot as plt from kitti.raw import load_stereo_frame from kitti.velodyne import load_disparity_points from bp_wrapper import downsample, coarse_bp, foveal_bp, error_on_points, plot_fovea from hunse_tools.timing import tic, toc idrive = 51 iframe = 50 frame = load_stereo_frame(idrive, iframe) points = load_disparity_points(idrive, iframe) full_values = 128 frame_down_factor = 1 values = full_values / 2**frame_down_factor iters = 3 params = { 'data_weight': 0.16145115747533928, 'disc_max': 294.1504935618425, 'data_max': 32.024780646200725, 'ksize': 3} frame = (downsample(frame[0], frame_down_factor), downsample(frame[1], frame_down_factor)) tic() coarse_disp = coarse_bp(frame, values=values, down_factor=1, iters=iters, **params) coarse_disp *= 2**frame_down_factor
def test_disparity_with_bm(drive=11, frame=0): # cv2.createStereoBM seems to be absent from current version of opencv import cv2 xyd = load_disparity_points(drive, frame, color=True) disp = np.zeros(image_shape, dtype=np.uint8) for x, y, d in np.round(xyd): disp[y, x] = d # compare with block matching left, right = load_stereo_frame(drive, frame, color=False) ndisp = 128 bm = cv2.createStereoBM(numDisparities=ndisp, blockSize=9) bm.setPreFilterSize(41) bm.setPreFilterCap(31) bm.setTextureThreshold(20) bm.setUniquenessRatio(10) bm.setSpeckleWindowSize(100) bm.setSpeckleRange(32) bm_disp = bm.compute(left, right) / 16 # compute difference diff = np.zeros(image_shape) for x, y, d in np.round(xyd): if bm_disp[y, x] >= 0: diff[y, x] = bm_disp[y, x] - d # downsample for visualization purposes diff2 = np.zeros((image_shape[0] / 2, image_shape[1] / 2)) for i in range(diff2.shape[0]): for j in range(diff2.shape[1]): r = diff[2*i:2*i+2, 2*j:2*j+2].flatten() ind = np.argmax(np.abs(r)) diff2[i, j] = r[ind] # custom colormap from matplotlib.colors import LinearSegmentedColormap g = 1.0 cdict1 = { 'red': ((0.0, 0.0, 0.0), (0.5, 0.0, 0.0), (1.0, 1.0, 1.0)), 'green': ((0.0, g, g), (0.5, 0.0, 0.0), (1.0, g, g)), 'blue': ((0.0, 1.0, 1.0), (0.5, 0.0, 0.0), (1.0, 0.0, 0.0)), } cmap = LinearSegmentedColormap('BlueRed', cdict1) plt.figure(1) plt.clf() plt.subplot(311) plt.imshow(disp) plt.colorbar() plt.subplot(312) plt.imshow(bm_disp) plt.colorbar() plt.subplot(313) plt.imshow(diff2, cmap=cmap, interpolation='nearest') plt.colorbar() plt.show()
import cv2 import numpy as np import matplotlib.pyplot as plt from kitti.raw import load_stereo_frame from kitti.velodyne import load_disparity_points from bp_wrapper import downsample, coarse_bp, foveal_bp, error_on_points, plot_fovea from hunse_tools.timing import tic, toc idrive = 51 iframe = 50 frame = load_stereo_frame(idrive, iframe) points = load_disparity_points(idrive, iframe) full_values = 128 frame_down_factor = 1 values = full_values / 2**frame_down_factor iters = 3 params = { 'data_weight': 0.16145115747533928, 'disc_max': 294.1504935618425, 'data_max': 32.024780646200725, 'ksize': 3 } frame = (downsample(frame[0], frame_down_factor), downsample(frame[1], frame_down_factor))
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()