def se3_inverse(p):
"""
:param p: absolute SE(3) pose
:return: the inverted pose
"""
r_inv = p[:3, :3].transpose()
t_inv = -r_inv.dot(p[:3, 3])
return se3(r_inv, t_inv)
def scale(self, s):
"""
apply a scaling to the whole path
:param s: scale factor
"""
if hasattr(self, "_poses_se3"):
self._poses_se3 = [lie.se3(p[:3, :3], s * p[:3, 3]) for p in self._poses_se3]
if hasattr(self, "_positions_xyz"):
self._positions_xyz = s * self._positions_xyz
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 xyz_quat_wxyz_to_se3_poses(xyz, quat):
poses = [lie.se3(lie.so3_from_se3(tr.quaternion_matrix(quat)), xyz)
for quat, xyz in zip(quat, xyz)]
return poses
def to_transform_mtx(pose):
trans = pose[0:3]
quat = pose[3:7]
rot = quat2mat(quat)
return se3(rot, trans)