def re_normalize(m3):
inv = np.linalg.inv(m3)
tra = m3.transpose()
m1 = (inv + tra) / 2
m2 = np.linalg.inv(m1)
A = tr.euler_from_matrix(m2)
m = tr.euler_matrix(A[0], A[1], A[2])
return m[:3, :3]
def get_orientations_euler(self, axes="sxyz"):
if hasattr(self, "_poses_se3"):
return np.array(
[tr.euler_from_matrix(p, axes=axes) for p in self._poses_se3])
elif hasattr(self, "_orientations_quat_wxyz"):
return np.array([
tr.euler_from_quaternion(q, axes=axes)
for q in self._orientations_quat_wxyz
])
def orientations_euler(self):
if not hasattr(self, "_orientations_euler"):
if hasattr(self, "_poses_se3"):
self._orientations_euler \
= np.array([tr.euler_from_matrix(p, axes="sxyz") for p in self._poses_se3])
elif hasattr(self, "_orientations_quat_wxyz"):
self._orientations_euler \
= np.array([tr.euler_from_quaternion(q, axes="sxyz")
for q in self._orientations_quat_wxyz])
return self._orientations_euler
def orientations_euler(self):
if not hasattr(self, "_orientations_euler"):
if hasattr(self, "_poses_se3"):
self._orientations_euler \
= np.array([tr.euler_from_matrix(p, axes="sxyz") for p in self._poses_se3])
elif hasattr(self, "_orientations_quat_wxyz"):
self._orientations_euler \
= np.array([tr.euler_from_quaternion(q, axes="sxyz")
for q in self._orientations_quat_wxyz])
return self._orientations_euler
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,
align_odom: 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)
elif align_odom:
logger.debug(SEP)
alignment_transformation = traj_est.align_odom(traj_ref)
if alignment_transformation is not None:
print('alignment_rotation: ',
transformations.euler_from_matrix(alignment_transformation))
print('alignment_translation: ', alignment_transformation[:3, 3])
# 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 get_A(Q):
A = tr.euler_from_matrix(Q[:3, :3], axes='sxzy')
A = np.array(A) * 180.0/np.pi
return A
def fromMatrix(R, order="rXYZ"):
if order == "rXYZ":
x, y, z = euler_from_matrix(R, "rxyz")
return Euler(x, y, z)
else:
raise Exception("Not Implemented Yet!")
