Python rotation_from_matrix示例

示例#1

文件： lie_algebra.py 项目： danpeng2/evo

def so3_log(r, return_angle_only=True, return_skew=False):
    """
    :param r: SO(3) rotation matrix
    :param return_angle_only: return only the angle (default)
    :param return_skew: return skew symmetric Lie algebra element
    :return: axis/angle
        or if skew:
             3x3 skew symmetric logarithmic map in so(3) (Ma, Soatto eq. 2.8)
    """
    if not is_so3(r):
        raise LieAlgebraException("matrix is not a valid SO(3) group element")
    if return_angle_only and not return_skew:
        return np.arccos(min(1, max(-1, (np.trace(r) - 1)/2)))
    angle, axis, _ = tr.rotation_from_matrix(se3(r, [0, 0, 0]))
    if return_skew:
        return hat(axis*angle)
    else:
        return axis, angle

示例#2

def so3_log(r, return_angle_only=True, return_skew=False):
    """
    :param r: SO(3) rotation matrix
    :param return_angle_only: return only the angle (default)
    :param return_skew: return skew symmetric Lie algebra element
    :return: axis/angle
        or if skew:
             3x3 skew symmetric logarithmic map in so(3) (Ma, Soatto eq. 2.8)
    """
    if not is_so3(r):
        raise LieAlgebraException("matrix is not a valid SO(3) group element")
    if return_angle_only and not return_skew:
        return np.arccos(min(1, max(-1, (np.trace(r) - 1) / 2)))
    angle, axis, _ = tr.rotation_from_matrix(se3(r, [0, 0, 0]))
    if return_skew:
        return hat(axis * angle)
    else:
        return axis, angle