def __init__(self, sequences):
self.errors = []
self.gt_poses = {}
for seq in sequences:
gt_abs_poses = SequenceData(seq).get_poses()
self.gt_poses[seq] = gt_abs_poses
def __init__(self, sequences):
self.errors = []
self.gt_traj = {}
self.raw_timestamps = {}
for seq in sequences:
gt_traj = file_interface.read_euroc_csv_trajectory(
os.path.join(par.data_dir, seq, "groundtruth.csv"))
self.gt_traj[seq] = gt_traj
self.raw_timestamps[seq] = np.array(
SequenceData(seq).get_timestamps_raw()) / 10**9
def ekf_test_case(self,
seqs,
seqs_range,
init_covar,
imu_covar,
vis_meas_covar,
device,
req_grad,
accel_bias_inject=None,
gyro_bias_inject=None):
seqs_data = [SequenceData(seq) for seq in seqs]
data_frames = [d.df[seqs_range[0]:seqs_range[1]] for d in seqs_data]
data_frames_lengths = [len(d) for d in data_frames]
assert (all(data_frames_lengths[0] == l for l in data_frames_lengths))
timestamps = np.array(
[list(df.loc[:, "timestamp"].values) for df in data_frames])
gt_poses = np.array(
[list(df.loc[:, "T_i_vk"].values) for df in data_frames])
gt_vels = np.array(
[list(df.loc[:, "v_vk_i_vk"].values) for df in data_frames])
T_imu_cam = np.array([d.T_cam_imu for d in seqs_data])
if accel_bias_inject is None:
accel_bias_inject = np.zeros([len(seqs_data), 3])
if gyro_bias_inject is None:
gyro_bias_inject = np.zeros([len(seqs_data), 3])
imu_timestamps = np.array(
[df.loc[:, "imu_timestamps"].values for df in data_frames])
gyro_measurements = np.array(
[df.loc[:, "gyro_measurements"].values for df in data_frames])
accel_measurements = np.array(
[df.loc[:, "accel_measurements"].values for df in data_frames])
ekf = IMUKalmanFilter()
self.assertEqual(timestamps.shape[0:2], gt_poses.shape[0:2])
self.assertEqual(timestamps.shape[0:2], gt_vels.shape[0:2])
self.assertEqual(timestamps.shape[0], T_imu_cam.shape[0])
g = np.array([0, 0, 9.808679801065017])
init_state = []
for i in range(0, len(gt_poses)):
init_state.append(
IMUKalmanFilter.encode_state(
torch.tensor(gt_poses[i, 0, 0:3, 0:3].transpose().dot(g),
dtype=torch.float32), # g
torch.eye(3, 3), # C
torch.zeros(3), # r
torch.tensor(gt_vels[i, 0], dtype=torch.float32), # v
torch.zeros(3), # bw
torch.zeros(3)).to(device)) # ba
init_state = torch.stack(init_state)
init_pose = torch.tensor([np.linalg.inv(p[0]) for p in gt_poses],
dtype=torch.float32).to(device)
init_covar = torch.tensor(init_covar, dtype=torch.float32).to(device)
imu_covar = torch.tensor(imu_covar, dtype=torch.float32).to(device)
# collect the data
imu_data = []
imu_max_length = 12
for i in range(0, timestamps.shape[1]):
imu_data_time_k = self.concat_imu_data_at_time_k(
imu_max_length, imu_timestamps[:, i], gyro_measurements[:, i],
accel_measurements[:, i], gyro_bias_inject, accel_bias_inject)
imu_data.append(imu_data_time_k)
imu_data = torch.tensor(np.stack(imu_data, 1),
dtype=torch.float32).to(device)
vis_meas = []
for i in range(0, timestamps.shape[1] - 1):
T_rel = np.matmul(np.linalg.inv(gt_poses[:, i]), gt_poses[:,
i + 1])
T_rel_vis = np.matmul(np.matmul(np.linalg.inv(T_imu_cam), T_rel),
T_imu_cam)
vis_meas.append(
np.concatenate([
np.array([log_SO3(T[:3, :3])
for T in T_rel_vis]), T_rel_vis[:, 0:3, 3]
], -1))
vis_meas = np.expand_dims(np.stack(vis_meas, 1), -1)
vis_meas = torch.tensor(vis_meas, dtype=torch.float32).to(device)
vis_meas_covars = vis_meas_covar.repeat(vis_meas.shape[0],
vis_meas.shape[1], 1,
1).to(device)
vis_meas_covars.requires_grad = req_grad
imu_covar.requires_grad = req_grad
init_covar.requires_grad = req_grad
init_pose.requires_grad = req_grad
init_state.requires_grad = req_grad
init_pose.requires_grad = req_grad
start_time = time.time()
poses, states, covars = ekf.forward(
imu_data, imu_covar, init_pose, init_state, init_covar, vis_meas,
vis_meas_covars,
torch.tensor(T_imu_cam, dtype=torch.float32).to(device))
print("ekf.forward elapsed %.5f" % (time.time() - start_time))
return timestamps, gt_poses, gt_vels, poses, states, covars
def predict_test_case(self, seqs, seqs_range, device, req_grad):
seqs_data = [SequenceData(seq) for seq in seqs]
if seqs_range is None:
data_frames = [d.df for d in seqs_data]
else:
data_frames = [
d.df[seqs_range[0]:seqs_range[1]] for d in seqs_data
]
data_frames_lengths = [len(d) for d in data_frames]
assert (all(data_frames_lengths[0] == l for l in data_frames_lengths))
timestamps = np.array(
[list(df.loc[:, "timestamp"].values) for df in data_frames])
gt_poses = np.array(
[list(df.loc[:, "T_i_vk"].values) for df in data_frames])
gt_vels = np.array(
[list(df.loc[:, "v_vk_i_vk"].values) for df in data_frames])
imu_timestamps = np.array(
[df.loc[:, "imu_timestamps"].values for df in data_frames])
gyro_measurements = np.array(
[df.loc[:, "gyro_measurements"].values for df in data_frames])
accel_measurements = np.array(
[df.loc[:, "accel_measurements"].values for df in data_frames])
ekf = IMUKalmanFilter()
self.assertEqual(timestamps.shape[0:2], gt_poses.shape[0:2])
self.assertEqual(timestamps.shape[0:2], gt_vels.shape[0:2])
g = np.array([0, 0, 9.808679801065017])
init_state = []
for i in range(0, len(gt_poses)):
init_state.append(
IMUKalmanFilter.encode_state(
torch.tensor(gt_poses[i, 0, 0:3, 0:3].transpose().dot(g),
dtype=torch.float32), # g
torch.eye(3, 3), # C
torch.zeros(3), # r
torch.tensor(gt_vels[i, 0], dtype=torch.float32), # v
torch.zeros(3), # bw
torch.zeros(3)).to(device)) # ba
states = [torch.stack(init_state)]
poses = [
torch.tensor([np.linalg.inv(p[0]) for p in gt_poses],
dtype=torch.float32).to(device)
]
covars = [torch.zeros(timestamps.shape[0], 18, 18).to(device)]
imu_noise = torch.eye(12, 12).to(device)
precomp_covars = [torch.zeros(timestamps.shape[0], 18, 18).to(device)]
states[0].requires_grad = req_grad
covars[0].requires_grad = req_grad
poses[0].requires_grad = req_grad
imu_max_length = 12
for i in range(0, timestamps.shape[1] - 1):
imu_data = self.concat_imu_data_at_time_k(imu_max_length,
imu_timestamps[:, i],
gyro_measurements[:, i],
accel_measurements[:, i])
imu_data = torch.tensor(imu_data, dtype=torch.float32).to(device)
pred_states, pred_covars = ekf.predict(imu_data, imu_noise,
states[-1], covars[-1])
precomp_covars.append(pred_covars[-1])
pose, state, covar = ekf.composition(poses[-1], pred_states[-1],
pred_covars[-1])
states.append(state)
covars.append(covar)
poses.append(pose)
states = torch.stack(states, 1)
covars = torch.stack(covars, 1)
poses = torch.stack(poses, 1)
precomp_covars = torch.stack(precomp_covars)
return timestamps, gt_poses, gt_vels, poses, states, covars, precomp_covars
def preprocess_kitti_raw(raw_seq_dir, output_dir, cam_subset_range, plot_figures=True):
logger.initialize(working_dir=output_dir, use_tensorboard=False)
logger.print("================ PREPROCESS KITTI RAW ================")
logger.print("Preprocessing %s" % raw_seq_dir)
logger.print("Output to: %s" % output_dir)
logger.print("Camera images: %d => %d" % (cam_subset_range[0], cam_subset_range[1]))
oxts_dir = os.path.join(raw_seq_dir, "oxts")
image_dir = os.path.join(raw_seq_dir, "image_02")
gps_poses = np.loadtxt(os.path.join(oxts_dir, "poses.txt"))
gps_poses = np.array([np.vstack([np.reshape(p, [3, 4]), [0, 0, 0, 1]]) for p in gps_poses])
T_velo_imu = np.loadtxt(os.path.join(raw_seq_dir, "../T_velo_imu.txt"))
T_cam_velo = np.loadtxt(os.path.join(raw_seq_dir, '../T_cam_velo.txt'))
T_cam_imu = T_cam_velo.dot(T_velo_imu)
# imu timestamps
imu_timestamps = read_timestamps(os.path.join(oxts_dir, "timestamps.txt"))
assert (len(imu_timestamps) == len(gps_poses))
# load image data
cam_timestamps = read_timestamps(os.path.join(image_dir, "timestamps.txt"))
image_paths = [os.path.join(image_dir, "data", p) for p in sorted(os.listdir(os.path.join(image_dir, "data")))]
assert (len(cam_timestamps) == len(image_paths))
assert (cam_subset_range[0] >= 0 and cam_subset_range[1] < len(image_paths))
# the first camera timestamps must be between IMU timestamps
assert (cam_timestamps[cam_subset_range[0]] >= imu_timestamps[0])
assert (cam_timestamps[cam_subset_range[1]] <= imu_timestamps[-1])
# take subset of the camera images int the range of images we are interested in
image_paths = image_paths[cam_subset_range[0]: cam_subset_range[1] + 1]
cam_timestamps = cam_timestamps[cam_subset_range[0]: cam_subset_range[1] + 1]
# convert to local time reference in seconds
cam_timestamps = (cam_timestamps - imu_timestamps[0]) / np.timedelta64(1, 's')
imu_timestamps = (imu_timestamps - imu_timestamps[0]) / np.timedelta64(1, 's')
# take a subset of imu data corresponds to camera images
idx_imu_data_start = find_timestamps_in_between(cam_timestamps[0], imu_timestamps)[0]
idx_imu_data_end = find_timestamps_in_between(cam_timestamps[-1], imu_timestamps)[1]
imu_timestamps = imu_timestamps[idx_imu_data_start:idx_imu_data_end + 1]
gps_poses = gps_poses[idx_imu_data_start:idx_imu_data_end + 1]
# load IMU data from list of text files
imu_data = []
imu_data_files = sorted(os.listdir(os.path.join(oxts_dir, "data")))
start_time = time.time()
for i in range(idx_imu_data_start, idx_imu_data_end + 1):
print("Loading IMU data files %d/%d (%.2f%%)"
% (i + 1 - idx_imu_data_start, len(imu_timestamps),
100 * (i + 1 - idx_imu_data_start) / len(imu_timestamps)), end='\r')
imu_data.append(np.loadtxt(os.path.join(oxts_dir, "data", imu_data_files[i])))
imu_data = np.array(imu_data)
logger.print("\nLoading IMU data took %.2fs" % (time.time() - start_time))
assert (len(imu_data) == len(gps_poses))
# imu_data = imu_data[idx_imu_data_start:idx_imu_data_end + 1]
imu_timestamps, imu_data, gps_poses = remove_negative_timesteps(imu_timestamps, imu_data, gps_poses)
data_frames = []
start_time = time.time()
idx_imu_slice_start = 0
idx_imu_slice_end = 0
for k in range(0, len(cam_timestamps) - 1):
print("Processing IMU data files %d/%d (%.2f%%)" % (
k + 1, len(cam_timestamps), 100 * (k + 1) / len(cam_timestamps)), end='\r')
t_k = cam_timestamps[k]
t_kp1 = cam_timestamps[k + 1]
# the start value does not need to be recomputed, since you can get that from the previous time step, but
# i am a lazy person, this will work
while imu_timestamps[idx_imu_slice_start] < t_k:
idx_imu_slice_start += 1
assert (imu_timestamps[idx_imu_slice_start - 1] <= t_k <= imu_timestamps[idx_imu_slice_start])
# interpolate
tk_i = imu_timestamps[idx_imu_slice_start - 1]
tk_j = imu_timestamps[idx_imu_slice_start]
alpha_k = (t_k - tk_i) / (tk_j - tk_i)
T_i_vk, v_vk, w_vk, a_vk = \
interpolate(imu_data[idx_imu_slice_start - 1], imu_data[idx_imu_slice_start],
gps_poses[idx_imu_slice_start - 1], gps_poses[idx_imu_slice_start], alpha_k)
while imu_timestamps[idx_imu_slice_end] < t_kp1:
idx_imu_slice_end += 1
assert (imu_timestamps[idx_imu_slice_end - 1] <= t_kp1 <= imu_timestamps[idx_imu_slice_end])
# interpolate
tkp1_i = imu_timestamps[idx_imu_slice_end - 1]
tkp1_j = imu_timestamps[idx_imu_slice_end]
alpha_kp1 = (t_kp1 - tkp1_i) / (tkp1_j - tkp1_i)
T_i_vkp1, v_vkp1, w_vkp1, a_vkp1 = \
interpolate(imu_data[idx_imu_slice_end - 1], imu_data[idx_imu_slice_end],
gps_poses[idx_imu_slice_end - 1], gps_poses[idx_imu_slice_end], alpha_kp1)
imu_timestamps_k_kp1 = np.concatenate(
[[t_k], imu_timestamps[idx_imu_slice_start:idx_imu_slice_end - 1], [t_kp1]])
imu_poses = np.concatenate([[T_i_vk], gps_poses[idx_imu_slice_start:idx_imu_slice_end - 1], [T_i_vkp1]])
accel_measurements_k_kp1 = np.concatenate([[a_vk],
imu_data[idx_imu_slice_start: idx_imu_slice_end - 1, ax:az + 1],
[a_vkp1]])
gyro_measurements_k_kp1 = np.concatenate([[w_vk],
imu_data[idx_imu_slice_start: idx_imu_slice_end - 1, wx:wz + 1],
[w_vkp1]])
frame_k = SequenceData.Frame(image_paths[k], t_k, T_i_vk, v_vk,
imu_poses, imu_timestamps_k_kp1, accel_measurements_k_kp1, gyro_measurements_k_kp1)
data_frames.append(frame_k)
# assertions for sanity check
assert (np.allclose(data_frames[-1].timestamp, data_frames[-1].imu_timestamps[0], atol=1e-13))
assert (np.allclose(data_frames[-1].T_i_vk, data_frames[-1].imu_poses[0], atol=1e-13))
if len(data_frames) > 1:
assert (np.allclose(data_frames[-1].timestamp, data_frames[-2].imu_timestamps[-1], atol=1e-13))
assert (np.allclose(data_frames[-1].T_i_vk, data_frames[-2].imu_poses[-1], atol=1e-13))
assert (
np.allclose(data_frames[-1].accel_measurements[0], data_frames[-2].accel_measurements[-1], atol=1e-13))
assert (
np.allclose(data_frames[-1].accel_measurements[0], data_frames[-2].accel_measurements[-1], atol=1e-13))
# add the last frame without any IMU data
data_frames.append(SequenceData.Frame(image_paths[-1], t_kp1, T_i_vkp1, v_vkp1,
np.zeros([0, 4, 4]), np.zeros([0]), np.zeros([0, 3]), np.zeros([0, 3])))
logger.print("\nProcessing data took %.2fs" % (time.time() - start_time))
df = SequenceData.save_as_pd(data_frames, np.array([0, 0, 9.808679801065017]), np.zeros(3), T_cam_imu, output_dir)
data = df.to_dict("list")
if not plot_figures:
logger.print("All done!")
return
# ============================== FIGURES FOR SANITY TESTS ==============================
# plot trajectory
start_time = time.time()
plotter = Plotter(output_dir)
p_poses = np.array(data["T_i_vk"])
p_timestamps = np.array(data["timestamp"])
p_velocities = np.array(data["v_vk_i_vk"])
p_imu_timestamps = np.concatenate([d[:-1] for d in data['imu_timestamps']])
p_gyro_measurements = np.concatenate([d[:-1] for d in data['gyro_measurements']])
p_accel_measurements = np.concatenate([d[:-1] for d in data["accel_measurements"]])
p_imu_poses = np.concatenate([d[:-1, :, :] for d in data["imu_poses"]])
assert (len(p_imu_timestamps) == len(p_gyro_measurements))
assert (len(p_imu_timestamps) == len(p_accel_measurements))
assert (len(p_imu_timestamps) == len(p_imu_poses))
# integrate accel to compare against velocity
p_accel_int = [p_velocities[0, :]]
p_accel_int_int = [p_poses[0, :3, 3]]
# g = np.array([0, 0, 9.80665])
g = np.array([0, 0, 9.808679801065017])
# g = np.array([0, 0, 9.8096])
for i in range(0, len(p_imu_timestamps) - 1):
dt = p_imu_timestamps[i + 1] - p_imu_timestamps[i]
C_i_vk = p_imu_poses[i, :3, :3]
C_vkp1_vk = p_imu_poses[i + 1, :3, :3].transpose().dot(p_imu_poses[i, :3, :3])
v_vk_i_vk = p_accel_int[-1]
v_vkp1_vk_vk = dt * (p_accel_measurements[i] - C_i_vk.transpose().dot(g))
v_vkp1_i_vk = v_vk_i_vk + v_vkp1_vk_vk
p_accel_int.append(C_vkp1_vk.dot(v_vkp1_i_vk))
p_accel_int_int.append(p_accel_int_int[-1] + p_imu_poses[i, :3, :3].dot(p_accel_int[-1]) * dt)
p_accel_int = np.array(p_accel_int)
p_accel_int_int = np.array(p_accel_int_int)
# poses from integrating velocity
p_vel_int_poses = [p_poses[0, :3, 3]]
for i in range(0, len(p_velocities) - 1):
dt = p_timestamps[i + 1] - p_timestamps[i]
dp = p_poses[i, :3, :3].dot(p_velocities[i]) * dt
p_vel_int_poses.append(p_vel_int_poses[-1] + dp)
p_vel_int_poses = np.array(p_vel_int_poses)
plotter.plot(([p_poses[:, 0, 3], p_poses[:, 1, 3]],
[p_vel_int_poses[:, 0], p_vel_int_poses[:, 1]],
[p_accel_int_int[:, 0], p_accel_int_int[:, 1]],),
"x [m]", "Y [m]", "XY Plot", labels=["dat_poses", "dat_vel_int", "dat_acc_int^2"], equal_axes=True)
plotter.plot(([p_poses[:, 0, 3], p_poses[:, 2, 3]],
[p_vel_int_poses[:, 0], p_vel_int_poses[:, 2]],
[p_accel_int_int[:, 0], p_accel_int_int[:, 2]],),
"X [m]", "Z [m]", "XZ Plot", labels=["dat_poses", "dat_vel_int", "dat_acc_int^2"], equal_axes=True)
plotter.plot(([p_poses[:, 1, 3], p_poses[:, 2, 3]],
[p_vel_int_poses[:, 1], p_vel_int_poses[:, 2]],
[p_accel_int_int[:, 1], p_accel_int_int[:, 2]],),
"Y [m]", "Z [m]", "YZ Plot", labels=["dat_poses", "dat_vel_int", "dat_acc_int^2"], equal_axes=True)
plotter.plot(([p_timestamps, p_poses[:, 0, 3]],
[p_timestamps, p_vel_int_poses[:, 0]],
[p_imu_timestamps, p_accel_int_int[:, 0]],),
"t [s]", "Y [m]", "X Plot From Zero", labels=["dat_poses", "dat_vel_int", "dat_acc_int^2"])
plotter.plot(([p_timestamps, p_poses[:, 1, 3]],
[p_timestamps, p_vel_int_poses[:, 1]],
[p_imu_timestamps, p_accel_int_int[:, 1]],),
"t [s]", "Z [m]", "Y Plot From Zero", labels=["dat_poses", "dat_vel_int", "dat_acc_int^2"])
plotter.plot(([p_timestamps, p_poses[:, 2, 3]],
[p_timestamps, p_vel_int_poses[:, 2]],
[p_imu_timestamps, p_accel_int_int[:, 2]],),
"t [s]", "Z [m]", "Z Plot From Zero", labels=["dat_poses", "dat_vel_int", "dat_acc_int^2"])
# plot trajectory rotated wrt to the first frame
p_poses_from_I = np.array([np.linalg.inv(p_poses[0]).dot(p) for p in p_poses])
plotter.plot(([p_poses_from_I[:, 0, 3], p_poses_from_I[:, 1, 3]],),
"x [m]", "Y [m]", "XY Plot From Identity", equal_axes=True)
plotter.plot(([p_poses_from_I[:, 0, 3], p_poses_from_I[:, 2, 3]],),
"X [m]", "Z [m]", "XZ Plot From Identity", equal_axes=True)
plotter.plot(([p_poses_from_I[:, 1, 3], p_poses_from_I[:, 2, 3]],),
"Y [m]", "Z [m]", "YZ Plot From Identity", equal_axes=True)
# plot p_velocities
plotter.plot(([p_timestamps, p_velocities[:, 0]], [p_timestamps, p_velocities[:, 1]],
[p_timestamps, p_velocities[:, 2]]), "t [s]", "v [m/s]", "YZ Plot",
labels=["dat_vx", "dat_vy", "dat_vz"])
# make sure the interpolated acceleration and gyroscope measurements are the same
plotter.plot(([p_imu_timestamps, p_gyro_measurements[:, 0]], [imu_timestamps, imu_data[:, wx]],),
"t [s]", "w [rad/s]", "Rot Vel X Verification")
plotter.plot(([p_imu_timestamps, p_gyro_measurements[:, 1]], [imu_timestamps, imu_data[:, wy]],),
"t [s]", "w [rad/s]", "Rot Vel Y Verification")
plotter.plot(([p_imu_timestamps, p_gyro_measurements[:, 2]], [imu_timestamps, imu_data[:, wz]],),
"t [s]", "w [rad/s]", "Rot Vel Z Verification")
plotter.plot(([p_imu_timestamps, p_accel_measurements[:, 0]], [imu_timestamps, imu_data[:, ax]],),
"t [s]", "a [m/s^2]", "Accel X Verification")
plotter.plot(([p_imu_timestamps, p_accel_measurements[:, 1]], [imu_timestamps, imu_data[:, ay]],),
"t [s]", "a [m/s^2]", "Accel Y Verification")
plotter.plot(([p_imu_timestamps, p_accel_measurements[:, 2]], [imu_timestamps, imu_data[:, az]],),
"t [s]", "a [m/s^2]", "Accel Z Verification")
# integrate gyroscope to compare against rotation
p_gyro_int = [data["T_i_vk"][0][:3, :3]]
for i in range(0, len(p_imu_timestamps) - 1):
dt = p_imu_timestamps[i + 1] - p_imu_timestamps[i]
p_gyro_int.append(p_gyro_int[-1].dot(exp_SO3(dt * p_gyro_measurements[i])))
p_gyro_int = np.array([log_SO3(o) for o in p_gyro_int])
p_orientation = np.array([log_SO3(p[:3, :3]) for p in data["T_i_vk"]])
plotter.plot(([p_imu_timestamps, np.unwrap(p_gyro_int[:, 0])], [p_timestamps, np.unwrap(p_orientation[:, 0])],),
"t [s]", "rot [rad/s]", "Theta X Cmp Plot", labels=["gyro_int", "dat_pose"])
plotter.plot(([p_imu_timestamps, np.unwrap(p_gyro_int[:, 1])], [p_timestamps, np.unwrap(p_orientation[:, 1])],),
"t [s]", "rot [rad/s]", "Theta Y Cmp Plot", labels=["gyro_int", "dat_pose"])
plotter.plot(([p_imu_timestamps, np.unwrap(p_gyro_int[:, 2])], [p_timestamps, np.unwrap(p_orientation[:, 2])],),
"t [s]", "rot [rad/s]", "Theta Z Cmp Plot", labels=["gyro_int", "dat_pose"])
vel_from_gps_rel_poses = []
for k in range(0, len(gps_poses) - 1):
dt = imu_timestamps[k + 1] - imu_timestamps[k]
T_i_vk = gps_poses[k]
T_i_vkp1 = gps_poses[k + 1]
T_vk_vkp1 = np.linalg.inv(T_i_vk).dot(T_i_vkp1)
vel_from_gps_rel_poses.append(T_vk_vkp1[0:3, 3] / dt)
# vel_from_gps_rel_poses.append(log_SE3(T_vk_vkp1)[0:3] / dt)
vel_from_gps_rel_poses = np.array(vel_from_gps_rel_poses)
plotter.plot(([imu_timestamps[1:], vel_from_gps_rel_poses[:, 0]],
[p_timestamps, p_velocities[:, 0]],
[p_imu_timestamps, p_accel_int[:, 0]],),
"t [s]", "v [m/s]", "Velocity X Cmp Plot", labels=["gps_rel", "dat_vel", "dat_accel_int"])
plotter.plot(([imu_timestamps[1:], vel_from_gps_rel_poses[:, 1]],
[p_timestamps, p_velocities[:, 1]],
[p_imu_timestamps, p_accel_int[:, 1]],),
"t [s]", "v [m/s]", "Velocity Y Cmp Plot", labels=["gps_rel", "dat_vel", "dat_accel_int"])
plotter.plot(([imu_timestamps[1:], vel_from_gps_rel_poses[:, 2]],
[p_timestamps, p_velocities[:, 2]],
[p_imu_timestamps, p_accel_int[:, 2]],),
"t [s]", "v [m/s]", "Velocity Z Cmp Plot", labels=["gps_rel", "dat_vel", "dat_accel_int"])
logger.print("Generating figures took %.2fs" % (time.time() - start_time))
logger.print("All done!")
def test_ekf_euroc(self):
output_dir = os.path.join(par.results_coll_dir, "test_ekf_euroc")
logger.initialize(output_dir, use_tensorboard=False)
# seqs = ["MH_01"]
seqs = [
"MH_01", "MH_02", "MH_03", "MH_04", "MH_05", "V1_01", "V1_02",
"V1_03", "V2_01", "V2_02", "V2_03"
]
# seqs2 = ["MH_01_eval", "MH_02_eval", "MH_03_eval", "MH_04_eval", "MH_05_eval",
# "V1_01_eval", "V1_02_eval", "V1_03_eval", "V2_01_eval", "V2_02_eval", "V2_03_eval"]
# seqs = seqs + seqs2
error_calc = EurocErrorCalc(seqs)
imu_covar = torch.diag(
torch.tensor([
1e-3, 1e-3, 1e-3, 1e-5, 1e-5, 1e-5, 1e-1, 1e-1, 1e-1, 1e-2,
1e-2, 1e-2
]))
vis_meas_covar = torch.diag(
torch.tensor([1e-3, 1e-3, 1e-3, 1e-3, 1e-3,
1e-3])).view(1, 1, 6, 6)
init_covar = np.eye(18, 18)
init_covar[0:3, 0:3] = np.eye(3, 3) * 1e-2 # g
init_covar[3:9, 3:9] = np.zeros([6, 6]) # C,r
init_covar[9:12, 9:12] = np.eye(3, 3) * 1e-2 # v
init_covar[12:15, 12:15] = np.eye(3, 3) * 1e-2 # bw
init_covar[15:18, 15:18] = np.eye(3, 3) * 1e2 # ba
init_covar = torch.tensor(init_covar,
dtype=torch.float32).view(1, 18, 18)
ekf = IMUKalmanFilter()
for seq in seqs:
subseqs = get_subseqs([seq],
2,
overlap=1,
sample_times=1,
training=False)
dataset = SubseqDataset(subseqs, (par.img_h, par.img_w),
0,
1,
True,
training=False,
no_image=True)
dataloader = torch.utils.data.DataLoader(dataset,
batch_size=1,
shuffle=False,
num_workers=0)
seq_data = SequenceData(seq)
est_poses = []
est_ekf_states = []
est_ekf_covars = []
for i, data in enumerate(dataloader):
# print('%d/%d (%.2f%%)' % (i, len(dataloader), i * 100 / len(dataloader)), end="\n")
_, _, imu_data, init_state, _, gt_poses_inv, gt_rel_poses = data
gt_rel_poses = gt_rel_poses.view(1, 1, 6, 1)
if i == 0:
pose, ekf_state, ekf_covar = ekf.forward(
imu_data, imu_covar, gt_poses_inv[:, 0], init_state,
init_covar, gt_rel_poses, vis_meas_covar,
torch.eye(4, 4).view(1, 4, 4))
est_poses.append(pose[:, 0])
est_ekf_states.append(ekf_state[:, 0])
est_ekf_covars.append(ekf_covar[:, 0])
est_poses.append(pose[:, -1])
est_ekf_states.append(ekf_state[:, -1])
est_ekf_covars.append(ekf_covar[:, -1])
else:
pose, ekf_state, ekf_covar = ekf.forward(
imu_data, imu_covar, est_poses[-1], est_ekf_states[-1],
est_ekf_covars[-1], gt_rel_poses, vis_meas_covar,
torch.eye(4, 4).view(1, 4, 4))
est_poses.append(pose[:, -1])
est_ekf_states.append(ekf_state[:, -1])
est_ekf_covars.append(ekf_covar[:, -1])
est_poses_np = torch.stack(
est_poses,
1).squeeze().detach().cpu().numpy().astype("float64")
err = error_calc.accumulate_error(seq, np.linalg.inv(est_poses_np))
logger.print("Error: %.5f" % err)
logger.print("Plotting figures...")
plot_ekf_data(os.path.join(output_dir, seq),
seq_data.get_timestamps(), seq_data.get_poses(),
seq_data.get_velocities(),
torch.stack(est_poses, 1).squeeze(),
torch.stack(est_ekf_states, 1).squeeze())
logger.print("Finished Sequence %s" % seq)
logger.print("Done! Ave Error: %.5f" % error_calc.get_average_error())
def calc_image_mean_std(sequences):
logger.initialize(working_dir=par.data_dir, use_tensorboard=False)
logger.print("================ CALC IMAGE MEAN STD ================")
logger.print("Sequences: [%s]" % ",".join(sequences))
to_tensor = torchvision.transforms.ToTensor()
image_count = 0
mean_sum = np.array([0.0, 0.0, 0.0])
var_sum = np.array([0.0, 0.0, 0.0])
image_paths = []
for i, seq in enumerate(sequences):
print("Collecting image paths %d/%d (%.2f%%)" %
(i + 1, len(sequences), (i + 1) * 100 / len(sequences)),
end="\r")
image_paths += list(SequenceData(seq).get_images_paths())
print()
start_time = time.time()
for i, path in enumerate(image_paths):
print("Computing mean %d/%d (%.2f%%)" %
(i + 1, len(image_paths), (i + 1) * 100 / len(image_paths)),
end="\r")
img = np.array(to_tensor(Image.open(path)))
if par.minus_point_5:
img = img - 0.5
mean_sum += np.mean(img, (
1,
2,
))
image_count += 1
print("\nTook %.2fs" % (time.time() - start_time))
mean = mean_sum / image_count
num_pixels = 0
start_time = time.time()
for i, path in enumerate(image_paths):
print("Computing standard deviation %d/%d (%.2f%%)" %
(i + 1, len(image_paths), (i + 1) * 100 / len(image_paths)),
end="\r")
img = np.array(to_tensor(Image.open(path)))
if par.minus_point_5:
img = img - 0.5
img[0, :, :] = img[0, :, :] - mean[0]
img[1, :, :] = img[1, :, :] - mean[1]
img[2, :, :] = img[2, :, :] - mean[2]
var_sum += np.sum(np.square(img), (
1,
2,
))
num_pixels += img.shape[1] * img.shape[2]
print("\nTook %.2fs" % (time.time() - start_time))
std = np.sqrt(var_sum / (num_pixels - 1))
logger.print("Mean: [%f, %f, %f]" % (mean[0], mean[1], mean[2]))
logger.print("Std: [%f, %f, %f]" % (std[0], std[1], std[2]))
def package_euroc_data(seq_dir, cam_timestamps, imu_timestamps, imu_data, gt_timestamps, gt_data):
assert len(gt_timestamps) == len(imu_timestamps)
assert len(gt_timestamps) == len(imu_data)
assert np.max(np.abs(np.array(imu_timestamps) - np.array(gt_timestamps))) < 1000
assert cam_timestamps[0] == imu_timestamps[0]
assert cam_timestamps[-1] == imu_timestamps[-1]
data_frames = []
ref_time = np.datetime64(int(min([cam_timestamps[0], imu_timestamps[0], ])), "ns")
cam_period = 100 * 10 ** 6 # nanoseconds
imu_skip = 2
i_start = 0
# for i in range(0, len(cam_timestamps) - 1):
while i_start < len(cam_timestamps) - 1:
t_k = cam_timestamps[i_start]
i_end = (np.abs(np.array(cam_timestamps) - (cam_timestamps[i_start] + cam_period))).argmin()
t_kp1 = cam_timestamps[i_end]
imu_start_idx = imu_timestamps.index(t_k)
imu_end_idx = imu_timestamps.index(t_kp1)
if not t_kp1 - t_k == cam_period:
# ignore the last frame if it is does not at the desired rate
if i_end == len(cam_timestamps) - 1:
break
logger.print("WARN imu_end_idx - imu_start_idx != %.5s, "
"image: [%d -> %d] imu: [%d -> %d] time: [%d -> %d] diff %.5f"
% (cam_period / 10 ** 9, i_start, i_end, imu_start_idx, imu_end_idx, t_k, t_kp1,
(t_kp1 - t_k) / 10 ** 9))
imu_poses = []
imu_timestamps_k_kp1 = []
accel_measurements_k_kp1 = []
gyro_measurements_k_kp1 = []
for j in range(imu_start_idx, imu_end_idx + 1, imu_skip):
imu_pose = transformations.quaternion_matrix(gt_data[j, [qw, qx, qy, qz]])
imu_pose[0:3, 3] = gt_data[j, [px, py, pz]]
imu_poses.append(imu_pose)
imu_timestamps_k_kp1.append((np.datetime64(imu_timestamps[j], "ns") - ref_time) / np.timedelta64(1, "s"))
accel_measurements_k_kp1.append(imu_data[j, [ax, ay, az]])
gyro_measurements_k_kp1.append(imu_data[j, [wx, wy, wz]])
T_i_vk = imu_poses[0]
frame_k = SequenceData.Frame(os.path.join(seq_dir, "cam0", "data", "%09d.png" % t_k),
(np.datetime64(t_k, "ns") - ref_time) / np.timedelta64(1, "s"),
T_i_vk,
T_i_vk[0:3, 0:3].transpose().dot(gt_data[imu_start_idx, [vx, vy, vz]]), # v_vk
imu_poses,
imu_timestamps_k_kp1,
accel_measurements_k_kp1,
gyro_measurements_k_kp1,
timestamp_raw=t_k)
data_frames.append(frame_k)
i_start = i_end
T_i_vkp1 = imu_poses[-1]
data_frames.append(SequenceData.Frame(os.path.join(seq_dir, "cam0", "data", "%09d.png" % t_kp1),
(np.datetime64(t_kp1, "ns") - ref_time) / np.timedelta64(1, "s"),
T_i_vkp1,
T_i_vkp1[0:3, 0:3].transpose().dot(gt_data[imu_end_idx, [vx, vy, vz]]),
np.zeros([0, 4, 4]), np.zeros([0]), np.zeros([0, 3]), np.zeros([0, 3]),
timestamp_raw=t_kp1))
return data_frames
def preprocess_euroc(seq_dir, output_dir, cam_still_range):
logger.initialize(working_dir=output_dir, use_tensorboard=False)
logger.print("================ PREPROCESS EUROC ================")
logger.print("Preprocessing %s" % seq_dir)
logger.print("Output to: %s" % output_dir)
logger.print("Camera still range [%d -> %d]" % (cam_still_range[0], cam_still_range[1]))
left_cam_csv = open(os.path.join(seq_dir, 'cam0', 'data.csv'), 'r')
imu_csv = open(os.path.join(seq_dir, 'imu0', "data.csv"), 'r')
gt_csv = open(os.path.join(seq_dir, "state_groundtruth_estimate0", "data.csv"), "r")
cam_sensor_yaml_config = yaml.load(open(os.path.join(seq_dir, "cam0", "sensor.yaml")))
T_cam_imu = np.linalg.inv(np.array(cam_sensor_yaml_config["T_BS"]["data"]).reshape(4, 4))
cam_timestamps = []
imu_timestamps = []
imu_data = []
gt_timestamps = []
gt_data = []
left_cam_csv.readline() # skip header
for line in left_cam_csv:
line = line.split(",")
timestamp_str = line[0]
assert str(timestamp_str + ".png" == line[1])
cam_timestamps.append(int(timestamp_str))
imu_csv.readline() # skip header
for line in imu_csv:
line = line.split(",")
timestamp = int(line[0])
data = [float(line[i + 1]) for i in range(0, 6)]
imu_timestamps.append(timestamp)
imu_data.append(data)
gt_csv.readline() # skip header
for line in gt_csv:
line = line.split(",")
timestamp = int(line[0])
data = [float(line[i + 1]) for i in range(0, 16)]
gt_timestamps.append(timestamp)
gt_data.append(data)
cam_timestamps = cam_timestamps
imu_timestamps = imu_timestamps
imu_data = np.array(imu_data)
gt_timestamps = gt_timestamps
gt_data = np.array(gt_data)
assert np.all(np.diff(cam_timestamps) > 0), "nonmonotonic timestamp"
assert np.all(np.diff(imu_timestamps) > 0), "nonmonotonic timestamp"
assert np.all(np.diff(gt_timestamps) > 0), "nonmonotonic timestamp"
# align imu and ground truth timestamps, and the time difference should not be more than 1 us
assert (gt_timestamps[0] > imu_timestamps[0] and gt_timestamps[-1] < imu_timestamps[-1])
imu_gt_aligned_start_idx = (np.abs(np.array(imu_timestamps) - gt_timestamps[0])).argmin() # gta = gt aligned
imu_timestamps_gt_aligned = imu_timestamps[imu_gt_aligned_start_idx:imu_gt_aligned_start_idx + len(gt_timestamps)]
imu_data_gt_aligned = imu_data[imu_gt_aligned_start_idx:imu_gt_aligned_start_idx + len(gt_timestamps)]
gt_align_time_sync_diff = np.array(gt_timestamps) - np.array(imu_timestamps_gt_aligned)
assert np.all(np.abs(gt_align_time_sync_diff) < 1000), "timestamp out of sync by > 1 us"
# first_cam_timestamp
cam_imu_aligned_start_idx = -1
for i in range(0, len(cam_timestamps)):
if cam_timestamps[i] in imu_timestamps:
cam_imu_aligned_start_idx = i
break
assert cam_imu_aligned_start_idx >= 0
cam_imu_aligned_end_idx = -1
for i in range(len(cam_timestamps) - 1, -1, -1):
if cam_timestamps[i] in imu_timestamps:
cam_imu_aligned_end_idx = i
break
assert cam_imu_aligned_end_idx >= 0
assert cam_imu_aligned_start_idx < cam_still_range[1], "sanity check that the alignment is at the start"
imu_still_idx_start = imu_timestamps.index(cam_timestamps[max(cam_still_range[0], cam_imu_aligned_start_idx)])
imu_still_idx_end = imu_timestamps.index(cam_timestamps[cam_still_range[1]])
gyro_bias_from_still = np.mean(imu_data[imu_still_idx_start:imu_still_idx_end, [wx, wy, wz]], axis=0)
gravity_from_still = np.mean(imu_data[imu_still_idx_start:imu_still_idx_end, [ax, ay, az]], axis=0)
print("still estimated g_b0:", gravity_from_still)
print("still estimated g_b0 norm: ", np.linalg.norm(gravity_from_still))
# for training /validation
logger.print("Processing data for training...")
every_N_frames = 10
rounded_length = len(imu_timestamps_gt_aligned) // every_N_frames * every_N_frames
gravity_from_gt = find_initial_gravity(imu_timestamps_gt_aligned[0:rounded_length],
imu_data_gt_aligned[0:rounded_length],
gt_timestamps[0:rounded_length], gt_data[0:rounded_length], 10)
cam_gt_aligned_start_idx = -1
for i in range(0, len(cam_timestamps)):
if cam_timestamps[i] in imu_timestamps_gt_aligned:
cam_gt_aligned_start_idx = i
break
assert cam_gt_aligned_start_idx >= 0
cam_gt_aligned_end_idx = -1
for i in range(len(cam_timestamps) - 1, -1, -1):
if cam_timestamps[i] in imu_timestamps_gt_aligned:
cam_gt_aligned_end_idx = i
break
assert cam_gt_aligned_end_idx >= 0
gt_cam_aligned_start_idx = imu_timestamps_gt_aligned.index(cam_timestamps[cam_gt_aligned_start_idx])
gt_cam_aligned_end_idx = imu_timestamps_gt_aligned.index(cam_timestamps[cam_gt_aligned_end_idx])
logger.print("Camera index [%d -> %d]" % (cam_gt_aligned_start_idx, cam_gt_aligned_end_idx))
data_frames = package_euroc_data(seq_dir, cam_timestamps[cam_gt_aligned_start_idx:cam_gt_aligned_end_idx + 1],
imu_timestamps_gt_aligned[gt_cam_aligned_start_idx:gt_cam_aligned_end_idx + 1],
imu_data_gt_aligned[gt_cam_aligned_start_idx:gt_cam_aligned_end_idx + 1],
gt_timestamps[gt_cam_aligned_start_idx:gt_cam_aligned_end_idx + 1],
gt_data[gt_cam_aligned_start_idx:gt_cam_aligned_end_idx + 1])
SequenceData.save_as_pd(data_frames, gravity_from_gt, gyro_bias_from_still, T_cam_imu, output_dir)
copyfile(os.path.join(seq_dir, "state_groundtruth_estimate0", "data.csv"),
os.path.join(output_dir, "groundtruth.csv"))
# for evaluation
logger.print("Processing data for evaluation...")
imu_cam_aligned_start_idx = imu_timestamps.index(cam_timestamps[cam_still_range[1]])
imu_cam_aligned_end_idx = imu_timestamps.index(cam_timestamps[cam_imu_aligned_end_idx])
imu_timestamps_cam_aligned = imu_timestamps[imu_cam_aligned_start_idx:imu_cam_aligned_end_idx + 1]
imu_data_cam_aligned = imu_data[imu_cam_aligned_start_idx:imu_cam_aligned_end_idx + 1]
zeros_gt = np.zeros([len(imu_timestamps_cam_aligned), 16])
zeros_gt[: qw] = 1
logger.print("Camera index [%d -> %d]" % (cam_still_range[1], cam_imu_aligned_end_idx))
eval_output_dir = output_dir + "_eval"
data_frames = package_euroc_data(seq_dir, cam_timestamps[cam_still_range[1]:cam_imu_aligned_end_idx + 1],
imu_timestamps_cam_aligned,
imu_data_cam_aligned,
imu_timestamps_cam_aligned,
zeros_gt)
logger.make_dir_if_not_exist(eval_output_dir)
SequenceData.save_as_pd(data_frames, gravity_from_still, gyro_bias_from_still, T_cam_imu, eval_output_dir)
copyfile(os.path.join(seq_dir, "state_groundtruth_estimate0", "data.csv"),
os.path.join(eval_output_dir, "groundtruth.csv"))
import sys
import pandas as pd
import os
import numpy as np
import time
import transformations
from data_loader import SequenceData
from eval.kitti_eval_pyimpl import calc_kitti_seq_errors
results_dir = "/home/cs4li/Dev/deep_ekf_vio/results/final_thesis_results/KITTI_imu_only"
sequences = ["K01", "K04", "K06", "K07", "K08", "K09", "K10"]
for seq in sequences:
seq_data = SequenceData(seq)
est_traj = np.load(os.path.join(results_dir, seq, "est.npy"))
gt_traj = np.array([T for T in seq_data.df.loc[:, "T_i_vk"]])
d = np.sum([np.linalg.norm((np.linalg.inv(gt_traj[i]).dot(gt_traj[i + 1]))[:3,3]) for i in range(0, len(gt_traj) - 1)])
# timestamps
err = np.array(calc_kitti_seq_errors(gt_traj, est_traj)[0])
print("Seq Error seq %s (t,r): %.15f, %.15f, %.15f dist: %.15f" % (seq, np.average(err[:, 0]), np.average(err[:, 1]) * 180 / np.pi * 100,
np.average(err[:, 1]) * 180 / np.pi, d / 1e3))
# print("%.15f, %.15f" % (np.average(err[:, 0]), np.average(err[:, 1]) * 180 / np.pi))
import sys
import os
import numpy as np
import pandas as pd
sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)), ".."))
from params import par
from log import Logger
from data_loader import SequenceData
working_dir = os.path.abspath(sys.argv[1])
pose_est_dir = os.path.join(working_dir, "est_poses")
pose_gt_dir = os.path.join(working_dir, "gt_poses")
pose_est_files = sorted(os.listdir(pose_est_dir))
for i, pose_est_file in enumerate(pose_est_files):
sequence = os.path.splitext(pose_est_file)[0]
traj_est = np.load(os.path.join(pose_est_dir, "%s.npy" % sequence))
length = traj_est.shape[0]
traj_gt = SequenceData(sequence).get_poses()
traj_gt = traj_gt[0:length, :, :]
np.save(Logger.ensure_file_dir_exists(os.path.join(pose_gt_dir, "%s.npy" % sequence)), traj_gt)
print("All Done")
def gen_trajectory(model_file_path, sequences, seq_len, prop_lstm_states):
# Path
model_file_path = os.path.abspath(model_file_path)
assert (os.path.exists(model_file_path))
working_dir = os.path.join(os.path.dirname(model_file_path),
os.path.basename(model_file_path) + ".traj")
logger.initialize(working_dir=working_dir, use_tensorboard=False)
logger.print("================ GENERATE TRAJECTORY REL ================")
# Load model
logger.print("Constructing model...")
model = E2EVIO()
model = model.cuda()
logger.print("Loading model from: ", model_file_path)
model.load_state_dict(
logger.clean_state_dict_key(torch.load(model_file_path)))
model.eval()
logger.log_parameters()
logger.print("Using sequence length:", seq_len)
logger.print("Prop LSTM states:", prop_lstm_states)
logger.print("Sequences: \n" + "\n".join(sequences))
for seq in sequences:
logger.print("Generating trajectory for seq...", seq)
start_time = time.time()
subseqs = get_subseqs([seq],
seq_len,
overlap=1,
sample_times=1,
training=False)
dataset = SubseqDataset(subseqs, (par.img_h, par.img_w),
par.img_means,
par.img_stds,
par.minus_point_5,
training=False)
dataloader = torch.utils.data.DataLoader(dataset,
batch_size=1,
shuffle=False,
num_workers=4)
seq_data = SequenceData(seq)
gt_abs_poses = seq_data.get_poses()
timestamps = seq_data.get_timestamps()
if par.enable_ekf:
logger.print("With EKF enabled ...")
est_vis_meas_dict, vis_meas_covar_dict, est_poses_dict, est_states_dict, est_covars_dict = \
gen_trajectory_abs_iter(model, {seq: dataloader})
est_vis_meas = est_vis_meas_dict[seq]
vis_meas_covar = vis_meas_covar_dict[seq]
est_states = est_states_dict[seq]
est_covars = est_covars_dict[seq]
est_poses = est_poses_dict[seq]
np.save(
logger.ensure_file_dir_exists(
os.path.join(working_dir, "ekf_states", "vis_meas",
seq + ".npy")), est_vis_meas)
np.save(
logger.ensure_file_dir_exists(
os.path.join(working_dir, "ekf_states", "vis_meas_covar",
seq + ".npy")), vis_meas_covar)
np.save(
logger.ensure_file_dir_exists(
os.path.join(working_dir, "ekf_states", "poses",
seq + ".npy")), est_poses)
np.save(
logger.ensure_file_dir_exists(
os.path.join(working_dir, "ekf_states", "states",
seq + ".npy")), est_states)
np.save(
logger.ensure_file_dir_exists(
os.path.join(working_dir, "ekf_states", "covars",
seq + ".npy")), est_covars)
np.save(
logger.ensure_file_dir_exists(
os.path.join(working_dir, "ekf_states", "gt_velocities",
seq + ".npy")), seq_data.get_velocities())
est_poses = np.linalg.inv(
np.array(est_poses_dict[seq]).astype(np.float64))
else:
logger.print("Without EKF enabled ...")
est_poses, est_vis_meas, vis_meas_covar = gen_trajectory_rel_iter(
model,
dataloader,
prop_lstm_states,
initial_pose=gt_abs_poses[0, :, :])
np.save(
logger.ensure_file_dir_exists(
os.path.join(working_dir, "vis_meas", "meas", seq + ".npy")),
est_vis_meas)
np.save(
logger.ensure_file_dir_exists(
os.path.join(working_dir, "vis_meas", "covar", seq + ".npy")),
vis_meas_covar)
np.save(
logger.ensure_file_dir_exists(
os.path.join(working_dir, "est_poses", seq + ".npy")),
est_poses)
np.save(
logger.ensure_file_dir_exists(
os.path.join(working_dir, "gt_poses", seq + ".npy")),
gt_abs_poses[:len(est_poses)]) # ensure same length as est poses
np.save(
logger.ensure_file_dir_exists(
os.path.join(working_dir, "timestamps", seq + ".npy")),
timestamps[:len(est_poses)])
logger.print("Done, took %.2f seconds" % (time.time() - start_time))
return working_dir