Commit 8acbc26717a11021a323b030c88ed51f526cdd09
1 parent
1ba56fa2
Exists in
master
FIX: minor standardization
Showing
1 changed file
with
56 additions
and
66 deletions
Show diff stats
invesalius/data/coordinates.py
... | ... | @@ -74,48 +74,6 @@ def GetCoordinatesForThread(trck_init, trck_id, ref_mode): |
74 | 74 | |
75 | 75 | return coord, markers_flag |
76 | 76 | |
77 | -def PolarisP4Coord(trck_init, trck_id, ref_mode): | |
78 | - trck = trck_init[0] | |
79 | - trck.Run() | |
80 | - | |
81 | - probe = trck.probe.decode(const.FS_ENCODE) | |
82 | - ref = trck.ref.decode(const.FS_ENCODE) | |
83 | - obj = trck.obj.decode(const.FS_ENCODE) | |
84 | - | |
85 | - probe = probe[2:] | |
86 | - ref = ref[2:] | |
87 | - obj = obj[2:] | |
88 | - | |
89 | - if probe[:7] == "MISSING": | |
90 | - coord1 = np.hstack(([0, 0, 0], [0, 0, 0])) | |
91 | - else: | |
92 | - q = [int(probe[i:i + 6]) * 0.0001 for i in range(0, 24, 6)] | |
93 | - t = [int(probe[i:i + 7]) * 0.01 for i in range(24, 45, 7)] | |
94 | - angles_probe = np.degrees(tr.euler_from_quaternion(q, axes='rzyx')) | |
95 | - trans_probe = np.array(t).astype(float) | |
96 | - coord1 = np.hstack((trans_probe, angles_probe)) | |
97 | - | |
98 | - if ref[:7] == "MISSING": | |
99 | - coord2 = np.hstack(([0, 0, 0], [0, 0, 0])) | |
100 | - else: | |
101 | - q = [int(ref[i:i + 6]) * 0.0001 for i in range(0, 24, 6)] | |
102 | - t = [int(ref[i:i + 7]) * 0.01 for i in range(24, 45, 7)] | |
103 | - angles_ref = np.degrees(tr.euler_from_quaternion(q, axes='rzyx')) | |
104 | - trans_ref = np.array(t).astype(float) | |
105 | - coord2 = np.hstack((trans_ref, angles_ref)) | |
106 | - | |
107 | - if obj[:7] == "MISSING": | |
108 | - coord3 = np.hstack(([0, 0, 0], [0, 0, 0])) | |
109 | - else: | |
110 | - q = [int(obj[i:i + 6]) * 0.0001 for i in range(0, 24, 6)] | |
111 | - t = [int(obj[i:i + 7]) * 0.01 for i in range(24, 45, 7)] | |
112 | - angles_obj = np.degrees(tr.euler_from_quaternion(q, axes='rzyx')) | |
113 | - trans_obj = np.array(t).astype(float) | |
114 | - coord3 = np.hstack((trans_obj, angles_obj)) | |
115 | - | |
116 | - coord = np.vstack([coord1, coord2, coord3]) | |
117 | - | |
118 | - return coord, [trck.probeID, trck.refID, trck.objID] | |
119 | 77 | |
120 | 78 | def OptitrackCoord(trck_init, trck_id, ref_mode): |
121 | 79 | """ |
... | ... | @@ -181,16 +139,49 @@ def PolarisCoord(trck_init, trck_id, ref_mode): |
181 | 139 | |
182 | 140 | return coord, [trck.probeID, trck.refID, trck.coilID] |
183 | 141 | |
184 | -def ElfinCoord(trck_init): | |
185 | - if len(trck_init) > 2: | |
186 | - robotcoordinates = trck_init[-1] | |
187 | - coord = robotcoordinates.GetRobotCoordinates() | |
188 | - if coord is None: | |
189 | - coord = np.array([0, 0, 0, 0, 0, 0]) | |
142 | + | |
143 | +def PolarisP4Coord(trck_init, trck_id, ref_mode): | |
144 | + trck = trck_init[0] | |
145 | + trck.Run() | |
146 | + | |
147 | + probe = trck.probe.decode(const.FS_ENCODE) | |
148 | + ref = trck.ref.decode(const.FS_ENCODE) | |
149 | + obj = trck.obj.decode(const.FS_ENCODE) | |
150 | + | |
151 | + probe = probe[2:] | |
152 | + ref = ref[2:] | |
153 | + obj = obj[2:] | |
154 | + | |
155 | + if probe[:7] == "MISSING": | |
156 | + coord1 = np.hstack(([0, 0, 0], [0, 0, 0])) | |
190 | 157 | else: |
191 | - coord = np.array([0, 0, 0, 0, 0, 0]) | |
158 | + q = [int(probe[i:i + 6]) * 0.0001 for i in range(0, 24, 6)] | |
159 | + t = [int(probe[i:i + 7]) * 0.01 for i in range(24, 45, 7)] | |
160 | + angles_probe = np.degrees(tr.euler_from_quaternion(q, axes='rzyx')) | |
161 | + trans_probe = np.array(t).astype(float) | |
162 | + coord1 = np.hstack((trans_probe, angles_probe)) | |
163 | + if ref[:7] == "MISSING": | |
164 | + coord2 = np.hstack(([0, 0, 0], [0, 0, 0])) | |
165 | + else: | |
166 | + q = [int(ref[i:i + 6]) * 0.0001 for i in range(0, 24, 6)] | |
167 | + t = [int(ref[i:i + 7]) * 0.01 for i in range(24, 45, 7)] | |
168 | + angles_ref = np.degrees(tr.euler_from_quaternion(q, axes='rzyx')) | |
169 | + trans_ref = np.array(t).astype(float) | |
170 | + coord2 = np.hstack((trans_ref, angles_ref)) | |
171 | + | |
172 | + if obj[:7] == "MISSING": | |
173 | + coord3 = np.hstack(([0, 0, 0], [0, 0, 0])) | |
174 | + else: | |
175 | + q = [int(obj[i:i + 6]) * 0.0001 for i in range(0, 24, 6)] | |
176 | + t = [int(obj[i:i + 7]) * 0.01 for i in range(24, 45, 7)] | |
177 | + angles_obj = np.degrees(tr.euler_from_quaternion(q, axes='rzyx')) | |
178 | + trans_obj = np.array(t).astype(float) | |
179 | + coord3 = np.hstack((trans_obj, angles_obj)) | |
180 | + | |
181 | + coord = np.vstack([coord1, coord2, coord3]) | |
182 | + | |
183 | + return coord, [trck.probeID, trck.refID, trck.objID] | |
192 | 184 | |
193 | - return coord, None | |
194 | 185 | |
195 | 186 | def CameraCoord(trck_init, trck_id, ref_mode): |
196 | 187 | trck = trck_init[0] |
... | ... | @@ -384,8 +375,6 @@ def DebugCoordRandom(trk_init, trck_id, ref_mode): |
384 | 375 | # coord4 = np.array([uniform(1, 200), uniform(1, 200), uniform(1, 200), |
385 | 376 | # uniform(-180.0, 180.0), uniform(-180.0, 180.0), uniform(-180.0, 180.0)]) |
386 | 377 | |
387 | - #Publisher.sendMessage('Sensors ID', probe_id=int(uniform(0, 5)), ref_id=int(uniform(0, 5)), obj_id=int(uniform(0, 5))) | |
388 | - | |
389 | 378 | return np.vstack([coord1, coord2, coord3, coord4]), [int(uniform(0, 5)), int(uniform(0, 5)), int(uniform(0, 5))] |
390 | 379 | |
391 | 380 | |
... | ... | @@ -471,11 +460,11 @@ def dynamic_reference_m(probe, reference): |
471 | 460 | :param reference: sensor two defined as reference |
472 | 461 | :return: rotated and translated coordinates |
473 | 462 | """ |
474 | - a, b, g = np.radians(reference[3:6]) | |
475 | - | |
476 | - trans = tr.translation_matrix(reference[:3]) | |
477 | - rot = tr.euler_matrix(a, b, g, 'rzyx') | |
478 | - affine = tr.concatenate_matrices(trans, rot) | |
463 | + affine = coordinates_to_transformation_matrix( | |
464 | + position=reference[:3], | |
465 | + orientation=reference[3:], | |
466 | + axes='rzyx', | |
467 | + ) | |
479 | 468 | probe_4 = np.vstack((probe[:3].reshape([3, 1]), 1.)) |
480 | 469 | coord_rot = np.linalg.inv(affine) @ probe_4 |
481 | 470 | # minus sign to the z coordinate |
... | ... | @@ -499,15 +488,16 @@ def dynamic_reference_m2(probe, reference): |
499 | 488 | :return: rotated and translated coordinates |
500 | 489 | """ |
501 | 490 | |
502 | - a, b, g = np.radians(reference[3:6]) | |
503 | - a_p, b_p, g_p = np.radians(probe[3:6]) | |
504 | - | |
505 | - T = tr.translation_matrix(reference[:3]) | |
506 | - T_p = tr.translation_matrix(probe[:3]) | |
507 | - R = tr.euler_matrix(a, b, g, 'rzyx') | |
508 | - R_p = tr.euler_matrix(a_p, b_p, g_p, 'rzyx') | |
509 | - M = tr.concatenate_matrices(T, R) | |
510 | - M_p = tr.concatenate_matrices(T_p, R_p) | |
491 | + M = coordinates_to_transformation_matrix( | |
492 | + position=reference[:3], | |
493 | + orientation=reference[3:], | |
494 | + axes='rzyx', | |
495 | + ) | |
496 | + M_p = coordinates_to_transformation_matrix( | |
497 | + position=probe[:3], | |
498 | + orientation=probe[3:], | |
499 | + axes='rzyx', | |
500 | + ) | |
511 | 501 | |
512 | 502 | M_dyn = np.linalg.inv(M) @ M_p |
513 | 503 | ... | ... |