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 xyz_quat_wxyz_to_se3_poses( xyz: np.ndarray, quat: np.ndarray) -> typing.Sequence[np.ndarray]: poses = [ lie.se3(lie.so3_from_se3(tr.quaternion_matrix(quat)), xyz) for quat, xyz in zip(quat, xyz) ] return poses
def convert_rel_traj_to_abs_traj(traj): """ Converts a relative pose trajectory to an absolute-pose trajectory. The incoming trajectory is processed elemente-wise. Poses at each timestamp are appended to the absolute pose from the previous timestamp. Args: traj: A PoseTrajectory3D object with timestamps as indices containing, at a minimum, columns representing the xyz position and wxyz quaternion-rotation at each timestamp, corresponding to the pose between previous and current timestamps. Returns: A PoseTrajectory3D object with xyz position and wxyz quaternion fields for the relative pose trajectory corresponding to the relative one given in `traj`. """ from evo.core import lie_algebra as lie new_poses = [lie.se3()] # origin at identity for i in range(0, len(traj.timestamps)): abs_pose = np.dot(new_poses[-1], traj.poses_se3[i]) new_poses.append(abs_pose) return trajectory.PoseTrajectory3D(timestamps=traj.timestamps[1:], poses_se3=new_poses)
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 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 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(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 load_transform_json(json_path): """ load a transformation stored in xyz + quaternion format in a .json file :param json_path: path to the .json file :return: t (SE(3) matrix), xyz (position), quat (orientation quaternion) """ with open(json_path, 'r') as tf_file: data = json.load(tf_file) keys = ("x", "y", "z", "qx", "qy", "qz", "qw") if not all(key in data for key in keys): raise FileInterfaceException( "invalid transform file - expected keys " + str(keys)) xyz = np.array([data["x"], data["y"], data["z"]]) quat = np.array([data["qw"], data["qx"], data["qy"], data["qz"]]) t = lie.se3(lie.so3_from_se3(tr.quaternion_matrix(quat)), xyz) return t, xyz, quat
You should have received a copy of the GNU General Public License along with evo. If not, see <http://www.gnu.org/licenses/>. """ import math import unittest import numpy as np import context from evo.core import filters from evo.core import lie_algebra as lie # some synthetic poses for testing # [0] [1] poses_1 = [lie.se3(np.eye(3), np.array([0, 0, 0])), lie.se3(np.eye(3), np.array([0, 0, 0.5])), lie.se3(np.eye(3), np.array([0, 0, 0])), lie.se3(np.eye(3), np.array([0, 0, 1]))] # [2] [3] # [0] [1] poses_2 = [lie.se3(np.eye(3), np.array([0, 0, 0])), lie.se3(np.eye(3), np.array([0, 0, 0.5])), lie.se3(np.eye(3), np.array([0, 0, 0.99])), lie.se3(np.eye(3), np.array([0, 0, 1.0]))] # [2] [3] # [0] [1] poses_3 = [lie.se3(np.eye(3), np.array([0, 0, 0.0])), lie.se3(np.eye(3), np.array([0, 0, 0.9])), lie.se3(np.eye(3), np.array([0, 0, 0.99])), lie.se3(np.eye(3), np.array([0, 0, 0.999])), lie.se3(np.eye(3), np.array([0, 0, 0.9999])), lie.se3(np.eye(3), np.array([0, 0, 0.99999])), lie.se3(np.eye(3), np.array([0, 0, 0.999999])), lie.se3(np.eye(3), np.array([0, 0, 0.9999999]))] # [6] [7]
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
import evo.core.lie_algebra as lie from evo.core import trajectory from evo.tools import plot, file_interface, log import numpy as np import matplotlib.pyplot as plt logger = logging.getLogger("evo") log.configure_logging(verbose=True) traj_ref = file_interface.read_kitti_poses_file("../test/data/KITTI_00_gt.txt") traj_est = file_interface.read_kitti_poses_file( "../test/data/KITTI_00_ORB.txt") # add artificial Sim(3) transformation traj_est.transform(lie.se3(np.eye(3), np.array([0, 0, 0]))) traj_est.scale(0.5) logger.info("\nUmeyama alignment without scaling") traj_est_aligned = copy.deepcopy(traj_est) traj_est_aligned.align(traj_ref) logger.info("\nUmeyama alignment with scaling") traj_est_aligned_scaled = copy.deepcopy(traj_est) traj_est_aligned_scaled.align(traj_ref, correct_scale=True) logger.info("\nUmeyama alignment with scaling only") traj_est_aligned_only_scaled = copy.deepcopy(traj_est) traj_est_aligned_only_scaled.align(traj_ref, correct_only_scale=True) fig = plt.figure(figsize=(8, 8))
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
GNU General Public License for more details. You should have received a copy of the GNU General Public License along with evo. If not, see <http://www.gnu.org/licenses/>. """ import math import unittest import numpy as np from evo.core import filters from evo.core import lie_algebra as lie poses_1 = [ lie.se3(np.eye(3), np.array([0, 0, 0])), lie.se3(np.eye(3), np.array([0, 0, 0.5])), lie.se3(np.eye(3), np.array([0, 0, 0])), lie.se3(np.eye(3), np.array([0, 0, 1])) ] poses_2 = [ lie.se3(np.eye(3), np.array([0, 0, 0])), lie.se3(np.eye(3), np.array([0, 0, 0.5])), lie.se3(np.eye(3), np.array([0, 0, 0.99])), lie.se3(np.eye(3), np.array([0, 0, 1.0])) ] poses_3 = [ lie.se3(np.eye(3), np.array([0, 0, 0.0])), lie.se3(np.eye(3), np.array([0, 0, 0.9])),
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
import evo.core.lie_algebra as lie from evo.core import trajectory from evo.tools import plot, file_interface, log import numpy as np import matplotlib.pyplot as plt logger = logging.getLogger("evo") log.configure_logging(verbose=True) traj_ref = file_interface.read_kitti_poses_file("../test/data/KITTI_00_gt.txt") traj_est = file_interface.read_kitti_poses_file("../test/data/KITTI_00_ORB.txt") # add artificial Sim(3) transformation traj_est.transform(lie.se3(np.eye(3), [0, 0, 0])) traj_est.scale(0.5) logger.info("\nUmeyama alignment without scaling") traj_est_aligned = trajectory.align_trajectory(traj_est, traj_ref) logger.info("\nUmeyama alignment with scaling") traj_est_aligned_scaled = trajectory.align_trajectory( traj_est, traj_ref, correct_scale=True) logger.info("\nUmeyama alignment with scaling only") traj_est_aligned_only_scaled = trajectory.align_trajectory( traj_est, traj_ref, correct_only_scale=True) fig = plt.figure(figsize=(8, 8)) plot_mode = plot.PlotMode.xz
from evo.tools import plot, file_interface import evo.core.lie_algebra as lie import numpy as np import matplotlib.pyplot as plt logging.basicConfig(format="%(message)s", stream=sys.stdout, level=logging.DEBUG) traj_ref = file_interface.read_kitti_poses_file("../data/KITTI_00_gt.txt") traj_est = file_interface.read_kitti_poses_file("../data/KITTI_00_ORB.txt") # add artificial Sim(3) transformation traj_est.transform(lie.se3(np.eye(3), [0, 0, 0])) traj_est.scale(0.5) logging.info("\nUmeyama alignment without scaling") traj_est_aligned = trajectory.align_trajectory(traj_est, traj_ref) logging.info("\nUmeyama alignment with scaling") traj_est_aligned_scaled = trajectory.align_trajectory(traj_est, traj_ref, correct_scale=True) logging.info("\nUmeyama alignment with scaling only") traj_est_aligned_only_scaled = trajectory.align_trajectory( traj_est, traj_ref, correct_only_scale=True) fig = plt.figure(figsize=(8, 8)) # fig.suptitle("Umeyama $\mathrm{Sim}(3)$ alignment") plot_mode = plot.PlotMode.xz