rotation matrix to euler angles python cv2

1# Checks if a matrix is a valid rotation matrix.
2def isRotationMatrix(R) :
3    Rt = np.transpose(R)
4    shouldBeIdentity = np.dot(Rt, R)
5    I = np.identity(3, dtype = R.dtype)
6    n = np.linalg.norm(I - shouldBeIdentity)
7    return n < 1e-6
8
9# Calculates rotation matrix to euler angles
10# The result is the same as MATLAB except the order
11# of the euler angles ( x and z are swapped ).
12def rotationMatrixToEulerAngles(R) :
13    assert(isRotationMatrix(R))
14    sy = math.sqrt(R[0,0] * R[0,0] +  R[1,0] * R[1,0])
15    singular = sy < 1e-6
16    if  not singular :
17        x = math.atan2(R[2,1] , R[2,2])
18        y = math.atan2(-R[2,0], sy)
19        z = math.atan2(R[1,0], R[0,0])
20    else :
21        x = math.atan2(-R[1,2], R[1,1])
22        y = math.atan2(-R[2,0], sy)
23        z = 0
24    return np.array([x, y, z])
25
26