def __init__(self, drive, n_frames=None, n_disp=128):
self.drive = drive
self.n_disp = n_disp
self.video = load_stereo_video(drive, n_frames)
self.n_frames = len(self.video) # not None; may be less than requested
self.positions = load_video_odometry(drive)
self.positions = self.positions[:self.n_frames]
self.ground_truth = self.get_ground_truth()
def __init__(self, drive, n_frames=None, n_disp=128):
self.drive = drive
self.n_disp = n_disp
self.video = load_stereo_video(drive, n_frames)
self.n_frames = len(self.video) # not None; may be less than requested
self.positions = load_video_odometry(drive)
self.positions = self.positions[:self.n_frames]
self.ground_truth = self.get_ground_truth()
def test_load_video_odometry():
import numpy as np
import matplotlib.pyplot as plt
dt = 0.1035
odometry = load_video_odometry(11, raw=True)
t = odometry.T[-1]
dts = np.diff(t)
print (t[-1] - t[0]) / (len(t) - 1)
print dts.mean()
positions = odometry_to_positions(odometry)
x, y = positions.T[:2]
x = x - x[0]
y = y - y[0]
vn, ve = odometry.T[6:8] # velocity north and east
x2 = np.cumsum(ve) * dt
y2 = np.cumsum(vn) * dt
yaw, vf, vl = odometry.T[[5, 8, 9]]
# vn3 = np.zeros_like(vn)
# ve3 = np.zeros_like(ve)
trap = lambda x: 0.5 * (x[:-1] + x[1:])
yaw2, vf2, vl2 = trap(yaw), trap(vf), trap(vl)
nv = len(x) - 1
u = np.zeros(nv) # v east
v = np.zeros(nv) # v north
for i in range(nv):
yawi, vfi, vli = yaw2[i], vf2[i], vl2[i]
cy, sy = np.cos(yawi), np.sin(yawi)
# u[i] = cy * vfi
# v[i] = sy * vfi
u[i] = cy * vfi - sy * vli
v[i] = sy * vfi + cy * vli
x3 = np.cumsum(u) * dt
y3 = np.cumsum(v) * dt
ax, ay, az, af, al, au = odometry.T[11:17]
# vf4 = np.cumsum(af) * dt + vf[0]
vf4 = vf[0] * np.ones_like(vf)
vf4[1:] += np.cumsum(trap(af) * dts)
plt.figure()
plt.plot(x, y, 'k.-')
plt.plot(x2, y2, 'b.-')
plt.plot(x3, y3, 'r.-')
plt.figure()
plt.plot(vf)
plt.plot(vf4)
plt.show()
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
time_budget = .2
coarse_time_budget = .9 * time_budget
coarse_down_factor, coarse_iters = find_coarse_params(coarse_time_budget, video, n_disp, drive, **coarse_params)
# coarse_down_factor = 3
# coarse_iters = 2 #TODO