Commit 8acbc26717a11021a323b030c88ed51f526cdd09

Authored by Renan
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  
... ...