rotmat_to_eulerang.py

# based on: https://www.learnopencv.com/rotation-matrix-to-euler-angles/
import numpy as np
import math

def is_rotmat(r) :
    ident = np.identity(3, dtype=r.dtype)
    r2 = np.dot(np.transpose(r), r)
    return np.linalg.norm(ident - r2) < 1e-6


def rotmat_to_eulerang(r) :
    if not is_rotmat(r):
        raise ValueError("supplied matrix is no rotation matrix")

    sy = math.sqrt(r[0,0] * r[0,0] + r[1,0] * r[1,0])
    is_singular = sy < 1e-6

    if not is_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([x, y, z])


def eulerang_to_rotmat(theta):
    rx = np.array([
        [1, 0, 0],
        [0, math.cos(theta[0]), -math.sin(theta[0])],
        [0, math.sin(theta[0]), math.cos(theta[0])]])

    ry = np.array([
        [math.cos(theta[1]), 0, math.sin(theta[1])],
        [0, 1, 0],
        [-math.sin(theta[1]), 0, math.cos(theta[1])]])

    rz = np.array([
        [math.cos(theta[2]), -math.sin(theta[2]), 0],
        [math.sin(theta[2]), math.cos(theta[2]), 0],
        [0, 0, 1]])

    return np.dot(rz, np.dot(ry, rx))

def main():
    rotmat = np.array([
        [0.9999830108, -0.0005844507985, -0.005799700295],
        [0.000448344363, 0.9997251109, -0.02344145153],
        [0.005811806396, 0.02343845301, 0.9997083884],
    ])
    eulerang = rotmat_to_eulerang(rotmat)
    print(np.degrees(eulerang))
    # [ 1.34307011 -0.33299385  0.02568867]

    # verify that conversion from angle to rotmat also works
    rotmat2 = eulerang_to_rotmat(eulerang)
    assert(np.sum(np.abs(rotmat-rotmat2)) < 1e-10)

if __name__ == "__main__":
    main()