forked from big-data-lab-team/fuzzy-linreg
-
Notifications
You must be signed in to change notification settings - Fork 0
/
transfo_utils.py
153 lines (132 loc) · 6.2 KB
/
transfo_utils.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
# Author: Tristan GLATARD
import numpy
import math
from math import cos, sin
## Conversions between transformation formats
def get_rot_angle_vec(rot_mat):
# See http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/
cos = (rot_mat.item(0,0)+rot_mat.item(1,1)+rot_mat.item(2,2)-1)/2
if(cos > 1):
print("Warning: cos is larger than 1: {0}".format(cos))
cos = 1
rot_angle = math.acos(cos)
x = (rot_mat.item(2,1)-rot_mat.item(1,2))/math.sqrt((rot_mat.item(2,1)-rot_mat.item(1,2))**2+(rot_mat.item(0,2)-rot_mat.item(2,0))**2 + (rot_mat.item(1,0)-rot_mat.item(0,1))**2)
y = (rot_mat.item(0,2)-rot_mat.item(2,0))/math.sqrt((rot_mat.item(2,1)-rot_mat.item(1,2))**2+(rot_mat.item(0,2)-rot_mat.item(2,0))**2 + (rot_mat.item(1,0)-rot_mat.item(0,1))**2)
z = (rot_mat.item(1,0)-rot_mat.item(0,1))/math.sqrt((rot_mat.item(2,1)-rot_mat.item(1,2))**2+(rot_mat.item(0,2)-rot_mat.item(2,0))**2 + (rot_mat.item(1,0)-rot_mat.item(0,1))**2)
rot_vec = numpy.array([x, y, z])
return rot_angle, rot_vec
def get_rot_mat(transfo_mat):
rot_mat = numpy.matrix([[transfo_mat.item(0,0), transfo_mat.item(0,1), transfo_mat.item(0,2)],
[transfo_mat.item(1,0), transfo_mat.item(1,1), transfo_mat.item(1,2)],
[transfo_mat.item(2,0), transfo_mat.item(2,1), transfo_mat.item(2,2)]])
return rot_mat
def get_tr_vec(transfo_mat):
tr_vec = numpy.array([transfo_mat.item(0,3), transfo_mat.item(1,3), transfo_mat.item(2,3)])
return tr_vec
def get_transfo_vector(transfo_mat):
tx, ty, tz = get_tr_vec(transfo_mat)
rx, ry, rz = get_euler_angles(transfo_mat)
return [tx, ty, tz, rx, ry, rz]
def get_euler_angles(transfo_mat):
# From http://nghiaho.com/?page_id=846
rx = math.atan2(transfo_mat.item(2,1), transfo_mat.item(2,2))
ry = math.atan2(-transfo_mat.item(2,0),
math.sqrt(transfo_mat.item(2,1)**2 + transfo_mat.item(2,2)**2)
)
rz = math.atan2(transfo_mat.item(1,0), transfo_mat.item(0,0))
return rx, ry, rz
def get_transfo_mat(x):
tx, ty, tz, rx, ry, rz = x
x = numpy.matrix([[1, 0, 0],
[0, cos(rx), -sin(rx)],
[0, sin(rx), cos(rx)]])
y = numpy.matrix([[cos(ry), 0, sin(ry)],
[0, 1, 0],
[-sin(ry), 0, cos(ry)]])
z = numpy.matrix([[cos(rz), -sin(rz), 0],
[sin(rz), cos(rz), 0],
[0, 0, 1]])
r = x*y*z
mat = numpy.matrix([[ r.item(0,0) , r.item(0,1) , r.item(0,2) , tx],
[ r.item(1,0) , r.item(1,1) , r.item(1,2) , ty],
[ r.item(2,0) , r.item(2,1) , r.item(2,2) , tz],
[ 0 , 0 , 0 , 1]])
return mat
# Reads an MNI xfm transformation
def read_transfo(file_name):
# Error message
def handle_read_error(err):
raise ValueError(f"Wrong format in transformation matrix: {file_name}. Should be 4*4, space-separated values.") from err
# Read & Check
with open(file_name) as f:
try:
transfo_mat = numpy.matrix([l.strip().split() for l in f.readlines()]).astype(float)
if transfo_mat.shape != (4, 4):
handle_read_error()
except Exception as err:
transfo_mat = numpy.matrix([])
handle_read_error(err)
# Return
return transfo_mat
# Reads an MNI xfm affine transformation and returns the equivalent of a rigid transformation matrix
def read_rigid_transfo(file_name):
# Error message
def handle_read_error(err):
raise ValueError(f"Wrong format in transformation matrix: {file_name}. Should be 4*4, space-separated values.") from err
# Read & Check
with open(file_name) as f:
try:
transfo_mat = numpy.matrix([l.strip().split() for l in f.readlines()]).astype(float)
if transfo_mat.shape != (4, 4):
handle_read_error()
except Exception as err:
transfo_mat = numpy.matrix([])
handle_read_error(err)
# Extract the top-left 3x3 sub-matrix (rotation with potential scaling/shearing)
rotation_matrix = transfo_mat[:3, :3]
# Use SVD to isolate pure rotation (remove scaling/shearing)
u, _, vh = numpy.linalg.svd(rotation_matrix)
pure_rotation_matrix = numpy.dot(u, vh)
# Ensure it's a proper rotation matrix (handling potential reflection issue)
if numpy.linalg.det(pure_rotation_matrix) < 0:
u[:, -1] *= -1
pure_rotation_matrix = numpy.dot(u, vh)
# Update the transformation matrix with pure_rotation_matrix
transfo_mat[:3, :3] = pure_rotation_matrix
# Return
return transfo_mat
# OLD VERSION
# # Reads an MNI xfm transformation
# def read_transfo(file_name):
# transfo = numpy.zeros(shape=(4, 4))
# transfo_line = -1
# with open(file_name) as f:
# for line in f:
# line = line.strip()
# if transfo_line >= 0 and transfo_line < 3:
# elements = line.split()
# assert(len(elements) == 4), "Wrong format in transformation line: {0} (file name: {1})".format(line, file_name)
# for i in range(0, 4):
# transfo[transfo_line][i] = float(elements[i].strip().replace(";",""))
# transfo_line += 1
# if transfo_line >= 4:
# break
# if line.startswith("Linear_Transform"):
# transfo_line = 0
# transfo_mat = numpy.matrix([[transfo[0][0], transfo[0][1], transfo[0][2], transfo[0][3]],
# [transfo[1][0], transfo[1][1], transfo[1][2], transfo[1][3]],
# [transfo[2][0], transfo[2][1], transfo[2][2], transfo[2][3]],
# [0, 0, 0, 1]
# ])
# return transfo_mat
# Prints an MNI xfm transformation
def print_mat(x, file_name):
s = "{0} {1} {2} {3}\n{4} {5} {6} {7}\n{8} {9} {10} {11}\n".format(
x.item(0,0), x.item(0,1), x.item(0,2), x.item(0,3),
x.item(1,0), x.item(1,1), x.item(1,2), x.item(1,3),
x.item(2,0), x.item(2,1), x.item(2,2), x.item(2,3))
with open(file_name, 'w') as f:
f.write("MNI Transform File\n")
f.write("Transform_Type = Linear;\nLinear_Transform =\n")
f.write(s)
f.write(";")