Download as
Browse Code »

Commit 490d87a83a37a37b3544842d88e251fdb2869c20

Authored by Victor Hugo Souza
Committed by GitHub
2 parents dd8b84f9 0b8581bf
Exists in master

Merge pull request #369 from rmatsuda/Thread-for-coordinates 

Creation of a new thread for coordinates acquisition
Showing 8 changed files with 92 additions and 51 deletions   Show diff stats
invesalius/constants.py
  Diff comments   View file @490d87a
... ... @@ -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 @490d87a
... ... @@ -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
  42 + wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag)
33 43  
  44 + def GetCoordinates(self):
  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,13 @@ 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 +
60 77  
61 78 def OptitrackCoord(trck_init, trck_id, ref_mode):
62 79 """
... ... @@ -93,8 +110,10 @@ def OptitrackCoord(trck_init, trck_id, ref_mode):
93 110 coord3 = np.array([float(trck.PositionCoilZ1) * scale[0], float(trck.PositionCoilX1) * scale[1],
94 111 float(trck.PositionCoilY1) * scale[2]])
95 112 coord3 = np.hstack((coord3, angles_coil))
  113 +
96 114 coord = np.vstack([coord1, coord2, coord3])
97   - return coord
  115 +
  116 + return coord, [trck.probeID, trck.HeadID, trck.coilID]
98 117  
99 118  
100 119 def PolarisCoord(trck_init, trck_id, ref_mode):
... ... @@ -117,9 +136,9 @@ def PolarisCoord(trck_init, trck_id, ref_mode):
117 136 coord3 = np.hstack((trans_obj, angles_obj))
118 137  
119 138 coord = np.vstack([coord1, coord2, coord3])
120   - # Publisher.sendMessage('Sensors ID', probe_id=trck.probeID, ref_id=trck.refID, obj_id=trck.objID)
121 139  
122   - return coord
  140 + return coord, [trck.probeID, trck.refID, trck.objID]
  141 +
123 142  
124 143 def PolarisP4Coord(trck_init, trck_id, ref_mode):
125 144 trck = trck_init[0]
... ... @@ -134,45 +153,41 @@ def PolarisP4Coord(trck_init, trck_id, ref_mode):
134 153 obj = obj[2:]
135 154  
136 155 if probe[:7] == "MISSING":
137   - if not "coord1" in locals():
138   - coord1 = np.hstack(([0, 0, 0], [0, 0, 0]))
  156 + coord1 = np.hstack(([0, 0, 0], [0, 0, 0]))
139 157 else:
140 158 q = [int(probe[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
141 159 t = [int(probe[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
142 160 angles_probe = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
143 161 trans_probe = np.array(t).astype(float)
144 162 coord1 = np.hstack((trans_probe, angles_probe))
145   -
146   - if ref[:7] == "MISSING":
147   - if not "coord2" in locals():
  163 + if ref[:7] == "MISSING":
148 164 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():
  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":
158 173 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))
  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))
165 180  
166   - Publisher.sendMessage('Sensors ID', probe_id=trck.probeID, ref_id=trck.refID, obj_id=trck.objID)
167   - coord = np.vstack([coord1, coord2, coord3])
  181 + coord = np.vstack([coord1, coord2, coord3])
  182 +
  183 + return coord, [trck.probeID, trck.refID, trck.objID]
168 184  
169   - return coord
170 185  
171 186 def CameraCoord(trck_init, trck_id, ref_mode):
172 187 trck = trck_init[0]
173   - coord, probeID, refID = trck.Run()
174   - Publisher.sendMessage('Sensors ID', probe_id=probeID, ref_id=refID)
175   - return coord
  188 + coord, probeID, refID, coilID = trck.Run()
  189 +
  190 + return coord, [probeID, refID, coilID]
176 191  
177 192 def ClaronCoord(trck_init, trck_id, ref_mode):
178 193 trck = trck_init[0]
... ... @@ -193,9 +208,7 @@ def ClaronCoord(trck_init, trck_id, ref_mode):
193 208  
194 209 coord = np.vstack([coord1, coord2, coord3])
195 210  
196   - Publisher.sendMessage('Sensors ID', probe_id=trck.probeID, ref_id=trck.refID)
197   -
198   - return coord
  211 + return coord, [trck.probeID, trck.refID, trck.coilID]
199 212  
200 213  
201 214 def PolhemusCoord(trck, trck_id, ref_mode):
... ... @@ -210,7 +223,7 @@ def PolhemusCoord(trck, trck_id, ref_mode):
210 223 elif trck[1] == 'wrapper':
211 224 coord = PolhemusWrapperCoord(trck[0], trck_id, ref_mode)
212 225  
213   - return coord
  226 + return coord, None
214 227  
215 228  
216 229 def PolhemusWrapperCoord(trck, trck_id, ref_mode):
... ... @@ -362,16 +375,13 @@ def DebugCoordRandom(trk_init, trck_id, ref_mode):
362 375 # coord4 = np.array([uniform(1, 200), uniform(1, 200), uniform(1, 200),
363 376 # uniform(-180.0, 180.0), uniform(-180.0, 180.0), uniform(-180.0, 180.0)])
364 377  
365   - Publisher.sendMessage('Sensors ID', probe_id=int(uniform(0, 5)), ref_id=int(uniform(0, 5)), obj_id=int(uniform(0, 5)))
366   -
367   - return np.vstack([coord1, coord2, coord3, coord4])
  378 + return np.vstack([coord1, coord2, coord3, coord4]), [int(uniform(0, 5)), int(uniform(0, 5)), int(uniform(0, 5))]
368 379  
369 380  
370 381 def coordinates_to_transformation_matrix(position, orientation, axes='sxyz'):
371 382 """
372 383 Transform vectors consisting of position and orientation (in Euler angles) in 3d-space into a 4x4
373 384 transformation matrix that combines the rotation and translation.
374   -
375 385 :param position: A vector of three coordinates.
376 386 :param orientation: A vector of three Euler angles in degrees.
377 387 :param axes: The order in which the rotations are done for the axes. See transformations.py for details. Defaults to 'sxyz'.
... ... @@ -391,9 +401,7 @@ def transformation_matrix_to_coordinates(matrix, axes='sxyz'):
391 401 """
392 402 Given a matrix that combines the rotation and translation, return the position and the orientation
393 403 determined by the matrix. The orientation is given as three Euler angles.
394   -
395 404 The inverse of coordinates_of_transformation_matrix when the parameter 'axes' matches.
396   -
397 405 :param matrix: A 4x4 transformation matrix.
398 406 :param axes: The order in which the rotations are done for the axes. See transformations.py for details. Defaults to 'sxyz'.
399 407 :return: The position (a vector of length 3) and Euler angles for the orientation in degrees (a vector of length 3).
... ... @@ -479,6 +487,7 @@ def dynamic_reference_m2(probe, reference):
479 487 :param reference: sensor two defined as reference
480 488 :return: rotated and translated coordinates
481 489 """
  490 +
482 491 M = coordinates_to_transformation_matrix(
483 492 position=reference[:3],
484 493 orientation=reference[3:],
... ... @@ -489,6 +498,7 @@ def dynamic_reference_m2(probe, reference):
489 498 orientation=probe[3:],
490 499 axes='rzyx',
491 500 )
  501 +
492 502 M_dyn = np.linalg.inv(M) @ M_p
493 503  
494 504 al, be, ga = tr.euler_from_matrix(M_dyn, 'rzyx')
... ... @@ -529,3 +539,17 @@ def offset_coordinate(p_old, norm_vec, offset):
529 539 """
530 540 p_offset = p_old - offset * norm_vec
531 541 return p_offset
  542 +
  543 +class ReceiveCoordinates(threading.Thread):
  544 + def __init__(self, trck_init, trck_id, TrackerCoordinates, event):
  545 + threading.Thread.__init__(self, name='ReceiveCoordinates')
  546 + self.trck_init = trck_init
  547 + self.trck_id = trck_id
  548 + self.event = event
  549 + self.TrackerCoordinates = TrackerCoordinates
  550 +
  551 + def run(self):
  552 + while not self.event.is_set():
  553 + coord_raw, markers_flag = GetCoordinatesForThread(self.trck_init, self.trck_id, const.DEFAULT_REF_MODE)
  554 + self.TrackerCoordinates.SetCoordinates(coord_raw, markers_flag)
  555 + sleep(const.SLEEP_COORDINATES)
532 556 \ No newline at end of file
... ...
invesalius/data/coregistration.py
  Diff comments   View file @490d87a
... ... @@ -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 @490d87a
... ... @@ -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 @490d87a
... ... @@ -3648,8 +3648,7 @@ class ICPCorregistrationDialog(wx.Dialog):
3648 3648 import invesalius.project as prj
3649 3649  
3650 3650 self.m_change = nav_prop[0]
3651   - self.tracker_id = nav_prop[1]
3652   - self.trk_init = nav_prop[2]
  3651 + self.tracker = nav_prop[1]
3653 3652 self.obj_ref_id = 2
3654 3653 self.obj_name = None
3655 3654 self.obj_actor = None
... ... @@ -3805,7 +3804,7 @@ class ICPCorregistrationDialog(wx.Dialog):
3805 3804 self.interactor.Render()
3806 3805  
3807 3806 def GetCurrentCoord(self):
3808   - coord_raw = dco.GetCoordinates(self.trk_init, self.tracker_id, const.DYNAMIC_REF)
  3807 + coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates()
3809 3808 coord, _ = dcr.corregistrate_dynamic((self.m_change, 0), coord_raw, const.DEFAULT_REF_MODE, [None, None])
3810 3809 return coord[:3]
3811 3810  
... ...
invesalius/navigation/icp.py
  Diff comments   View file @490d87a
... ... @@ -48,7 +48,7 @@ class ICP():
48 48 def OnICP(self, navigation, tracker, m_change):
49 49 ref_mode_id = navigation.GetReferenceMode()
50 50  
51   - dialog = dlg.ICPCorregistrationDialog(nav_prop=(m_change, tracker.tracker_id, tracker.trk_init, ref_mode_id))
  51 + dialog = dlg.ICPCorregistrationDialog(nav_prop=(m_change, tracker, ref_mode_id))
52 52  
53 53 if dialog.ShowModal() == wx.ID_OK:
54 54 m_icp, point_coord, transformed_points, prev_error, final_error = dialog.GetValue()
... ...
invesalius/navigation/navigation.py
  Diff comments   View file @490d87a
... ... @@ -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 @490d87a
... ... @@ -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,12 @@ class Tracker():
36 37  
37 38 self.tracker_connected = False
38 39  
  40 + self.thread_coord = None
  41 +
  42 + self.event_coord = threading.Event()
  43 +
  44 + self.TrackerCoordinates = dco.TrackerCoordinates()
  45 +
39 46 def SetTracker(self, new_tracker):
40 47 if new_tracker:
41 48 self.DisconnectTracker()
... ... @@ -49,6 +56,9 @@ class Tracker():
49 56 else:
50 57 self.tracker_id = new_tracker
51 58 self.tracker_connected = True
  59 + self.thread_coord = dco.ReceiveCoordinates(self.trk_init, self.tracker_id, self.TrackerCoordinates,
  60 + self.event_coord)
  61 + self.thread_coord.start()
52 62  
53 63 def DisconnectTracker(self):
54 64 if self.tracker_connected:
... ... @@ -62,6 +72,11 @@ class Tracker():
62 72 self.tracker_connected = False
63 73 self.tracker_id = 0
64 74  
  75 + if self.thread_coord:
  76 + self.event_coord.set()
  77 + self.thread_coord.join()
  78 + self.event_coord.clear()
  79 +
65 80 Publisher.sendMessage('Update status text in GUI',
66 81 label=_("Tracker disconnected"))
67 82 print("Tracker disconnected!")
... ... @@ -81,7 +96,7 @@ class Tracker():
81 96 coord_samples = {}
82 97  
83 98 for i in range(n_samples):
84   - coord_raw = dco.GetCoordinates(self.trk_init, self.tracker_id, ref_mode_id)
  99 + coord_raw, markers_flag = self.TrackerCoordinates.GetCoordinates()
85 100  
86 101 if ref_mode_id == const.DYNAMIC_REF:
87 102 coord = dco.dynamic_reference_m(coord_raw[0, :], coord_raw[1, :])
... ...