transformations.py

def matrix_from_euler(euler):
   rotation_x = torch.tensor([[1, 0, 0], [0, torch.cos(euler[0]), -torch.sin(euler[0])], [0, torch.sin(euler[0]), torch.cos(euler[0])]])
   rotation_y = torch.tensor([[torch.cos(euler[1]), 0, torch.sin(euler[1])], [0, 1, 0], [-torch.sin(euler[1]), 0, torch.cos(euler[1])]])
   rotation_z = torch.tensor([[torch.cos(euler[2]), -torch.sin(euler[2]), 0], [torch.sin(euler[2]), torch.cos(euler[2]), 0], [0, 0, 1]])
  
   rotation_3d = torch.matmul(torch.matmul(rotation_x, rotation_y), rotation_z)
     
   return rotation_3d


def euler_from_matrix(rotation):
     
    return torch.tensor([
        torch.atan2(-rotation[1][2], rotation[2][2]),
        torch.asin(rotation[0][2])
        torch.atan2(-rotation[0][1], rotation[0][0])])

    
def matrix_from_quaternion(quaternion):
    q = quaternion
    n = torch.dot(q, q)
    if n < torch.finfo(torch.float).eps * 4.0:
        return torch.identity(4)
    q *= torch.sqrt(2.0 / n)
    q = torch.ger(q, q)
    return torch.tensor([
        [1.0-q[2, 2]-q[3, 3],     q[1, 2]-q[3, 0],     q[1, 3]+q[2, 0]],
        [    q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3],     q[2, 3]-q[1, 0]],
        [    q[1, 3]-q[2, 0],     q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2]]])
  

def quaternion_from_matrix(matrix):
    M = matrix
    q = torch.empty(4)
    t = torch.trace(M)
    print(t)
    if t > 1:
        q[0] = t
        q[3] = M[1, 0] - M[0, 1]
        q[2] = M[0, 2] - M[2, 0]
        q[1] = M[2, 1] - M[1, 2]
    else:
        i, j, k = 0, 1, 2
        if M[1, 1] > M[0, 0]:
            i, j, k = 1, 2, 0
        if M[2, 2] > M[i, i]:
            i, j, k = 2, 0, 1
        t = M[i, i] - (M[j, j] + M[k, k]) + 1
        q[i] = t
        q[j] = M[i, j] + M[j, i]
        q[k] = M[k, i] + M[i, k]
        q[3] = M[k, j] - M[j, k]
        q = q[[3, 0, 1, 2]]
        q*= 0.5 / torch.sqrt(t)
    if q[0] < 0.0:
        torch.negative(q, q)
    return q