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

To help you get started, we’ve selected a few evo examples, based on popular ways it is used in public projects.

Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.

github MichaelGrupp / evo / test / test_lie_algebra.py View on Github external
def test_is_sim3(self):
        r = lie.random_so3()
        t = np.array([1, 2, 3])
        s = 3
        p = lie.sim3(r, t, s)
        self.assertTrue(lie.is_sim3(p, s))
github MichaelGrupp / evo / test / test_lie_algebra.py View on Github external
def test_so3_log_exp(self):
        r = lie.random_so3()
        self.assertTrue(lie.is_so3(r))
        rotvec = lie.so3_log(r, return_angle_only=False)
        self.assertTrue(np.allclose(r, lie.so3_exp(rotvec), atol=1e-6))
        angle = lie.so3_log(r)
        self.assertAlmostEqual(np.linalg.norm(rotvec), angle)
github MichaelGrupp / evo / test / test_lie_algebra.py View on Github external
def test_so3_from_se3(self):
        p = lie.random_se3()
        r = lie.so3_from_se3(p)
        self.assertTrue(lie.is_so3(r))
github MichaelGrupp / evo / test / test_trajectory.py View on Github external
def test_transform_sim3(self):
        path = helpers.fake_path(10)
        path_transformed = copy.deepcopy(path)
        t = lie.sim3(r=lie.random_so3(), t=np.ones(3), s=1.234)
        path_transformed.transform(t)
        self.assertAlmostEqual(path_transformed.path_length,
                               path.path_length * 1.234)
github MichaelGrupp / evo / evo / core / metrics.py View on Github external
if self.pose_relation == PoseRelation.translation_part:
            self.error = [np.linalg.norm(E_i[:3, 3]) for E_i in self.E]
        elif self.pose_relation == PoseRelation.rotation_part:
            # ideal: rot(E_i) = 3x3 identity
            self.error = np.array([
                np.linalg.norm(lie.so3_from_se3(E_i) - np.eye(3))
                for E_i in self.E
            ])
        elif self.pose_relation == PoseRelation.full_transformation:
            # ideal: E_i = 4x4 identity
            self.error = np.array(
                [np.linalg.norm(E_i - np.eye(4)) for E_i in self.E])
        elif self.pose_relation == PoseRelation.rotation_angle_rad:
            self.error = np.array(
                [abs(lie.so3_log(E_i[:3, :3])) for E_i in self.E])
        elif self.pose_relation == PoseRelation.rotation_angle_deg:
            self.error = np.array([
                abs(lie.so3_log(E_i[:3, :3])) * 180 / np.pi for E_i in self.E
            ])
        else:
            raise MetricsException("unsupported pose_relation: ",
                                   self.pose_relation)
github MichaelGrupp / evo / evo / main_traj.py View on Github external
if args.merge:
        if args.subcommand == "kitti":
            die("Can't merge KITTI files.")
        if len(trajectories) == 0:
            die("No trajectories to merge (excluding --ref).")
        trajectories = {
            "merged_trajectory": trajectory.merge(trajectories.values())
        }

    if args.transform_left or args.transform_right:
        tf_type = "left" if args.transform_left else "right"
        tf_path = args.transform_left \
                if args.transform_left else args.transform_right
        transform = file_interface.load_transform_json(tf_path)
        logger.debug(SEP)
        if not lie.is_se3(transform):
            logger.warning("Not a valid SE(3) transformation!")
        if args.invert_transform:
            transform = lie.se3_inverse(transform)
        logger.debug("Applying a {}-multiplicative transformation:\n{}".format(
            tf_type, transform))
        for traj in trajectories.values():
            traj.transform(transform, right_mul=args.transform_right,
                           propagate=args.propagate_transform)

    if args.t_offset:
        logger.debug(SEP)
        for name, traj in trajectories.items():
            if type(traj) is trajectory.PosePath3D:
                die("{} doesn't have timestamps - can't add time offset.".
                    format(name))
            logger.info("Adding time offset to {}: {} (s)".format(