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