forked from chrisadamsonmcri/CCSegThickness
-
Notifications
You must be signed in to change notification settings - Fork 0
/
FLIRT.py
103 lines (96 loc) · 2.99 KB
/
FLIRT.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
import numpy
#function [rotmat, skew, scales, transl, angles] = fsl_decomp_aff(A)
def fsl_decomp_aff(A):
# aff3 = A(1:3, 1:3);
aff3 = A.take([0, 1, 2], axis=0).take([0, 1, 2], axis=1)
# x = A(1:3, 1);
x = A[0:3, 0]
# y = A(1:3, 2);
y = A[0:3, 1]
# z = A(1:3, 3);
z = A[0:3, 2]
# sx = sqrt(sum(x .* x));
sx = numpy.sqrt(numpy.sum(numpy.multiply(x, x)))
# % sy = std::sqrt( dot(y,y) - (Sqr(dot(x,y)) / Sqr(sx)) );
# DXY = dot(x, y);
DXY = numpy.sum(numpy.multiply(x, y))
# sy = sqrt(dot(y, y) - (DXY * DXY / (sx * sx)));
sy = numpy.sqrt(numpy.sum(numpy.multiply(y, y)) - (DXY * DXY / (sx * sx)))
# a = dot(x, y) / (sx * sy);
a = numpy.sum(numpy.multiply(x, y)) / (sx * sy);
x0 = x / sx;
y0 = y / sy - a * x0;
# % sz = std::sqrt(dot(z,z) - Sqr(dot(x0,z)) - Sqr(dot(y0,z)));
Dx0z = numpy.sum(numpy.multiply(x0, z));
Dy0z = numpy.sum(numpy.multiply(y0, z));
# sz = sqrt(dot(z, z) - Dx0z * Dx0z - Dy0z * Dy0z);
sz = numpy.sqrt(numpy.sum(numpy.multiply(z, z)) - Dx0z * Dx0z - Dy0z * Dy0z);
#
# b = dot(x0, z) / sz;
b = numpy.sum(numpy.multiply(x0, z)) / sz;
# c = dot(y0, z) / sz;
c = numpy.sum(numpy.multiply(y0, z)) / sz;
# % params(7) = sx; params(8) = sy; params(9) = sz;
# scales = diag([sx, sy, sz]);
scales = numpy.matrix(numpy.diag([sx, sy, sz]))
# % diag(scales,diagvals);
# % Real skewvals[] = {1,a,b,0 , 0,1,c,0 , 0,0,1,0 , 0,0,0,1};
# skew = [1, a, b, 0; 0, 1, c, 0; 0, 0, 1, 0; 0, 0, 0, 1];
skew = numpy.matrix(numpy.vstack([[1, a, b, 0], [0, 1, c, 0], [0, 0, 1, 0], [0, 0, 0, 1]]))
# % params(10) = a; params(11) = b; params(12) = c;
# % Matrix rotmat(3,3);
rotmat = numpy.matrix(numpy.zeros((3, 3)))
# rotmat = aff3 * inv(scales) * inv(skew(1:3, 1:3));
rotmat = aff3 * scales.I * skew[0:3, 0:3].I
# transl = A(1:3, 4);
transl = A[0:3, 3]
# print "rotmat"
# print rotmat
# print "transl"
# print transl
# % ColumnVector transl(3);
# % transl = affmat.SubMatrix(1,3,1,3)*centre + affmat.SubMatrix(1,3,4,4)
# % - centre;
#
# angles = zeros(1, 3);
angles = numpy.zeros((3, 1))
#
# % from miscmaths/miscmaths.cc
# % construct euler angles
# cy = sqrt(rotmat(1, 1) * rotmat(1, 1) + rotmat(1, 2) * rotmat(1, 2));
cy = numpy.sqrt(rotmat[0, 0] * rotmat[0, 0] + rotmat[0, 1] * rotmat[0, 1])
# if cy < 1e-4
if cy < 1e-4:
# cx = rotmat(2,2);
cx = rotmat[1, 1]
# sx = -rotmat(3,2);
sx = -rotmat[2, 1]
# sy = -rotmat(1,3);
sy = -rotmat[0, 2]
# angles(1) = atan2(sx, cx);
angles[0] = numpy.arctan2(sx, cx)
# angles(2) = atan2(sy, 0);
angles[1] = numpy.arctan2(sy, 0)
# angles(3) = 0.0;
else:
# cz = rotmat(1,1)/cy;
cz = rotmat[0, 0] / cy
# sz = rotmat(1,2)/cy;
sz = rotmat[0, 1] / cy
# cx = rotmat(3,3)/cy;
cx = rotmat[2, 2] / cy
# sx = rotmat(2,3)/cy;
sx = rotmat[1, 2] / cy
# sy = -rotmat(1,3);
sy = -rotmat[0, 2]
# angles(1) = atan2(sx,cx);
angles[0] = numpy.arctan2(sx,cx)
# angles(2) = atan2(sy,cy);
angles[1] = numpy.arctan2(sy,cy)
# angles(3) = atan2(sz,cz);
angles[2] = numpy.arctan2(sz,cz)
# end
# print "angles"
# print angles
return (rotmat, skew, scales, transl, angles)
#end