Mercurial > hg > orthanc-stone

annotate Resources/Computations/ComputeWarp.py @ 473:628941d63b8c bgo-commands-codegen

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Ongoing work. Parsing tests work
author bgo-osimis
date Wed, 13 Feb 2019 12:07:00 +0100
parents 4abddd083374
children
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
193
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
1 #!/usr/bin/python
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
2
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
3 from sympy import *
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
4 from sympy.solvers import solve
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
5 import pprint
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
6 import sys
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
7
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
8 init_printing(use_unicode=True)
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
9
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
10
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
11 # Create a test 3D vector using homogeneous coordinates
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
12 x, y, z, w = symbols('x y z w')
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
13 p = Matrix([ x, y, z, w ])
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
14
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
15
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
16 # Create a shear matrix, and a scale/shift "T * S" transform as in
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
17 # Lacroute's thesis (Equation A.16, page 209)
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
18 ex, ey, ew = symbols('ex ey ew')
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
19 sx, sy, tx, ty = symbols('sx sy tx ty')
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
20
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
21 TS = Matrix([[ sx, 0, 0, tx ],
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
22 [ 0, sy, 0, ty ],
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
23 [ 0, 0, 1, 0 ],
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
24 [ 0, 0, 0, 1 ]])
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
25
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
26 pureShear = Matrix([[ 1, 0, ex, 0 ],
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
27 [ 0, 1, ey, 0 ],
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
28 [ 0, 0, 1, 0 ],
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
29 [ 0, 0, ew, 1 ]])
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
30
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
31
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
32 # Create a general warp matrix, that corresponds to "M_warp" in
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
33 # Equation (A.17) of Lacroute's thesis:
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
34 ww11, ww12, ww13, ww14, ww21, ww22, ww23, ww24, ww31, ww32, ww33, ww34, ww41, ww42, ww43, ww44 = symbols('ww11 ww12 ww13 ww14 ww21 ww22 ww23 ww24 ww31 ww32 ww33 ww34 ww41 ww42 ww43 ww44')
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
35
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
36 WW = Matrix([[ ww11, ww12, ww13, ww14 ],
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
37 [ ww21, ww22, ww23, ww24 ],
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
38 [ ww31, ww32, ww33, ww34 ],
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
39 [ ww41, ww43, ww43, ww44 ]])
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
40
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
41
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
42 # Create the matrix of intrinsic parameters of the camera
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
43 k11, k22, k14, k24 = symbols('k11 k22 k14 k24')
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
44 K = Matrix([[ k11, 0, 0, k14 ],
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
45 [ 0, k22, 0, k24 ],
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
46 [ 0, 0, 0, 1 ]])
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
47
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
48
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
49 # The full decomposition is:
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
50 M_shear = TS * pureShear
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
51 M_warp = K * WW * TS.inv()
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
52 AA = M_warp * M_shear
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
53
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
54 # Check that the central component "M_warp == K * WW * TS.inv()" that
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
55 # is the left part of "A" is another general warp matrix (i.e. no
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
56 # exception is thrown about incompatible matrix sizes):
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
57 M_warp * p
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
58
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
59 if (M_warp.cols != 4 or
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
60 M_warp.rows != 3):
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
61 raise Exception('Invalid matrix size')
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
62
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
63
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
64 # We've just shown that "M_warp" is a general 3x4 matrix. Let's call
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
65 # it W:
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
66 w11, w12, w13, w14, w21, w22, w23, w24, w41, w42, w43, w44 = symbols('w11 w12 w13 w14 w21 w22 w23 w24 w41 w42 w43 w44')
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
67
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
68 W = Matrix([[ w11, w12, w13, w14 ],
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
69 [ w21, w22, w23, w24 ],
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
70 [ w41, w43, w43, w44 ]])
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
71
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
72 # This shows that it is sufficient to study a decomposition of the
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
73 # following form:
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
74 A = W * M_shear
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
75 print('\nA = W * M_shear =')
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
76 pprint.pprint(A)
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
77
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
78 sys.stdout.write('\nW = ')
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
79 pprint.pprint(W)
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
80
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
81 sys.stdout.write('\nM_shear = ')
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
82 pprint.pprint(M_shear)
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
83
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
84
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
85
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
86 # Let's consider one fixed 2D point (i,j) in the intermediate
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
87 # image. The 3D points (x,y,z,1) that are mapped to (i,j) must satisfy
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
88 # the equation "(i,j) == M_shear * (x,y,z,w)". As "M_shear" is
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
89 # invertible, we solve "(x,y,z,w) == inv(M_shear) * (i,j,k,1)".
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
90
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
91 i, j, k = symbols('i j k')
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
92 l = M_shear.inv() * Matrix([ i, j, k, 1 ])
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
93
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
94 print('\nLocus for points imaged to some fixed (i,j,k,l) point in the intermediate image:')
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
95 print('x = %s' % l[0])
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
96 print('y = %s' % l[1])
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
97 print('z = %s' % l[2])
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
98 print('w = %s' % l[3])
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
99
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
100
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
101 # By inspecting the 4 equations above, we see that the locus entirely
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
102 # depends upon the "k" value that encodes the Z-axis
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
103
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
104 print('\nGlobal effect of the shear-warp transform on this locus:')
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
105 q = expand(A * l)
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
106 pprint.pprint(q)
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
107
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
108 print("\nWe can arbitrarily fix the value of 'k', so let's choose 'k=0':")
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
109 pprint.pprint(q.subs(k, 0))
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
110
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
111 print("\nThis gives the warp transform.")
4abddd083374 ShearWarpProjectiveTransform::ApplyAxial()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
112 print("QED: line after Equation (A.17) on page 209.\n")