How to use the evo.core.lie_algebra.se3 function in evo

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]))
]

poses_4 = [
    lie.se3(np.eye(3), np.array([0, 0, 0])),
    lie.se3(np.eye(3), np.array([0, 0, 1])),
    lie.se3(np.eye(3), np.array([0, 0, 1])),
    lie.se3(np.eye(3), np.array([0, 0, 1]))
]


class TestFilterPairsByPath(unittest.TestCase):
    def test_poses1_all_pairs(self):
        target_path = 1.0
        tol = 0.0
        id_pairs = filters.filter_pairs_by_path(poses_1, target_path, tol,
                                                all_pairs=True)
        self.assertEqual(id_pairs, [(0, 2), (2, 3)])

    def test_poses1_wrong_target(self):
        target_path = 2.5
        tol = 0.0
        id_pairs = filters.filter_pairs_by_path(poses_1, target_path, tol,

all_pairs=True)
        self.assertEqual(id_pairs, [(0, 7)])


axis = np.array([1, 0, 0])
poses_5 = [
    lie.se3(lie.so3_exp(axis * 0.0), np.array([0, 0, 0])),
    lie.se3(lie.so3_exp(axis * math.pi), np.array([0, 0, 0])),
    lie.se3(lie.so3_exp(axis * 0.0), np.array([0, 0, 0])),
    lie.se3(lie.so3_exp(axis * math.pi / 3), np.array([0, 0, 0])),
    lie.se3(lie.so3_exp(axis * math.pi), np.array([0, 0, 0]))
]
stamps_5 = np.array([0, 1, 2, 3, 4])

axis = np.array([1, 0, 0])
p0 = lie.se3(lie.so3_exp(axis * 0.0), np.array([0, 0, 0]))
pd = lie.se3(lie.so3_exp(axis * (math.pi / 3.)), np.array([1, 2, 3]))
p1 = np.dot(p0, pd)
p2 = np.dot(p1, pd)
p3 = np.dot(p2, pd)
poses_6 = [p0, p1, p2, p3, p3]
stamps_6 = np.array([0, 1, 2, 3, 4])


class TestFilterPairsByAngle(unittest.TestCase):
    def test_poses5(self):
        tol = 0.001
        target_angle = math.pi - tol
        id_pairs = filters.filter_pairs_by_angle(poses_5, target_angle, tol,
                                                 all_pairs=False)
        self.assertEqual(id_pairs, [(0, 1), (1, 2), (2, 4)])

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

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

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

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