def __init__(self, shape, n=1, fovea_shape=None, fill_method='smudge'):
"""
Arguments
---------
shape - Expected shape of disparity images
n - Number of past frames to retain
fovea_shape - Shape of foveal region to remember (None=whole image)
fill_method - default 'smudge' works fine and is fast, other options
are 'none' or 'interp' (which is way too slow)
"""
self.shape = shape
self.fovea_shape = fovea_shape
self.n = n
calib = Calib()
self.disp2imu = calib.get_disp2imu()
self.imu2disp = calib.get_imu2disp()
self.past_position = []
self.past_disparity = []
self.transforms = []
assert fill_method in ('smudge', 'interp', 'none')
self.fill_method = fill_method
def __init__(self, shape, n=1, fovea_shape=None, fill_method='smudge'):
"""
Arguments
---------
shape - Expected shape of disparity images
n - Number of past frames to retain
fovea_shape - Shape of foveal region to remember (None=whole image)
fill_method - default 'smudge' works fine and is fast, other options
are 'none' or 'interp' (which is way too slow)
"""
self.shape = shape
self.fovea_shape = fovea_shape
self.n = n
calib = Calib()
self.disp2imu = calib.get_disp2imu()
self.imu2disp = calib.get_imu2disp()
self.past_position = []
self.past_disparity = []
self.transforms = []
assert fill_method in ('smudge', 'interp', 'none')
self.fill_method = fill_method
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()
print(down_factors)
print(iters)
print(errors)
idx = errors.index(min(errors))
return down_factors[idx], iters[idx], fovea_shapes[idx]
def get_coarse_subwindow():
pass
def get_seed():
pass
if __name__ == '__main__':
plt.ion()
calib = Calib()
disp2imu = calib.get_disp2imu()
imu2disp = calib.get_imu2disp()
drive = 51
video = load_stereo_video(drive)
positions = load_video_odometry(drive)
initial = video[0]
fig = plt.figure(1)
fig.clf()
ax_disp = plt.gca()
plot_disp = ax_disp.imshow(mean_disparity(drive, n_disp), vmin=0, vmax=n_disp)
fovea_ij = 100, 600
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()
