def align_trajectory(traj,
traj_ref,
correct_scale=False,
correct_only_scale=False,
n=-1,
return_parameters=False,
correct_origin=False):
"""
align a trajectory to a reference using Umeyama alignment
:param traj: the trajectory to align
:param traj_ref: reference trajectory
:param correct_scale: set to True to adjust also the scale
:param correct_only_scale: set to True to correct the scale, but not the pose
:param n: the number of poses to use, counted from the start (default: all)
:param return_parameters: also return result parameters of Umeyama's method
:return: the aligned trajectory
If return_parameters is set, the rotation matrix, translation vector and
scaling parameter of Umeyama's method are also returned.
"""
# otherwise np arrays will be references and mess up stuff
traj_aligned = copy.deepcopy(traj)
# quat = traj_ref.orientations_quat_wxyz[0,:]
# r_ref = tr.quaternion_matrix(quat)[:3,:3]
# t_ref = traj_ref.positions_xyz[0,:]
# traj_aligned.transform(lie.se3(r_ref, t_ref),propagate=True)
with_scale = correct_scale or correct_only_scale
if correct_only_scale:
logger.debug("Correcting scale...")
else:
logger.debug("Aligning using Umeyama's method..." +
(" (with scale correction)" if with_scale else ""))
if n == -1:
r_a, t_a, s = geometry.umeyama_alignment(traj_aligned.positions_xyz.T,
traj_ref.positions_xyz.T,
with_scale)
else:
r_a, t_a, s = geometry.umeyama_alignment(
traj_aligned.positions_xyz[:n, :].T,
traj_ref.positions_xyz[:n, :].T, with_scale)
if not correct_only_scale:
logger.debug("Rotation of alignment:\n{}"
"\nTranslation of alignment:\n{}".format(r_a, t_a))
logger.debug("Scale correction: {}".format(s))
if correct_only_scale:
traj_aligned.scale(s)
elif correct_scale:
traj_aligned.scale(s)
traj_aligned.transform(lie.se3(r_a, t_a))
else:
traj_aligned.transform(lie.se3(r_a, t_a))
if return_parameters:
return traj_aligned, r_a, t_a, s
else:
return traj_aligned
def align(self,
traj_ref: 'PosePath3D',
correct_scale: bool = False,
correct_only_scale: bool = False,
n: int = -1) -> geometry.UmeyamaResult:
"""
align to a reference trajectory using Umeyama alignment
:param traj_ref: reference trajectory
:param correct_scale: set to True to adjust also the scale
:param correct_only_scale: set to True to correct the scale, but not the pose
:param n: the number of poses to use, counted from the start (default: all)
:return: the result parameters of the Umeyama algorithm
"""
with_scale = correct_scale or correct_only_scale
if correct_only_scale:
logger.debug("Correcting scale...")
else:
logger.debug("Aligning using Umeyama's method..." +
(" (with scale correction)" if with_scale else ""))
if n == -1:
r_a, t_a, s = geometry.umeyama_alignment(self.positions_xyz.T,
traj_ref.positions_xyz.T,
with_scale)
else:
r_a, t_a, s = geometry.umeyama_alignment(
self.positions_xyz[:n, :].T, traj_ref.positions_xyz[:n, :].T,
with_scale)
if not correct_only_scale:
logger.debug("Rotation of alignment:\n{}"
"\nTranslation of alignment:\n{}".format(r_a, t_a))
logger.debug("Scale correction: {}".format(s))
if correct_only_scale:
self.scale(s)
elif correct_scale:
self.scale(s)
self.transform(lie.se3(r_a, t_a))
else:
self.transform(lie.se3(r_a, t_a))
return r_a, t_a, s
def align_trajectory_by_vicon_whole(trj_est, trj_vicon):
trj_vicon_sync, trj_est_sync = sync.associate_trajectories(
trj_vicon, trj_est)
trj_copy = copy.deepcopy(
trj_est_sync
) # otherwise np arrays will be references and mess up stuff
r_a, t_a, s = geometry.umeyama_alignment(trj_copy.positions_xyz.T,
trj_vicon_sync.positions_xyz.T,
False)
res = lie.se3(r_a, t_a)
return res
def align_trajectory(traj, traj_ref, correct_scale=False,
correct_only_scale=False, n=-1):
"""
align a trajectory to a reference using Umeyama alignment
:param traj: the trajectory to align
:param traj_ref: reference trajectory
:param correct_scale: set to True to adjust also the scale
:param correct_only_scale: set to True to correct the scale, but not the pose
:param n: the number of poses to use, counted from the start (default: all)
:return: the aligned trajectory
"""
# otherwise np arrays will be references and mess up stuff
traj_aligned = copy.deepcopy(traj)
with_scale = correct_scale or correct_only_scale
if correct_only_scale:
logger.debug("Correcting scale...")
else:
logger.debug("Aligning using Umeyama's method..." +
(" (with scale correction)" if with_scale else ""))
if n == -1:
r_a, t_a, s = geometry.umeyama_alignment(
traj_aligned.positions_xyz.T, traj_ref.positions_xyz.T, with_scale)
else:
r_a, t_a, s = geometry.umeyama_alignment(
traj_aligned.positions_xyz[:n, :].T,
traj_ref.positions_xyz[:n, :].T, with_scale)
if not correct_only_scale:
logger.debug("Rotation of alignment:\n{}"
"\nTranslation of alignment:\n{}".format(r_a, t_a))
logger.debug("Scale correction: {}".format(s))
if correct_only_scale:
traj_aligned.scale(s)
elif correct_scale:
traj_aligned.scale(s)
traj_aligned.transform(lie.se3(r_a, t_a))
else:
traj_aligned.transform(lie.se3(r_a, t_a))
return traj_aligned
def align_trajectory(traj,
traj_ref,
correct_scale=False,
correct_only_scale=False,
n=-1):
"""
align a trajectory to a reference using Umeyama alignment
:param traj: the trajectory to align
:param traj_ref: reference trajectory
:param correct_scale: set to True to adjust also the scale
:param correct_only_scale: set to True to correct the scale, but not the pose
:param n: the number of poses to use, counted from the start (default: all)
:return: the aligned trajectory
"""
traj_aligned = copy.deepcopy(
traj) # otherwise np arrays will be references and mess up stuff
with_scale = correct_scale or correct_only_scale
if n == -1:
r_a, t_a, s = geometry.umeyama_alignment(traj_aligned.positions_xyz.T,
traj_ref.positions_xyz.T,
with_scale)
else:
r_a, t_a, s = geometry.umeyama_alignment(
traj_aligned.positions_xyz[:n, :].T,
traj_ref.positions_xyz[:n, :].T, with_scale)
if not correct_only_scale:
logging.debug("rotation of alignment:\n" + str(r_a) +
"\ntranslation of alignment:\n" + str(t_a))
logging.debug("scale correction: " + str(s))
if correct_only_scale:
traj_aligned.scale(s)
elif correct_scale:
traj_aligned.scale(s)
traj_aligned.transform(lie.se3(r_a, t_a))
else:
traj_aligned.transform(lie.se3(r_a, t_a))
return traj_aligned
def align_trajectory(traj, traj_ref, correct_scale=False, correct_only_scale=False, n=-1):
"""
align a trajectory to a reference using Umeyama alignment
:param traj: the trajectory to align
:param traj_ref: reference trajectory
:param correct_scale: set to True to adjust also the scale
:param correct_only_scale: set to True to correct the scale, but not the pose
:param n: the number of poses to use, counted from the start (default: all)
:return: the aligned trajectory
"""
traj_aligned = copy.deepcopy(traj) # otherwise np arrays will be references and mess up stuff
with_scale = correct_scale or correct_only_scale
if correct_only_scale:
logger.debug("Correcting scale...")
else:
logger.debug("Aligning using Umeyama's method..."
+ (" (with scale correction)" if with_scale else ""))
if n == -1:
r_a, t_a, s = geometry.umeyama_alignment(traj_aligned.positions_xyz.T,
traj_ref.positions_xyz.T, with_scale)
else:
r_a, t_a, s = geometry.umeyama_alignment(traj_aligned.positions_xyz[:n, :].T,
traj_ref.positions_xyz[:n, :].T, with_scale)
if not correct_only_scale:
logger.debug("Rotation of alignment:\n{}"
"\nTranslation of alignment:\n{}".format(r_a, t_a))
logger.debug("Scale correction: {}".format(s))
if correct_only_scale:
traj_aligned.scale(s)
elif correct_scale:
traj_aligned.scale(s)
traj_aligned.transform(lie.se3(r_a, t_a))
else:
traj_aligned.transform(lie.se3(r_a, t_a))
return traj_aligned
def align_trajectory(traj,
traj_ref,
correct_scale=False,
correct_only_scale=False,
return_parameters=False,
n=-1,
discard_n_start_poses=0,
discard_n_end_poses=0):
"""
align a trajectory to a reference using Umeyama alignment
:param traj: the trajectory to align
:param traj_ref: reference trajectory
:param correct_scale: set to True to adjust also the scale
:param correct_only_scale: set to True to correct the scale, but not the pose
:param n: the number of poses to use, counted from the start (default: all)
:param return_parameters: also return result parameters of Umeyama's method
:param discard_n_start_poses: the number of poses to skip from the start (default: none)
:param discard_n_end_poses: the number of poses to discard counted from the end (default: none)
:return: the aligned trajectory
If return_parameters is set, the rotation matrix, translation vector and
scaling parameter of Umeyama's method are also returned.
"""
# otherwise np arrays will be references and mess up stuff
traj_aligned = copy.deepcopy(traj)
with_scale = correct_scale or correct_only_scale
if correct_only_scale:
logger.debug("Correcting scale...")
else:
logger.debug("Aligning using Umeyama's method..." +
(" (with scale correction)" if with_scale else ""))
end_pose_idx = len(traj_aligned.positions_xyz)
if n != -1:
if discard_n_start_poses != 0 or discard_n_end_poses != 0:
print(
"WARNING: you asked for %d poses, as well as discarding %d start poses and %d end poses.",
n, discard_n_start_poses, discard_n_end_poses)
print("Selecting option given the smallest number of poses:")
if end_pose_idx - discard_n_end_poses < n + discard_n_start_poses:
print(
"We are requiring more poses n (%d) than available, using instead %d starting from pose %d",
n,
end_pose_idx - discard_n_end_poses - discard_n_start_poses,
discard_n_start_poses)
end_pose_idx = end_pose_idx - discard_n_end_poses
else:
end_pose_idx = n + discard_n_start_poses
else:
end_pose_idx -= discard_n_end_poses
assert (discard_n_start_poses < end_pose_idx
) # Otherwise there will be no poses to align.
assert (end_pose_idx <= len(traj_aligned.positions_xyz)
) # Otherwise we will be requiring more poses than there are.
r_a, t_a, s = geometry.umeyama_alignment(
traj_aligned.positions_xyz[discard_n_start_poses:end_pose_idx, :].T,
traj_ref.positions_xyz[discard_n_start_poses:end_pose_idx, :].T,
with_scale)
if not correct_only_scale:
logger.debug("Rotation of alignment:\n{}"
"\nTranslation of alignment:\n{}".format(r_a, t_a))
logger.debug("Scale correction: {}".format(s))
if correct_only_scale:
traj_aligned.scale(s)
elif correct_scale:
traj_aligned.scale(s)
traj_aligned.transform(lie.se3(r_a, t_a))
else:
traj_aligned.transform(lie.se3(r_a, t_a))
if return_parameters:
return traj_aligned, r_a, t_a, s
else:
return traj_aligned
