Download as
Browse Code »

Commit 85f1368a72eb5b358701c174e25a6b7368c76c6c

Authored by Renan
1 parent 53accd20
Exists in master

ADD thread

Showing 7 changed files with 127 additions and 83 deletions   Show diff stats
invesalius/constants.py
  Diff comments   View file @85f1368
... ... @@ -818,6 +818,7 @@ SEED_RADIUS = 1.5
818 818  
819 819 # Increased the default sleep parameter from 0.1 to 0.15 to decrease CPU load during navigation.
820 820 SLEEP_NAVIGATION = 0.15
  821 +SLEEP_COORDINATES = 0.05
821 822  
822 823 BRAIN_OPACITY = 0.5
823 824 N_CPU = psutil.cpu_count()
... ...
invesalius/data/coordinates.py
  Diff comments   View file @85f1368
... ... @@ -19,6 +19,9 @@
19 19  
20 20 from math import sin, cos
21 21 import numpy as np
  22 +import queue
  23 +import threading
  24 +import wx
22 25  
23 26 import invesalius.data.bases as db
24 27 import invesalius.data.transformations as tr
... ... @@ -28,9 +31,21 @@ from time import sleep
28 31 from random import uniform
29 32 from invesalius.pubsub import pub as Publisher
30 33  
  34 +class TrackerCoordinates():
  35 + def __init__(self):
  36 + self.coord = None
  37 + self.markers_flag = [False, False, False]
31 38  
32   -def GetCoordinates(trck_init, trck_id, ref_mode):
  39 + def SetCoordinates(self, coord, markers_flag):
  40 + self.coord = coord
  41 + self.markers_flag = markers_flag
33 42  
  43 + def GetCoordinates(self):
  44 + wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag)
  45 + return self.coord, self.markers_flag
  46 +
  47 +
  48 +def GetCoordinatesForThread(trck_init, trck_id, ref_mode):
34 49 """
35 50 Read coordinates from spatial tracking devices using
36 51  
... ... @@ -52,11 +67,55 @@ def GetCoordinates(trck_init, trck_id, ref_mode):
52 67 const.OPTITRACK: OptitrackCoord,
53 68 const.DEBUGTRACKRANDOM: DebugCoordRandom,
54 69 const.DEBUGTRACKAPPROACH: DebugCoordRandom}
55   - coord = getcoord[trck_id](trck_init, trck_id, ref_mode)
  70 +
  71 + coord, markers_flag = getcoord[trck_id](trck_init, trck_id, ref_mode)
56 72 else:
57 73 print("Select Tracker")
58 74  
59   - return coord
  75 + return coord, markers_flag
  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]
60 119  
61 120 def OptitrackCoord(trck_init, trck_id, ref_mode):
62 121 """
... ... @@ -93,8 +152,10 @@ def OptitrackCoord(trck_init, trck_id, ref_mode):
93 152 coord3 = np.array([float(trck.PositionCoilZ1) * scale[0], float(trck.PositionCoilX1) * scale[1],
94 153 float(trck.PositionCoilY1) * scale[2]])
95 154 coord3 = np.hstack((coord3, angles_coil))
  155 +
96 156 coord = np.vstack([coord1, coord2, coord3])
97   - return coord
  157 +
  158 + return coord, [trck.probeID, trck.HeadID, trck.coilID]
98 159  
99 160  
100 161 def PolarisCoord(trck_init, trck_id, ref_mode):
... ... @@ -117,62 +178,25 @@ def PolarisCoord(trck_init, trck_id, ref_mode):
117 178 coord3 = np.hstack((trans_obj, angles_obj))
118 179  
119 180 coord = np.vstack([coord1, coord2, coord3])
120   - # Publisher.sendMessage('Sensors ID', probe_id=trck.probeID, ref_id=trck.refID, obj_id=trck.objID)
121 181  
122   - return coord
  182 + return coord, [trck.probeID, trck.refID, trck.coilID]
123 183  
124   -def PolarisP4Coord(trck_init, trck_id, ref_mode):
125   - trck = trck_init[0]
126   - trck.Run()
127   -
128   - probe = trck.probe.decode(const.FS_ENCODE)
129   - ref = trck.ref.decode(const.FS_ENCODE)
130   - obj = trck.obj.decode(const.FS_ENCODE)
131   -
132   - probe = probe[2:]
133   - ref = ref[2:]
134   - obj = obj[2:]
135   -
136   - if probe[:7] == "MISSING":
137   - if not "coord1" in locals():
138   - coord1 = np.hstack(([0, 0, 0], [0, 0, 0]))
  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])
139 190 else:
140   - q = [int(probe[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
141   - t = [int(probe[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
142   - angles_probe = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
143   - trans_probe = np.array(t).astype(float)
144   - coord1 = np.hstack((trans_probe, angles_probe))
  191 + coord = np.array([0, 0, 0, 0, 0, 0])
145 192  
146   - if ref[:7] == "MISSING":
147   - if not "coord2" in locals():
148   - coord2 = np.hstack(([0, 0, 0], [0, 0, 0]))
149   - else:
150   - q = [int(ref[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
151   - t = [int(ref[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
152   - angles_ref = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
153   - trans_ref = np.array(t).astype(float)
154   - coord2 = np.hstack((trans_ref, angles_ref))
155   -
156   - if obj[:7] == "MISSING":
157   - if not "coord3" in locals():
158   - coord3 = np.hstack(([0, 0, 0], [0, 0, 0]))
159   - else:
160   - q = [int(obj[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
161   - t = [int(obj[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
162   - angles_obj = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
163   - trans_obj = np.array(t).astype(float)
164   - coord3 = np.hstack((trans_obj, angles_obj))
165   -
166   - Publisher.sendMessage('Sensors ID', probe_id=trck.probeID, ref_id=trck.refID, obj_id=trck.objID)
167   - coord = np.vstack([coord1, coord2, coord3])
168   -
169   - return coord
  193 + return coord, None
170 194  
171 195 def CameraCoord(trck_init, trck_id, ref_mode):
172 196 trck = trck_init[0]
173   - coord, probeID, refID = trck.Run()
174   - Publisher.sendMessage('Sensors ID', probe_id=probeID, ref_id=refID)
175   - return coord
  197 + coord, probeID, refID, coilID = trck.Run()
  198 +
  199 + return coord, [probeID, refID, coilID]
176 200  
177 201 def ClaronCoord(trck_init, trck_id, ref_mode):
178 202 trck = trck_init[0]
... ... @@ -193,9 +217,7 @@ def ClaronCoord(trck_init, trck_id, ref_mode):
193 217  
194 218 coord = np.vstack([coord1, coord2, coord3])
195 219  
196   - Publisher.sendMessage('Sensors ID', probe_id=trck.probeID, ref_id=trck.refID)
197   -
198   - return coord
  220 + return coord, [trck.probeID, trck.refID, trck.coilID]
199 221  
200 222  
201 223 def PolhemusCoord(trck, trck_id, ref_mode):
... ... @@ -210,7 +232,7 @@ def PolhemusCoord(trck, trck_id, ref_mode):
210 232 elif trck[1] == 'wrapper':
211 233 coord = PolhemusWrapperCoord(trck[0], trck_id, ref_mode)
212 234  
213   - return coord
  235 + return coord, None
214 236  
215 237  
216 238 def PolhemusWrapperCoord(trck, trck_id, ref_mode):
... ... @@ -362,16 +384,15 @@ def DebugCoordRandom(trk_init, trck_id, ref_mode):
362 384 # coord4 = np.array([uniform(1, 200), uniform(1, 200), uniform(1, 200),
363 385 # uniform(-180.0, 180.0), uniform(-180.0, 180.0), uniform(-180.0, 180.0)])
364 386  
365   - Publisher.sendMessage('Sensors ID', probe_id=int(uniform(0, 5)), ref_id=int(uniform(0, 5)), obj_id=int(uniform(0, 5)))
  387 + #Publisher.sendMessage('Sensors ID', probe_id=int(uniform(0, 5)), ref_id=int(uniform(0, 5)), obj_id=int(uniform(0, 5)))
366 388  
367   - return np.vstack([coord1, coord2, coord3, coord4])
  389 + return np.vstack([coord1, coord2, coord3, coord4]), [int(uniform(0, 5)), int(uniform(0, 5)), int(uniform(0, 5))]
368 390  
369 391  
370 392 def coordinates_to_transformation_matrix(position, orientation, axes='sxyz'):
371 393 """
372 394 Transform vectors consisting of position and orientation (in Euler angles) in 3d-space into a 4x4
373 395 transformation matrix that combines the rotation and translation.
374   -
375 396 :param position: A vector of three coordinates.
376 397 :param orientation: A vector of three Euler angles in degrees.
377 398 :param axes: The order in which the rotations are done for the axes. See transformations.py for details. Defaults to 'sxyz'.
... ... @@ -391,9 +412,7 @@ def transformation_matrix_to_coordinates(matrix, axes='sxyz'):
391 412 """
392 413 Given a matrix that combines the rotation and translation, return the position and the orientation
393 414 determined by the matrix. The orientation is given as three Euler angles.
394   -
395 415 The inverse of coordinates_of_transformation_matrix when the parameter 'axes' matches.
396   -
397 416 :param matrix: A 4x4 transformation matrix.
398 417 :param axes: The order in which the rotations are done for the axes. See transformations.py for details. Defaults to 'sxyz'.
399 418 :return: The position (a vector of length 3) and Euler angles for the orientation in degrees (a vector of length 3).
... ... @@ -452,11 +471,11 @@ def dynamic_reference_m(probe, reference):
452 471 :param reference: sensor two defined as reference
453 472 :return: rotated and translated coordinates
454 473 """
455   - affine = coordinates_to_transformation_matrix(
456   - position=reference[:3],
457   - orientation=reference[3:],
458   - axes='rzyx',
459   - )
  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)
460 479 probe_4 = np.vstack((probe[:3].reshape([3, 1]), 1.))
461 480 coord_rot = np.linalg.inv(affine) @ probe_4
462 481 # minus sign to the z coordinate
... ... @@ -479,16 +498,17 @@ def dynamic_reference_m2(probe, reference):
479 498 :param reference: sensor two defined as reference
480 499 :return: rotated and translated coordinates
481 500 """
482   - M = coordinates_to_transformation_matrix(
483   - position=reference[:3],
484   - orientation=reference[3:],
485   - axes='rzyx',
486   - )
487   - M_p = coordinates_to_transformation_matrix(
488   - position=probe[:3],
489   - orientation=probe[3:],
490   - axes='rzyx',
491   - )
  501 +
  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)
  511 +
492 512 M_dyn = np.linalg.inv(M) @ M_p
493 513  
494 514 al, be, ga = tr.euler_from_matrix(M_dyn, 'rzyx')
... ... @@ -529,3 +549,17 @@ def offset_coordinate(p_old, norm_vec, offset):
529 549 """
530 550 p_offset = p_old - offset * norm_vec
531 551 return p_offset
  552 +
  553 +class ReceiveCoordinates(threading.Thread):
  554 + def __init__(self, trck_init, trck_id, TrackerCoordinates, event):
  555 + threading.Thread.__init__(self, name='ReceiveCoordinates')
  556 + self.trck_init = trck_init
  557 + self.trck_id = trck_id
  558 + self.event = event
  559 + self.TrackerCoordinates = TrackerCoordinates
  560 +
  561 + def run(self):
  562 + while not self.event.is_set():
  563 + coord_raw, markers_flag = GetCoordinatesForThread(self.trck_init, self.trck_id, const.DEFAULT_REF_MODE)
  564 + self.TrackerCoordinates.SetCoordinates(coord_raw, markers_flag)
  565 + sleep(const.SLEEP_COORDINATES)
532 566 \ No newline at end of file
... ...
invesalius/data/coregistration.py
  Diff comments   View file @85f1368
... ... @@ -219,7 +219,7 @@ class CoordinateCorregistrate(threading.Thread):
219 219 else:
220 220 self.icp, self.m_icp = self.icp_queue.get_nowait()
221 221 # print(f"Set the coordinate")
222   - coord_raw = dco.GetCoordinates(trck_init, trck_id, self.ref_mode_id)
  222 + coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates()
223 223 coord, m_img = corregistrate_object_dynamic(coreg_data, coord_raw, self.ref_mode_id, [self.icp, self.m_icp])
224 224  
225 225 # XXX: This is not the best place to do the logic related to approaching the target when the
... ... @@ -294,7 +294,7 @@ class CoordinateCorregistrateNoObject(threading.Thread):
294 294 self.icp, self.m_icp = self.icp_queue.get_nowait()
295 295 # print(f"Set the coordinate")
296 296 #print(self.icp, self.m_icp)
297   - coord_raw = dco.GetCoordinates(trck_init, trck_id, self.ref_mode_id)
  297 + coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates()
298 298 coord, m_img = corregistrate_dynamic(coreg_data, coord_raw, self.ref_mode_id, [self.icp, self.m_icp])
299 299 # print("Coord: ", coord)
300 300 m_img_flip = m_img.copy()
... ...
invesalius/data/viewer_volume.py
  Diff comments   View file @85f1368
... ... @@ -368,7 +368,9 @@ class Viewer(wx.Panel):
368 368  
369 369 self.interactor.Render()
370 370  
371   - def OnSensors(self, probe_id, ref_id, obj_id=0):
  371 + def OnSensors(self, markers_flag):
  372 + probe_id, ref_id, obj_id = markers_flag
  373 +
372 374 if not self.probe:
373 375 self.CreateSensorID()
374 376  
... ...
invesalius/gui/dialogs.py
  Diff comments   View file @85f1368
... ... @@ -3804,7 +3804,7 @@ class ICPCorregistrationDialog(wx.Dialog):
3804 3804 self.interactor.Render()
3805 3805  
3806 3806 def GetCurrentCoord(self):
3807   - coord_raw = dco.GetCoordinates(self.trk_init, self.tracker_id, const.DYNAMIC_REF)
  3807 + coord_raw = dco.TrackerCoordinates.GetCoordinates()
3808 3808 coord, _ = dcr.corregistrate_dynamic((self.m_change, 0), coord_raw, const.DEFAULT_REF_MODE, [None, None])
3809 3809 return coord[:3]
3810 3810  
... ...
invesalius/navigation/navigation.py
  Diff comments   View file @85f1368
... ... @@ -255,7 +255,7 @@ class Navigation():
255 255 coreg_data = [m_change, obj_ref_mode]
256 256  
257 257 if self.ref_mode_id:
258   - coord_raw = dco.GetCoordinates(tracker.trk_init, tracker.tracker_id, self.ref_mode_id)
  258 + coord_raw, markers_flag = tracker.TrackerCoordinates.GetCoordinates()
259 259 else:
260 260 coord_raw = np.array([None])
261 261  
... ...
invesalius/navigation/tracker.py
  Diff comments   View file @85f1368
... ... @@ -18,6 +18,7 @@
18 18 #--------------------------------------------------------------------------
19 19  
20 20 import numpy as np
  21 +import threading
21 22  
22 23 import invesalius.constants as const
23 24 import invesalius.data.coordinates as dco
... ... @@ -36,6 +37,10 @@ class Tracker():
36 37  
37 38 self.tracker_connected = False
38 39  
  40 + self.event_coord = threading.Event()
  41 +
  42 + self.TrackerCoordinates = dco.TrackerCoordinates()
  43 +
39 44 def SetTracker(self, new_tracker):
40 45 if new_tracker:
41 46 self.DisconnectTracker()
... ... @@ -49,6 +54,8 @@ class Tracker():
49 54 else:
50 55 self.tracker_id = new_tracker
51 56 self.tracker_connected = True
  57 + dco.ReceiveCoordinates(self.trk_init, self.tracker_id, self.TrackerCoordinates,
  58 + self.event_coord).start()
52 59  
53 60 def DisconnectTracker(self):
54 61 if self.tracker_connected:
... ... @@ -81,7 +88,7 @@ class Tracker():
81 88 coord_samples = {}
82 89  
83 90 for i in range(n_samples):
84   - coord_raw = dco.GetCoordinates(self.trk_init, self.tracker_id, ref_mode_id)
  91 + coord_raw, markers_flag = self.TrackerCoordinates.GetCoordinates()
85 92  
86 93 if ref_mode_id == const.DYNAMIC_REF:
87 94 coord = dco.dynamic_reference_m(coord_raw[0, :], coord_raw[1, :])
... ...