def test_transform_sim3(self):
path = helpers.fake_path(10)
path_transformed = copy.deepcopy(path)
t = lie.sim3(r=lie.random_so3(), t=np.ones(3), s=1.234)
path_transformed.transform(t)
self.assertAlmostEqual(path_transformed.path_length,
path.path_length * 1.234)
def ape(traj_ref: PosePath3D,
traj_est: PosePath3D,
pose_relation: metrics.PoseRelation,
align: bool = False,
correct_scale: bool = False,
n_to_align: int = -1,
align_origin: bool = False,
ref_name: str = "reference",
est_name: str = "estimate") -> Result:
# Align the trajectories.
only_scale = correct_scale and not align
alignment_transformation = None
if align or correct_scale:
logger.debug(SEP)
alignment_transformation = lie_algebra.sim3(
*traj_est.align(traj_ref, correct_scale, only_scale, n=n_to_align))
elif align_origin:
logger.debug(SEP)
alignment_transformation = traj_est.align_origin(traj_ref)
# Calculate APE.
logger.debug(SEP)
data = (traj_ref, traj_est)
ape_metric = metrics.APE(pose_relation)
ape_metric.process_data(data)
title = str(ape_metric)
if align and not correct_scale:
title += "\n(with SE(3) Umeyama alignment)"
elif align and correct_scale:
title += "\n(with Sim(3) Umeyama alignment)"
elif only_scale:
title += "\n(scale corrected)"
elif align_origin:
title += "\n(with origin alignment)"
else:
title += "\n(not aligned)"
if (align or correct_scale) and n_to_align != -1:
title += " (aligned poses: {})".format(n_to_align)
ape_result = ape_metric.get_result(ref_name, est_name)
ape_result.info["title"] = title
logger.debug(SEP)
logger.info(ape_result.pretty_str())
ape_result.add_trajectory(ref_name, traj_ref)
ape_result.add_trajectory(est_name, traj_est)
if isinstance(traj_est, PoseTrajectory3D):
seconds_from_start = np.array(
[t - traj_est.timestamps[0] for t in traj_est.timestamps])
ape_result.add_np_array("seconds_from_start", seconds_from_start)
ape_result.add_np_array("timestamps", traj_est.timestamps)
if alignment_transformation is not None:
ape_result.add_np_array("alignment_transformation_sim3",
alignment_transformation)
return ape_result
def test_sim3_inverse(self):
r = lie.random_so3()
t = np.array([1, 2, 3])
s = random.random() * 10
p = lie.sim3(r, t, s)
self.assertTrue(lie.is_sim3(p, s))
p_inv = lie.sim3_inverse(p)
self.assertTrue(np.allclose(p_inv.dot(p), np.eye(4)))
def test_sim3_scale_effect(self):
r = lie.random_so3()
t = np.array([0, 0, 0])
s = random.random() * 10
x = np.array([1, 0, 0, 1]).T # homogeneous vector
p = lie.sim3(r, t, s)
self.assertTrue(lie.is_sim3(p, s))
x = p.dot(x) # apply Sim(3) transformation
self.assertTrue(np.equal(x, lie.se3(r).dot(np.array([s, 0, 0, 1]))).all())
def test_is_sim3(self):
r = lie.random_so3()
t = np.array([1, 2, 3])
s = 3
p = lie.sim3(r, t, s)
self.assertTrue(lie.is_sim3(p, s))
def rpe(traj_ref: PosePath3D,
traj_est: PosePath3D,
pose_relation: metrics.PoseRelation,
delta: float,
delta_unit: metrics.Unit,
rel_delta_tol: float = 0.1,
all_pairs: bool = False,
align: bool = False,
correct_scale: bool = False,
n_to_align: int = -1,
align_origin: bool = False,
ref_name: str = "reference",
est_name: str = "estimate",
support_loop: bool = False) -> Result:
# Align the trajectories.
only_scale = correct_scale and not align
alignment_transformation = None
if align or correct_scale:
logger.debug(SEP)
alignment_transformation = lie_algebra.sim3(
*traj_est.align(traj_ref, correct_scale, only_scale, n=n_to_align))
elif align_origin:
logger.debug(SEP)
alignment_transformation = traj_est.align_origin(traj_ref)
# Calculate RPE.
logger.debug(SEP)
data = (traj_ref, traj_est)
rpe_metric = metrics.RPE(pose_relation, delta, delta_unit, rel_delta_tol,
all_pairs)
rpe_metric.process_data(data)
title = str(rpe_metric)
if align and not correct_scale:
title += "\n(with SE(3) Umeyama alignment)"
elif align and correct_scale:
title += "\n(with Sim(3) Umeyama alignment)"
elif only_scale:
title += "\n(scale corrected)"
elif align_origin:
title += "\n(with origin alignment)"
else:
title += "\n(not aligned)"
if (align or correct_scale) and n_to_align != -1:
title += " (aligned poses: {})".format(n_to_align)
rpe_result = rpe_metric.get_result(ref_name, est_name)
rpe_result.info["title"] = title
logger.debug(SEP)
logger.info(rpe_result.pretty_str())
# Restrict trajectories to delta ids for further processing steps.
if support_loop:
# Avoid overwriting if called repeatedly e.g. in Jupyter notebook.
import copy
traj_ref = copy.deepcopy(traj_ref)
traj_est = copy.deepcopy(traj_est)
traj_ref.reduce_to_ids(rpe_metric.delta_ids)
traj_est.reduce_to_ids(rpe_metric.delta_ids)
rpe_result.add_trajectory(ref_name, traj_ref)
rpe_result.add_trajectory(est_name, traj_est)
if isinstance(traj_est, PoseTrajectory3D):
seconds_from_start = np.array(
[t - traj_est.timestamps[0] for t in traj_est.timestamps])
rpe_result.add_np_array("seconds_from_start", seconds_from_start)
rpe_result.add_np_array("timestamps", traj_est.timestamps)
if alignment_transformation is not None:
rpe_result.add_np_array("alignment_transformation_sim3",
alignment_transformation)
return rpe_result
def process_data(self,
data: PathPair,
align=False,
align_origin=False,
align_odom=False) -> None:
"""
Calculate the APE of segments from a batch of SE(3) poses from trajectories
:param data: tuple (traj_ref, traj_est) with:
traj_ref: reference evo.trajectory.PosePath or derived
traj_est: estimated evo.trajectory.PosePath or derived
"""
if len(data) != 2:
raise MetricsException(
"please provide data tuple as: (traj_ref, traj_est)")
traj_ref, traj_est = data
if traj_ref.num_poses != traj_est.num_poses:
raise MetricsException(
"trajectories must have same number of poses")
step_size = 10
i = 0
for first_pose in range(0, traj_ref.num_poses, step_size):
print('processing segment ', i)
i = i + 1
last_pose = self.last_pose_from_segment_length(
traj_ref, first_pose)
if (last_pose == -1):
break
traj_ref_segment = trajectory.PosePath3D(
poses_se3=traj_ref.poses_se3[first_pose:last_pose])
traj_est_segment = trajectory.PosePath3D(
poses_se3=traj_est.poses_se3[first_pose:last_pose])
# if align or correct_scale:
if align:
alignment_transformation = lie.sim3(*traj_est_segment.align(
traj_ref_segment, False, False, -1))
elif align_origin:
print('align origin: ', align_origin)
alignment_transformation = traj_est_segment.align_origin(
traj_ref_segment)
elif align_odom:
alignment_transformation = traj_est_segment.align_odom(
traj_ref_segment)
self.segment_index.append([first_pose, last_pose])
if i == 1:
print('starting idx: ', first_pose)
print('ending idx: ', last_pose)
# print('origin 0 position: ', traj_est_segment.poses_se3[0])
# print('origin 1 position: ', traj_est_segment.poses_se3[1])
# print('ref 0 position: ', traj_ref_segment.poses_se3[0])
# print('results 1 position: ', traj_ref_segment.poses_se3[1])
fig = plt.figure()
plot_option = '3D'
if plot_option == '2D':
ax = fig.add_subplot() #projection="3d"
elif plot_option == '3D':
ax = fig.add_subplot(projection="3d")
x_ref = traj_ref_segment.positions_xyz[:, 0]
y_ref = traj_ref_segment.positions_xyz[:, 1]
z_ref = traj_ref_segment.positions_xyz[:, 2]
front_vector_origin = np.array([1, 0, 0, 1])
f_vecs = np.array([
(np.dot(pose, front_vector_origin) - pose[:, 3]) * 10
for pose in traj_ref_segment.poses_se3
])
if plot_option == '2D':
ax.plot(x_ref, y_ref) #, z_ref)
# plt.quiver(x_ref, y_ref, f_vecs[:,0], f_vecs[:,1], color='g')#, arrow_length_ratio=0.05)
elif plot_option == '3D':
ax.plot(x_ref, y_ref, z_ref)
plt.quiver(x_ref,
y_ref,
z_ref,
f_vecs[:, 0],
f_vecs[:, 1],
f_vecs[:, 2],
color='g',
arrow_length_ratio=0.05)
x_est = traj_est_segment.positions_xyz[:, 0]
y_est = traj_est_segment.positions_xyz[:, 1]
z_est = traj_est_segment.positions_xyz[:, 2]
f_vecs = np.array([
(np.dot(pose, front_vector_origin) - pose[:, 3]) * 10
for pose in traj_est_segment.poses_se3
])
if plot_option == '2D':
ax.plot(x_est, y_est, color='r')
# plt.quiver(x_est, y_est, f_vecs[:,0], f_vecs[:,1], color='b')
elif plot_option == '3D':
ax.plot(x_est, y_est, z_est, color='r')
plt.quiver(x_est,
y_est,
z_est,
f_vecs[:, 0],
f_vecs[:, 1],
f_vecs[:, 2],
color='b',
arrow_length_ratio=0.05)
plt.title('segment_length: ' + str(self.segment_length) + 'm')
plt.savefig('first_segment_aligned.png')
if self.plot_show:
plt.show()
if self.pose_relation == PoseRelation.translation_part:
segment_E = traj_est_segment.positions_xyz - traj_ref_segment.positions_xyz
elif self.pose_relation == PoseRelation.z:
segment_E = traj_est_segment.positions_xyz - traj_ref_segment.positions_xyz
elif self.pose_relation == PoseRelation.xy:
segment_E = traj_est_segment.positions_xyz - traj_ref_segment.positions_xyz
else:
segment_E = [
self.ape_base(x_t, x_t_star) for x_t, x_t_star in zip(
traj_est_segment.poses_se3, traj_ref_segment.poses_se3)
]
logger.debug("Compared {} absolute pose pairs.".format(len(
self.E)))
logger.debug("Calculating APE for {} pose relation...".format(
(self.pose_relation.value)))
if self.pose_relation == PoseRelation.translation_part:
segment_error = np.array(
[np.linalg.norm(E_i) for E_i in segment_E])
elif self.pose_relation == PoseRelation.z:
segment_error = np.array([E_i[2] for E_i in segment_E])
elif self.pose_relation == PoseRelation.xy:
segment_error = np.array(
[np.linalg.norm(E_i[0:2]) for E_i in segment_E])
elif self.pose_relation == PoseRelation.rotation_part:
segment_error = np.array([
np.linalg.norm(lie.so3_from_se3(E_i) - np.eye(3))
for E_i in segment_E
])
elif self.pose_relation == PoseRelation.full_transformation:
segment_error = np.array(
[np.linalg.norm(E_i - np.eye(4)) for E_i in segment_E])
elif self.pose_relation == PoseRelation.rotation_angle_rad:
segment_error = np.array(
[abs(lie.so3_log(E_i[:3, :3])) for E_i in segment_E])
elif self.pose_relation == PoseRelation.rotation_angle_deg:
segment_error = np.array([
abs(lie.so3_log(E_i[:3, :3])) * 180 / np.pi
for E_i in segment_E
])
else:
raise MetricsException("unsupported pose_relation")
self.E.append(segment_E)
self.seg_error.append(segment_error)
