to_euler_angles.py

def to_euler_angles(R):
    """Calculate rotation matrix to euler angles.

    The result is the same as MATLAB except the order
    of the euler angles ( x and z are swapped ).

    Args:
        R (np.ndarray): rotation matrix.
    """

    def isValid(R):
        # Checks if a matrix is a valid rotation matrix.
        Rt = np.transpose(R)
        shouldBeIdentity = np.dot(Rt, R)
        i = np.identity(3, dtype=R.dtype)
        n = np.linalg.norm(i - shouldBeIdentity)
        return n < 1e-6

    R = R[:3, :3]
    if not isValid(R):
        return np.zeros([1, 3])

    sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])

    singular = sy < 1e-6

    if not singular:
        x = math.atan2(R[2, 1], R[2, 2])
        y = math.atan2(-R[2, 0], sy)
        z = math.atan2(R[1, 0], R[0, 0])
    else:
        x = math.atan2(-R[1, 2], R[1, 1])
        y = math.atan2(-R[2, 0], sy)
        z = 0

    return np.array([np.rad2deg(x), np.rad2deg(y), np.rad2deg(z)])