How to use the evo.core.trajectory.align_trajectory function in evo

def test_alignment_degenerate_case(self):
        length = 100
        poses = [lie.random_se3()] * length
        traj_1 = PoseTrajectory3D(
            poses_se3=poses,
            timestamps=helpers.fake_timestamps(length, 1, 0.0))
        traj_2 = copy.deepcopy(traj_1)
        traj_2.transform(lie.random_se3())
        traj_2.scale(1.234)
        self.assertNotEqual(traj_1, traj_2)

        with self.assertRaises(GeometryException):
            trajectory.align_trajectory(traj_1, traj_2)

        with self.assertRaises(GeometryException):
            trajectory.align_trajectory(traj_1, traj_2, correct_scale=True)

def test_sim3_alignment(self):
        traj = helpers.fake_trajectory(1000, 1)
        traj_transformed = copy.deepcopy(traj)
        traj_transformed.transform(lie.random_se3())
        traj_transformed.scale(1.234)
        self.assertNotEqual(traj, traj_transformed)
        traj_aligned = trajectory.align_trajectory(traj_transformed, traj,
                                                   correct_scale=True)
        self.assertEqual(traj_aligned, traj)

def test_alignment_degenerate_case(self):
        length = 100
        poses = [lie.random_se3()] * length
        traj_1 = PoseTrajectory3D(
            poses_se3=poses,
            timestamps=helpers.fake_timestamps(length, 1, 0.0))
        traj_2 = copy.deepcopy(traj_1)
        traj_2.transform(lie.random_se3())
        traj_2.scale(1.234)
        self.assertNotEqual(traj_1, traj_2)

        with self.assertRaises(GeometryException):
            trajectory.align_trajectory(traj_1, traj_2)

        with self.assertRaises(GeometryException):
            trajectory.align_trajectory(traj_1, traj_2, correct_scale=True)

from __future__ import print_function

print("loading required evo modules")
from evo.core import trajectory, sync, metrics
from evo.tools import file_interface

print("loading trajectories")
traj_ref = file_interface.read_tum_trajectory_file(
    "../../test/data/fr2_desk_groundtruth.txt")
traj_est = file_interface.read_tum_trajectory_file(
    "../../test/data/fr2_desk_ORB.txt")

print("registering and aligning trajectories")
traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est)
traj_est = trajectory.align_trajectory(traj_est, traj_ref, correct_scale=False)

print("calculating APE")
data = (traj_ref, traj_est)
ape_metric = metrics.APE(metrics.PoseRelation.translation_part)
ape_metric.process_data(data)
ape_statistics = ape_metric.get_all_statistics()
print("mean:", ape_statistics["mean"])

print("loading plot modules")
from evo.tools import plot
import matplotlib.pyplot as plt

print("plotting")
plot_collection = plot.PlotCollection("Example")
# metric values
fig_1 = plt.figure(figsize=(8, 8))

from evo.core import sync
        if not args.ref:
            logger.debug(SEP)
            die("Can't align or sync without a reference! (--ref)  *grunt*")
        for name, traj in trajectories.items():
            if args.subcommand == "kitti":
                ref_traj_tmp = ref_traj
            else:
                logger.debug(SEP)
                ref_traj_tmp, trajectories[name] = sync.associate_trajectories(
                    ref_traj, traj, max_diff=args.t_max_diff,
                    first_name="reference", snd_name=name)
            if args.align or args.correct_scale:
                logger.debug(SEP)
                logger.debug("Aligning {} to reference.".format(name))
                trajectories[name] = trajectory.align_trajectory(
                    trajectories[name], ref_traj_tmp,
                    correct_scale=args.correct_scale,
                    correct_only_scale=args.correct_scale and not args.align,
                    n=args.n_to_align)
            if args.align_origin:
                logger.debug(SEP)
                logger.debug("Aligning {}'s origin to reference.".format(name))
                trajectories[name] = trajectory.align_trajectory_origin(
                    trajectories[name], ref_traj_tmp)
            if SETTINGS.plot_pose_correspondences:
                synced_refs[name] = ref_traj_tmp

    print_compact_name = not args.subcommand == "bag"
    for name, traj in trajectories.items():
        print_traj_info(name, traj, args.verbose, args.full_check,
                        print_compact_name)

if pose_relation is metrics.PoseRelation.translation_part:
        rpe_unit = metrics.Unit.meters
    elif pose_relation is metrics.PoseRelation.rotation_angle_deg:
        rpe_unit = metrics.Unit.degrees
    elif pose_relation is metrics.PoseRelation.rotation_angle_rad:
        rpe_unit = metrics.Unit.radians

    correct_only_scale = correct_scale and not align
    if align or correct_scale:
        logger.debug(SEP)
        if correct_only_scale:
            logger.debug("correcting scale...")
        else:
            logger.debug("aligning using Umeyama's method..."
                          + (" (with scale correction)" if correct_scale else ""))
        traj_est = trajectory.align_trajectory(traj_est, traj_ref, correct_scale,
                                               correct_only_scale)

    results = []
    for bin, rel_tol, in zip(bins, rel_tols):
        logger.debug(SEP)
        logger.info(
            "calculating RPE for each sub-sequence of " + str(bin) + " (" + bin_unit.value + ")")

        tol = bin * rel_tol
        id_pairs = []
        if mode == "path":
            id_pairs = filters.filter_pairs_by_path(traj_ref.poses_se3, bin, tol, all_pairs=True)
        elif mode == "angle":
            id_pairs = filters.filter_pairs_by_angle(traj_ref.poses_se3, bin, tol, degrees=True)
        elif mode == "speed":
            id_pairs = filters.filter_pairs_by_speed(traj_ref.poses_se3, traj_ref.timestamps,

def ape(traj_ref, traj_est, pose_relation, align=False, correct_scale=False,
        align_origin=False, ref_name="reference", est_name="estimate"):
    from evo.core import metrics
    from evo.core import trajectory

    # Align the trajectories.
    only_scale = correct_scale and not align
    if align or correct_scale:
        logger.debug(SEP)
        traj_est = trajectory.align_trajectory(traj_est, traj_ref,
                                               correct_scale, only_scale)
    elif align_origin:
        logger.debug(SEP)
        traj_est = trajectory.align_trajectory_origin(traj_est, traj_ref)

    # 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)"