@@ -40,15 +40,21 @@ def angles2matrix(rot_z=0, rot_y=0, rot_x=0):
4040 if rot_z :
4141 cos = math .cos (rot_z )
4242 sin = math .sin (rot_z )
43- rot_matrix .append (np .array ([cos , - sin , 0 , sin , cos , 0 , 0 , 0 , 1 ]).reshape ((3 , 3 )))
43+ rot_matrix .append (
44+ np .array ([cos , - sin , 0 , sin , cos , 0 , 0 , 0 , 1 ]).reshape ((3 , 3 ))
45+ )
4446 if rot_y :
4547 cos = math .cos (rot_y )
4648 sin = math .sin (rot_y )
47- rot_matrix .append (np .array ([cos , 0 , sin , 0 , 1 , 0 , - sin , 0 , cos ]).reshape ((3 , 3 )))
49+ rot_matrix .append (
50+ np .array ([cos , 0 , sin , 0 , 1 , 0 , - sin , 0 , cos ]).reshape ((3 , 3 ))
51+ )
4852 if rot_x :
4953 cos = math .cos (rot_x )
5054 sin = math .sin (rot_x )
51- rot_matrix .append (np .array ([1 , 0 , 0 , 0 , cos , - sin , 0 , sin , cos ]).reshape ((3 , 3 )))
55+ rot_matrix .append (
56+ np .array ([1 , 0 , 0 , 0 , cos , - sin , 0 , sin , cos ]).reshape ((3 , 3 ))
57+ )
5258 if rot_matrix :
5359 return reduce (np .dot , rot_matrix [::- 1 ])
5460 return np .eye (3 )
@@ -96,7 +102,9 @@ def to_reduced_row_echelon_form(matrix):
96102 for i in range (row_count ):
97103 if i != r :
98104 lv = matrix [i ][lead ]
99- matrix [i ] = [iv - lv * rv for rv , iv in zip (matrix [r ], matrix [i ])]
105+ matrix [i ] = [
106+ iv - lv * rv for rv , iv in zip (matrix [r ], matrix [i ])
107+ ]
100108 lead += 1
101109 return matrix
102110
@@ -130,34 +138,36 @@ def affine_points_fit(points_start, points_end):
130138
131139 dim = len (points_start [0 ])
132140 if len (points_start ) < dim :
133- raise RuntimeError ("Too few starting points => under-determined system." )
141+ raise RuntimeError (
142+ "Too few starting points => under-determined system."
143+ )
134144
135145 # Fill an an empty (dim+1) x (dim) matrix
136- c = [[0.0 for a in range (dim )] for i in range (dim + 1 )]
146+ c = [[0.0 for a in range (dim )] for i in range (dim + 1 )]
137147 for j in range (dim ):
138- for k in range (dim + 1 ):
148+ for k in range (dim + 1 ):
139149 for i , pnts_i in enumerate (points_start ):
140150 qt = list (pnts_i ) + [1 ]
141151 c [k ][j ] += qt [k ] * points_end [i ][j ]
142152
143153 # Fill an an empty (dim+1) x (dim+1) matrix
144- Q = [[0.0 for a in range (dim )] + [0 ] for i in range (dim + 1 )]
154+ Q = [[0.0 for a in range (dim )] + [0 ] for i in range (dim + 1 )]
145155 for qi in points_start :
146156 qt = list (qi ) + [1 ]
147- for i in range (dim + 1 ):
148- for j in range (dim + 1 ):
157+ for i in range (dim + 1 ):
158+ for j in range (dim + 1 ):
149159 Q [i ][j ] += qt [i ] * qt [j ]
150160
151-
152161 # Augement Q with c and get the reduced row echelon form of the result
153- affine_matrix = [Q [i ] + c [i ] for i in range (dim + 1 )]
162+ affine_matrix = [Q [i ] + c [i ] for i in range (dim + 1 )]
154163
155- if np .linalg .cond (affine_matrix ) < 1 / sys .float_info .epsilon :
164+ if np .linalg .cond (affine_matrix ) < 1 / sys .float_info .epsilon :
156165 rref_aff_matrix = to_reduced_row_echelon_form (affine_matrix )
157166 rref_aff_matrix = np .array (rref_aff_matrix )
158167 else :
159- raise RuntimeError ("Error: singular matrix. Points are probably coplanar." )
160-
168+ raise RuntimeError (
169+ "Error: singular matrix. Points are probably coplanar."
170+ )
161171
162172 def transform_vector (source ):
163173 """
@@ -171,9 +181,9 @@ def transform_vector(source):
171181 destination = np .zeros (dim )
172182 for i in range (dim ):
173183 for j in range (dim ):
174- destination [j ] += source [i ] * rref_aff_matrix [i ][j + dim + 1 ]
184+ destination [j ] += source [i ] * rref_aff_matrix [i ][j + dim + 1 ]
175185 # Add the last line of the rref
176- destination [i ] += rref_aff_matrix [dim ][i + dim + 1 ]
186+ destination [i ] += rref_aff_matrix [dim ][i + dim + 1 ]
177187 return destination
178188
179189 return transform_vector
0 commit comments