Download as
Browse Code »

Commit 78997860df3ddd429558f605ab6090b3e940e8ad

Authored by Renan
1 parent 7a197822
Exists in master

FIX: merge adjustments

Showing 6 changed files with 173 additions and 28 deletions   Show diff stats
invesalius/data/coregistration.py
  Diff comments   View file @7899786
... ... @@ -20,11 +20,12 @@
20 20 import numpy as np
21 21 import queue
22 22 import threading
23   -from time import sleep
  23 +from time import sleep
24 24  
25 25 import invesalius.constants as const
26 26 import invesalius.data.transformations as tr
27 27 import invesalius.data.bases as bases
  28 +import invesalius.data.coordinates as dco
28 29  
29 30  
30 31 # TODO: Replace the use of degrees by radians in every part of the navigation pipeline
... ...
invesalius/gui/dialogs.py
  Diff comments   View file @7899786
... ... @@ -2920,11 +2920,6 @@ class FFillSegmentationOptionsDialog(wx.Dialog):
2920 2920 sizer.Add(0, 0, (14, 0))
2921 2921 except TypeError:
2922 2922 sizer.AddStretchSpacer((14, 0))
2923   - sizer.Add(self.close_btn, (15, 0), (1, 6), flag=wx.ALIGN_RIGHT|wx.RIGHT, border=5)
2924   - try:
2925   - sizer.Add(0, 0, (16, 0))
2926   - except TypeError:
2927   - sizer.AddStretchSpacer((16, 0))
2928 2923  
2929 2924 self.SetSizer(sizer)
2930 2925 sizer.Fit(self)
... ... @@ -4329,7 +4324,7 @@ class SetTrackerDevice2Robot(wx.Dialog):
4329 4324 def _init_gui(self):
4330 4325 # ComboBox for spatial tracker device selection
4331 4326 tooltip = wx.ToolTip(_("Choose the tracking device"))
4332   - tracker_options = [_("Select tracker:")] + const.TRACKERS[:-3]
  4327 + tracker_options = [_("Select tracker:")] + const.TRACKERS[:-1]
4333 4328 choice_trck = wx.ComboBox(self, -1, "",
4334 4329 choices=tracker_options, style=wx.CB_DROPDOWN | wx.CB_READONLY)
4335 4330 choice_trck.SetToolTip(tooltip)
... ...
invesalius/gui/task_navigator.py
  Diff comments   View file @7899786
... ... @@ -68,6 +68,8 @@ from invesalius.gui import utils as gui_utils
68 68 from invesalius.navigation.icp import ICP
69 69 from invesalius.navigation.navigation import Navigation
70 70 from invesalius.navigation.tracker import Tracker
  71 +from invesalius.navigation.robot import Robot
  72 +import invesalius.data.transformations as tr
71 73  
72 74 HAS_PEDAL_CONNECTION = True
73 75 try:
... ... @@ -647,6 +649,7 @@ class NeuronavigationPanel(wx.Panel):
647 649 self.robot.OnRobotConnection(self.tracker, self.robotcoordinates)
648 650 trk_init_robot = self.tracker.trk_init[1][0]
649 651 if trk_init_robot:
  652 + #todo: create a variable to stop thread
650 653 self.robot.StartRobotNavigation(self.tracker, self.robotcoordinates,
651 654 self.navigation.coord_queue)
652 655  
... ... @@ -684,9 +687,11 @@ class NeuronavigationPanel(wx.Panel):
684 687 colour = (0., 1., 0.)
685 688 size = 2
686 689 seed = 3 * [0.]
  690 + head = 6 * [0.]
  691 + robot = 6 * [0.]
687 692  
688 693 Publisher.sendMessage('Create marker', coord=coord, colour=colour, size=size,
689   - label=label, seed=seed)
  694 + label=label, seed=seed, head=head, robot=robot)
690 695 else:
691 696 for m in [0, 1, 2]:
692 697 self.numctrls_fiducial[n][m].SetValue(float(self.current_coord[m]))
... ... @@ -1119,6 +1124,18 @@ class MarkersPanel(wx.Panel):
1119 1124 x_seed : float = 0
1120 1125 y_seed : float = 0
1121 1126 z_seed : float = 0
  1127 + x_head : float = 0
  1128 + y_head : float = 0
  1129 + z_head : float = 0
  1130 + alpha_head : float = 0
  1131 + beta_head : float = 0
  1132 + gamma_head : float = 0
  1133 + x_robot : float = 0
  1134 + y_robot : float = 0
  1135 + z_robot : float = 0
  1136 + alpha_robot : float = 0
  1137 + beta_robot: float = 0
  1138 + gamma_robot : float = 0
1122 1139 is_target : int = 0 # is_target is int instead of boolean to avoid
1123 1140 # problems with CSV export
1124 1141  
... ... @@ -1149,6 +1166,24 @@ class MarkersPanel(wx.Panel):
1149 1166 def seed(self, new_seed):
1150 1167 self.x_seed, self.y_seed, self.z_seed = new_seed
1151 1168  
  1169 + # x_head, y_head, z_head, alpha_head, beta_head, gamma_head can be jointly accessed as robot
  1170 + @property
  1171 + def head(self):
  1172 + return list((self.x_head, self.y_head, self.z_head, self.alpha_head, self.beta_head, self.gamma_head),)
  1173 +
  1174 + @head.setter
  1175 + def head(self, new_head):
  1176 + self.x_head, self.y_head, self.z_head, self.alpha_head, self.beta_head, self.gamma_head = new_head
  1177 +
  1178 + # x_robot, y_robot, z_robot, alpha_robot, beta_robot, gamma_robot can be jointly accessed as robot
  1179 + @property
  1180 + def robot(self):
  1181 + return list((self.x_robot, self.y_robot, self.z_robot, self.alpha_robot, self.beta_robot, self.gamma_robot),)
  1182 +
  1183 + @robot.setter
  1184 + def robot(self, new_robot):
  1185 + self.x_robot, self.y_robot, self.z_robot, self.alpha_robot, self.beta_robot, self.gamma_robot = new_robot
  1186 +
1152 1187 @classmethod
1153 1188 def get_headers(cls):
1154 1189 """Return the list of field names (headers) for exporting to csv."""
... ... @@ -1288,7 +1323,7 @@ class MarkersPanel(wx.Panel):
1288 1323 Publisher.subscribe(self.UpdateSeedCoordinates, 'Update tracts')
1289 1324 Publisher.subscribe(self.UpdateMchange, 'Update matrix change')
1290 1325 Publisher.subscribe(self.UpdateMRef, 'Update ref matrix')
1291   - Publisher.subscribe(self.UpdateRawCoord, 'Update raw coord')
  1326 + Publisher.subscribe(self.UpdateRobotCoord, 'Update raw coord')
1292 1327 Publisher.subscribe(self.UpdateObjectMarker2Center, 'Update object marker to center')
1293 1328 Publisher.subscribe(self.OnObjectTarget, 'Coil at target')
1294 1329  
... ... @@ -1377,8 +1412,8 @@ class MarkersPanel(wx.Panel):
1377 1412 def UpdateMchange(self, mchange):
1378 1413 self.mchange = mchange
1379 1414  
1380   - def UpdateRawCoord(self, coord_raw, markers_flag):
1381   - self.current_ref = coord_raw[1]
  1415 + def UpdateRobotCoord(self, coord_raw, markers_flag):
  1416 + self.current_head = coord_raw[1]
1382 1417 self.current_robot = coord_raw[2]
1383 1418  
1384 1419 def UpdateObjectMarker2Center(self, s0_raw, t_offset):
... ... @@ -1452,16 +1487,13 @@ class MarkersPanel(wx.Panel):
1452 1487  
1453 1488 Publisher.sendMessage('Set new color', index=index, color=color_new)
1454 1489  
1455   - # def OnContinuousSendCoord(self, evt=None):
1456   - # self.timer.Start(30000)
1457   - #
1458   - # def OnUpdateSendCoord(self, evt):
1459   - # self.OnMenuSendCoord(evt=None)
1460   -
1461 1490 def OnMenuSendCoord(self, evt):
1462 1491 if isinstance(evt, int):
1463 1492 self.lc.Focus(evt)
1464   - coord = self.list_coord[self.lc.GetFocusedItem()]
  1493 +
  1494 + robot = self.markers[self.lc.GetFocusedItem()].robot
  1495 + head = self.markers[self.lc.GetFocusedItem()].head
  1496 + print(robot)
1465 1497 # coord_target = self.list_coord[3]
1466 1498 # coord_home = self.list_coord[4]
1467 1499 # if self.flag_target:
... ... @@ -1469,13 +1501,13 @@ class MarkersPanel(wx.Panel):
1469 1501 # else:
1470 1502 # coord = coord_target
1471 1503  
1472   - trans = tr.translation_matrix(coord[14:17])
1473   - a, b, g = np.radians(coord[17:20])
  1504 + trans = tr.translation_matrix(robot[:3])
  1505 + a, b, g = np.radians(robot[3:])
1474 1506 rot = tr.euler_matrix(a, b, g, 'rzyx')
1475 1507 m_robot_target = tr.concatenate_matrices(trans, rot)
1476 1508  
1477   - trans = tr.translation_matrix(coord[20:23])
1478   - a, b, g = np.radians(coord[23:26])
  1509 + trans = tr.translation_matrix(head[:3])
  1510 + a, b, g = np.radians(head[3:])
1479 1511 rot = tr.euler_matrix(a, b, g, 'rzyx')
1480 1512 m_ref_target = tr.concatenate_matrices(trans, rot)
1481 1513  
... ... @@ -1618,7 +1650,7 @@ class MarkersPanel(wx.Panel):
1618 1650 def OnSelectSize(self, evt, ctrl):
1619 1651 self.marker_size = ctrl.GetValue()
1620 1652  
1621   - def CreateMarker(self, coord=None, colour=None, size=None, label='*', is_target=0, seed=None, robot=None, ref=None):
  1653 + def CreateMarker(self, coord=None, colour=None, size=None, label='*', is_target=0, seed=None, head=None, robot=None):
1622 1654 new_marker = self.Marker()
1623 1655 new_marker.coord = coord or self.current_coord
1624 1656 new_marker.colour = colour or self.marker_colour
... ... @@ -1626,8 +1658,8 @@ class MarkersPanel(wx.Panel):
1626 1658 new_marker.label = label
1627 1659 new_marker.is_target = is_target
1628 1660 new_marker.seed = seed or self.current_seed
  1661 + new_marker.head = head or self.current_head
1629 1662 new_marker.robot = robot or self.current_robot
1630   - new_marker.ref = ref or self.current_ref
1631 1663  
1632 1664 # Note that ball_id is zero-based, so we assign it len(self.markers) before the new marker is added
1633 1665 Publisher.sendMessage('Add marker', ball_id=len(self.markers),
... ...
invesalius/navigation/navigation.py
  Diff comments   View file @7899786
... ... @@ -82,7 +82,7 @@ class UpdateNavigationScene(threading.Thread):
82 82  
83 83 threading.Thread.__init__(self, name='UpdateScene')
84 84 self.serial_port_enabled, self.view_tracts, self.peel_loaded = vis_components
85   - self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue = vis_queues
  85 + self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue, self.robottarget_queue = vis_queues
86 86 self.sle = sle
87 87 self.event = event
88 88  
... ... @@ -91,7 +91,7 @@ class UpdateNavigationScene(threading.Thread):
91 91 while not self.event.is_set():
92 92 got_coords = False
93 93 try:
94   - coord, m_img, view_obj = self.coord_queue.get_nowait()
  94 + coord, [coord_raw, markers_flag], m_img, view_obj = self.coord_queue.get_nowait()
95 95 got_coords = True
96 96  
97 97 # print('UpdateScene: get {}'.format(count))
... ... @@ -116,6 +116,7 @@ class UpdateNavigationScene(threading.Thread):
116 116 # see the red cross in the position of the offset marker
117 117 wx.CallAfter(Publisher.sendMessage, 'Update slices position', position=coord[:3])
118 118 wx.CallAfter(Publisher.sendMessage, 'Set cross focal point', position=coord)
  119 + wx.CallAfter(Publisher.sendMessage, 'Update raw coord', coord_raw=coord_raw, markers_flag=markers_flag)
119 120 wx.CallAfter(Publisher.sendMessage, 'Update slice viewer')
120 121  
121 122 if view_obj:
... ... @@ -146,6 +147,8 @@ class Navigation():
146 147 self.event = threading.Event()
147 148 self.coord_queue = QueueCustom(maxsize=1)
148 149 self.icp_queue = QueueCustom(maxsize=1)
  150 + self.objattarget_queue = QueueCustom(maxsize=1)
  151 + self.robottarget_queue = QueueCustom(maxsize=1)
149 152 # self.visualization_queue = QueueCustom(maxsize=1)
150 153 self.serial_port_queue = QueueCustom(maxsize=1)
151 154 self.coord_tracts_queue = QueueCustom(maxsize=1)
... ... @@ -228,7 +231,7 @@ class Navigation():
228 231 self.event.clear()
229 232  
230 233 vis_components = [self.SerialPortEnabled(), self.view_tracts, self.peel_loaded]
231   - vis_queues = [self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue]
  234 + vis_queues = [self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue, self.robottarget_queue]
232 235  
233 236 Publisher.sendMessage("Navigation status", nav_status=True, vis_status=vis_components)
234 237  
... ... @@ -262,7 +265,7 @@ class Navigation():
262 265 obj_data = db.object_registration(obj_fiducials, obj_orients, coord_raw, m_change)
263 266 coreg_data.extend(obj_data)
264 267  
265   - queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue]
  268 + queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue, self.objattarget_queue]
266 269 jobs_list.append(dcr.CoordinateCorregistrate(self.ref_mode_id, tracker, coreg_data,
267 270 self.view_tracts, queues,
268 271 self.event, self.sleep_nav, tracker.tracker_id,
... ...
invesalius/navigation/robot.py 0 → 100644
  Diff comments   View file @7899786
... ... @@ -0,0 +1,97 @@
  1 +#--------------------------------------------------------------------------
  2 +# Software: InVesalius - Software de Reconstrucao 3D de Imagens Medicas
  3 +# Copyright: (C) 2001 Centro de Pesquisas Renato Archer
  4 +# Homepage: http://www.softwarepublico.gov.br
  5 +# Contact: invesalius@cti.gov.br
  6 +# License: GNU - GPL 2 (LICENSE.txt/LICENCA.txt)
  7 +#--------------------------------------------------------------------------
  8 +# Este programa e software livre; voce pode redistribui-lo e/ou
  9 +# modifica-lo sob os termos da Licenca Publica Geral GNU, conforme
  10 +# publicada pela Free Software Foundation; de acordo com a versao 2
  11 +# da Licenca.
  12 +#
  13 +# Este programa eh distribuido na expectativa de ser util, mas SEM
  14 +# QUALQUER GARANTIA; sem mesmo a garantia implicita de
  15 +# COMERCIALIZACAO ou de ADEQUACAO A QUALQUER PROPOSITO EM
  16 +# PARTICULAR. Consulte a Licenca Publica Geral GNU para obter mais
  17 +# detalhes.
  18 +#--------------------------------------------------------------------------
  19 +import wx
  20 +import queue
  21 +
  22 +import invesalius.data.bases as db
  23 +import invesalius.gui.dialogs as dlg
  24 +from invesalius.pubsub import pub as Publisher
  25 +
  26 +try:
  27 + import invesalius.data.elfin as elfin
  28 + import invesalius.data.elfin_robot as elfin_process
  29 + has_robot = True
  30 +except ImportError:
  31 + has_robot = False
  32 +
  33 +class Robot():
  34 + def __init__(self):
  35 + self.trk_init = None
  36 + self.robottarget_queue = None
  37 + self.objattarget_queue = None
  38 + self.process_tracker = None
  39 +
  40 + self.__bind_events()
  41 +
  42 + def __bind_events(self):
  43 + Publisher.subscribe(self.OnSendCoordinates, 'Send coord to robot')
  44 + Publisher.subscribe(self.OnUpdateRobotTargetMatrix, 'Robot target matrix')
  45 + Publisher.subscribe(self.OnObjectTarget, 'Coil at target')
  46 +
  47 + def OnRobotConnection(self, tracker, robotcoordinates):
  48 + if not tracker.trk_init[0][0] or not tracker.trk_init[1][0]:
  49 + dlg.ShowNavigationTrackerWarning(tracker.tracker_id, tracker.trk_init[1])
  50 + tracker.tracker_id = 0
  51 + tracker.tracker_connected = False
  52 + else:
  53 + tracker.trk_init.append(robotcoordinates)
  54 + self.process_tracker = elfin_process.TrackerProcessing()
  55 + dlg_correg_robot = dlg.CreateTransformationMatrixRobot(tracker)
  56 + if dlg_correg_robot.ShowModal() == wx.ID_OK:
  57 + M_tracker_2_robot = dlg_correg_robot.GetValue()
  58 + db.transform_tracker_2_robot.M_tracker_2_robot = M_tracker_2_robot
  59 + self.robot_server = tracker.trk_init[1][0]
  60 + self.trk_init = tracker.trk_init
  61 + else:
  62 + dlg.ShowNavigationTrackerWarning(tracker.tracker_id, 'disconnect')
  63 + tracker.trk_init = None
  64 + tracker.tracker_id = 0
  65 + tracker.tracker_connected = False
  66 +
  67 + # Publisher.sendMessage('Update tracker initializer',
  68 + # nav_prop=(tracker.tracker_id, tracker.trk_init, tracker.TrackerCoordinates, tracker.GetReferenceMode()))
  69 +
  70 + def StartRobotNavigation(self, tracker, robotcoordinates, coord_queue):
  71 + if tracker.event_robot.is_set():
  72 + tracker.event_robot.clear()
  73 + elfin_process.ControlRobot(self.trk_init, tracker, robotcoordinates,
  74 + [coord_queue, self.robottarget_queue,
  75 + self.objattarget_queue],
  76 + self.process_tracker, tracker.event_robot).start()
  77 +
  78 + def OnSendCoordinates(self, coord):
  79 + self.robot_server.SendCoordinates(coord)
  80 +
  81 + def OnUpdateRobotTargetMatrix(self, robot_tracker_flag, m_change_robot2ref):
  82 + try:
  83 + self.robottarget_queue.put_nowait([robot_tracker_flag, m_change_robot2ref])
  84 + except queue.Full:
  85 + print('full target')
  86 + pass
  87 +
  88 + def OnObjectTarget(self, state):
  89 + try:
  90 + if self.objattarget_queue:
  91 + self.objattarget_queue.put_nowait(state)
  92 + except queue.Full:
  93 + #print('full flag target')
  94 + pass
  95 +
  96 + def SetRobotQueues(self, queues):
  97 + self.robottarget_queue, self.objattarget_queue = queues
0 98 \ No newline at end of file
... ...
invesalius/navigation/tracker.py
  Diff comments   View file @7899786
... ... @@ -24,6 +24,7 @@ import invesalius.constants as const
24 24 import invesalius.data.coordinates as dco
25 25 import invesalius.data.trackers as dt
26 26 import invesalius.gui.dialogs as dlg
  27 +import invesalius.data.coregistration as dcr
27 28 from invesalius.pubsub import pub as Publisher
28 29  
29 30  
... ... @@ -34,12 +35,14 @@ class Tracker():
34 35  
35 36 self.tracker_fiducials = np.full([3, 3], np.nan)
36 37 self.tracker_fiducials_raw = np.zeros((6, 6))
  38 + self.m_tracker_fiducials_raw = np.zeros((6, 4, 4))
37 39  
38 40 self.tracker_connected = False
39 41  
40 42 self.thread_coord = None
41 43  
42 44 self.event_coord = threading.Event()
  45 + self.event_robot = threading.Event()
43 46  
44 47 self.TrackerCoordinates = dco.TrackerCoordinates()
45 48  
... ... @@ -74,8 +77,12 @@ class Tracker():
74 77  
75 78 if self.thread_coord:
76 79 self.event_coord.set()
  80 + self.event_robot.set()
77 81 self.thread_coord.join()
  82 + #get the robot thread??
78 83 self.event_coord.clear()
  84 + self.event_robot.clear()
  85 +
79 86  
80 87 Publisher.sendMessage('Update status text in GUI',
81 88 label=_("Tracker disconnected"))
... ... @@ -126,6 +133,9 @@ class Tracker():
126 133 self.tracker_fiducials_raw[2 * fiducial_index, :] = coord_raw[0, :]
127 134 self.tracker_fiducials_raw[2 * fiducial_index + 1, :] = coord_raw[1, :]
128 135  
  136 + self.m_tracker_fiducials_raw[2 * fiducial_index, :] = dcr.compute_marker_transformation(coord_raw, 0)
  137 + self.m_tracker_fiducials_raw[2 * fiducial_index + 1, :] = dcr.compute_marker_transformation(coord_raw, 1)
  138 +
129 139 print("Set tracker fiducial {} to coordinates {}.".format(fiducial_index, coord[0:3]))
130 140  
131 141 def ResetTrackerFiducials(self):
... ... @@ -135,6 +145,13 @@ class Tracker():
135 145 def GetTrackerFiducials(self):
136 146 return self.tracker_fiducials, self.tracker_fiducials_raw
137 147  
  148 + def GetMatrixTrackerFiducials(self):
  149 + m_probe_ref_left = np.linalg.inv(self.m_tracker_fiducials_raw[1]) @ self.m_tracker_fiducials_raw[0]
  150 + m_probe_ref_right = np.linalg.inv(self.m_tracker_fiducials_raw[3]) @ self.m_tracker_fiducials_raw[2]
  151 + m_probe_ref_nasion = np.linalg.inv(self.m_tracker_fiducials_raw[5]) @ self.m_tracker_fiducials_raw[4]
  152 +
  153 + return [m_probe_ref_left, m_probe_ref_right, m_probe_ref_nasion]
  154 +
138 155 def GetTrackerInfo(self):
139 156 return self.trk_init, self.tracker_id
140 157  
... ...