How to use the evo.core.lie_algebra.so3_log 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_so3_log_exp_skew(self):
        r = lie.random_so3()
        log = lie.so3_log(r, return_skew=True)  # skew-symmetric tangent space
        # here, axis is a rotation vector with norm = angle
        rotvec = lie.vee(log)
        self.assertTrue(np.allclose(r, lie.so3_exp(rotvec)))
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_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 / evo / core / trajectory.py View on Github external
def calc_angular_speed(p_1, p_2, t_1, t_2, degrees=False):
    """
    :param p_1: pose at timestamp 1
    :param p_2: pose at timestamp 2
    :param t_1: timestamp 1
    :param t_2: timestamp 2
    :param degrees: set to True to return deg/s
    :return: speed in rad/s
    """
    if (t_2 - t_1) <= 0:
        raise TrajectoryException("bad timestamps: " + str(t_1) + " & " +
                                  str(t_2))
    if degrees:
        angle_1 = lie.so3_log(p_1[:3, :3]) * 180 / np.pi
        angle_2 = lie.so3_log(p_2[:3, :3]) * 180 / np.pi
    else:
        angle_1 = lie.so3_log(p_1[:3, :3])
        angle_2 = lie.so3_log(p_2[:3, :3])
    return (angle_2 - angle_1) / (t_2 - t_1)
github MichaelGrupp / evo / evo / core / filters.py View on Github external
ids = range(len(poses))
        if degrees:
            angles = [lie.so3_log(p[:3, :3]) * 180 / np.pi for p in poses]
        else:
            angles = [lie.so3_log(p[:3, :3]) for p in poses]
        for i in ids:
            for j in ids[i + 1:]:
                current_angle = abs(angles[i] - angles[j])
                if lower_bound <= current_angle <= upper_bound:
                    id_pairs.append((i, j))
    else:
        ids = []
        if degrees:
            angles = [lie.so3_log(p[:3, :3]) * 180 / np.pi for p in poses]
        else:
            angles = [lie.so3_log(p[:3, :3]) for p in poses]
        previous_angle = angles[0]
        current_delta = 0.0
        ids.append(0)
        for i, current_angle in enumerate(angles):
            current_delta += abs(current_angle - previous_angle)
            previous_angle = current_angle
            if current_delta >= delta:
                ids.append(i)
                current_delta = 0.0
        id_pairs = [(i, j) for i, j in zip(ids, ids[1:])]
    return id_pairs
github MichaelGrupp / evo / evo / core / trajectory.py View on Github external
:param p_1: pose at timestamp 1
    :param p_2: pose at timestamp 2
    :param t_1: timestamp 1
    :param t_2: timestamp 2
    :param degrees: set to True to return deg/s
    :return: speed in rad/s
    """
    if (t_2 - t_1) <= 0:
        raise TrajectoryException("bad timestamps: " + str(t_1) + " & " +
                                  str(t_2))
    if degrees:
        angle_1 = lie.so3_log(p_1[:3, :3]) * 180 / np.pi
        angle_2 = lie.so3_log(p_2[:3, :3]) * 180 / np.pi
    else:
        angle_1 = lie.so3_log(p_1[:3, :3])
        angle_2 = lie.so3_log(p_2[:3, :3])
    return (angle_2 - angle_1) / (t_2 - t_1)
github MichaelGrupp / evo / evo / core / metrics.py View on Github external
# E is an array of position vectors only in this case
            self.error = [np.linalg.norm(E_i) for E_i in self.E]
        elif self.pose_relation == PoseRelation.rotation_part:
            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:
            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")
github MichaelGrupp / evo / evo / core / filters.py View on Github external
poses is considered
    :param poses: list of SE(3) poses
    :param delta: the angle in radians used for filtering
    :param tol: absolute angle tolerance to accept or reject pairs
                in all_pairs mode
    :param degrees: set to True if  is in degrees instead of radians
    :param all_pairs: use all pairs instead of consecutive pairs
    :return: list of index tuples of the filtered pairs
    """
    if all_pairs:
        upper_bound = delta + tol
        lower_bound = delta - tol
        id_pairs = []
        ids = range(len(poses))
        if degrees:
            angles = [lie.so3_log(p[:3, :3]) * 180 / np.pi for p in poses]
        else:
            angles = [lie.so3_log(p[:3, :3]) for p in poses]
        for i in ids:
            for j in ids[i + 1:]:
                current_angle = abs(angles[i] - angles[j])
                if lower_bound <= current_angle <= upper_bound:
                    id_pairs.append((i, j))
    else:
        ids = []
        if degrees:
            angles = [lie.so3_log(p[:3, :3]) * 180 / np.pi for p in poses]
        else:
            angles = [lie.so3_log(p[:3, :3]) for p in poses]
        previous_angle = angles[0]
        current_delta = 0.0
        ids.append(0)
github MichaelGrupp / evo / evo / core / metrics.py View on Github external
(self.pose_relation.value)))

        if self.pose_relation == PoseRelation.translation_part:
            # E is an array of position vectors only in this case
            self.error = [np.linalg.norm(E_i) for E_i in self.E]
        elif self.pose_relation == PoseRelation.rotation_part:
            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:
            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")