def test_get_ape_trans_0(self):
""" Test equivalent trajectories for zero ATE """
pos_xyz = [[random.random() for _ in range(3)] for _ in range(15)]
quat_wxyz = [[random.random() for _ in range(4)] for _ in range(15)]
timestamps = [i for i in range(15)]
traj_1 = trajectory.PoseTrajectory3D(pos_xyz, quat_wxyz, timestamps)
traj_2 = trajectory.PoseTrajectory3D(pos_xyz, quat_wxyz, timestamps)
ape_metric_trans = get_ape_trans((traj_1, traj_2))
self.assertEqual(ape_metric_trans.pose_relation,
metrics.PoseRelation.translation_part)
self.assertEqual(ape_metric_trans.unit, metrics.Unit.meters)
self.assertTrue(
np.allclose(ape_metric_trans.error,
[0. for i in range(len(ape_metric_trans.error))],
atol=1e-5))
def test_convert_abs_traj_to_rel_traj_1(self):
""" Test equivalent trajectories for zero relative ATE (not to-scale) """
pos_xyz = np.array([[random.random() for _ in range(3)]
for _ in range(15)])
quat_wxyz = np.array([[random.random() for _ in range(4)]
for _ in range(15)])
timestamps = np.array([i for i in range(15)])
traj_1 = trajectory.PoseTrajectory3D(pos_xyz, quat_wxyz, timestamps)
pos_xyz = [pos * 2 for pos in pos_xyz]
traj_2 = trajectory.PoseTrajectory3D(pos_xyz, quat_wxyz, timestamps)
traj_1 = convert_abs_traj_to_rel_traj(traj_1, True)
traj_2 = convert_abs_traj_to_rel_traj(traj_2, True)
ape_metric_trans = get_ape_trans((traj_1, traj_2))
self.assertTrue(
np.allclose(ape_metric_trans.error,
[0. for i in range(len(ape_metric_trans.error))],
atol=1e-5))
def test_get_ape_trans_1(self):
""" Test a 1-meter ATE on the first pose only """
pos_xyz = [[random.random() for _ in range(3)] for _ in range(15)]
quat_wxyz = [[random.random() for _ in range(4)] for _ in range(15)]
timestamps = [i for i in range(15)]
pos_xyz[0] = [0, 0, 0]
traj_1 = trajectory.PoseTrajectory3D(pos_xyz, quat_wxyz, timestamps)
pos_xyz[0] = [
1,
0,
0,
]
traj_2 = trajectory.PoseTrajectory3D(pos_xyz, quat_wxyz, timestamps)
ape_metric_trans = get_ape_trans((traj_1, traj_2))
self.assertAlmostEqual(ape_metric_trans.error[0], 1.)
self.assertTrue(
np.allclose(ape_metric_trans.error[1:],
[0. for i in range(len(ape_metric_trans.error[1:]))],
atol=1e-5))
ax.set_ylabel("Relative Angles [rad]")
plt.show()
# %% [markdown]
# ### Mono Relative-pose Errors (RPE)
#
# Calculate relative-pose-error (RPE) for the mono ransac poses obtained in the frontend.
#
# These are relative poses between keyframes and do not represent an entire trajectory. As such, they cannot be processed using the normal EVO evaluation pipeline.
#
# %%
# Get RPE for entire relative trajectory.
ape_rot = get_ape_rot((traj_ref_cam_rel, traj_est_rel))
ape_tran = get_ape_trans((traj_ref_cam_rel, traj_est_rel))
# calculate the translation errors up-to-scale
trans_errors = []
for i in range(len(traj_ref_cam_rel.timestamps)):
# normalized translation vector from gt
t_ref = traj_ref_cam_rel.poses_se3[i][0:3, 3]
if np.linalg.norm(t_ref) > 1e-6:
t_ref /= np.linalg.norm(t_ref)
# normalized translation vector from mono ransac
t_est = traj_est_rel.poses_se3[i][0:3, 3]
if np.linalg.norm(t_est) > 1e-6:
t_est /= np.linalg.norm(t_est)
#
# The following two plots show 1) VIO's absolute translation errors (ATE) in meters with respect to time, and 2) estimated trajectory color coded by magnitudes of the ATE.
# %%
# Plot APE of trajectory rotation and translation parts.
num_of_poses = traj_est.num_poses
traj_est.reduce_to_ids(
range(int(discard_n_start_poses), int(num_of_poses - discard_n_end_poses),
1))
traj_ref.reduce_to_ids(
range(int(discard_n_start_poses), int(num_of_poses - discard_n_end_poses),
1))
seconds_from_start = [t - traj_est.timestamps[0] for t in traj_est.timestamps]
ape_tran = get_ape_trans((traj_ref, traj_est))
fig1 = plot_metric(ape_tran, "VIO ATE in Meters")
plt.show()
# %%
# Plot the ground truth and estimated trajectories against each other with APE overlaid.
fig = plot_traj_colormap_ape(
ape_tran,
traj_ref,
traj_est,
plot_title="VIO Trajectory Tracking - Color Coded by ATE",
)
plt.show()
# %% [markdown]
# ### Absolute Rotation Errors
plt.figure(figsize=(18, 10))
# plt.plot(gt_angles_timestamps, np.rad2deg(gt_angles), "b", label="GT")
# plt.plot(gt_angles_timestamps, np.rad2deg(est_angles), "g", label="Est")
plt.plot(gt_angles_timestamps, np.rad2deg(rot_errors), "r", label="Error")
plt.legend(loc="upper right")
ax = plt.gca()
ax.set_xlabel("Timestamps [-]")
ax.set_ylabel("Rotation Part Error [deg]")
ax.set_title("Pose Recovery (3d3d or 2d3d Ransac) Rotation Part Error")
plt.show()
# %%
# Get RPE for entire relative trajectory.
ape_rot = get_ape_rot((traj_ref_cam_rel, traj_est_rel))
ape_tran = get_ape_trans((traj_ref_cam_rel, traj_est_rel))
# calculate the translation errors
trans_errors = []
for i in range(len(traj_ref_cam_rel.timestamps)):
t_ref = traj_ref_cam_rel.poses_se3[i][:3, 3]
t_est = traj_est_rel.poses_se3[i][:3, 3]
trans_errors.append(np.linalg.norm(t_ref - t_est))
plt.figure(figsize=(18, 10))
plt.plot(traj_ref_cam_rel.timestamps, trans_errors, "r", label="Error")
plt.legend(loc="upper right")
ax = plt.gca()
ax.set_xlabel("Timestamps [-]")
ax.set_ylabel("Translation Part Error [m]")