transforms.py 文件源码

def convert_to_euler(R):
    """Compute the euler angles of this rotation.
    Refer to [http://www.staff.city.ac.uk/~sbbh653/publications/euler.pdf]"""
    alpha, beta, gamma = 0, 0, 0
    if not np.isclose(np.abs(R[2,0]), 1):
        beta = - np.arcsin(R[2,0])
        alpha = np.arctan2(R[2,1] / np.cos(beta), R[2,2] / np.cos(beta))
        gamma = np.arctan2(R[1,0] / np.cos(beta), R[0,0] / np.cos(beta))
    else:
        gamma = 0
        if np.isclose(R[2,0], -1):
            beta = np.pi / 2
            alpha = gamma + np.arctan2(R[0,1], R[0,2])
        else:
            beta = - np.pi / 2
            alpha = - gamma + np.arctan2(-R[0,1], -R[0,2])
    return np.array([alpha, beta, gamma])