Compare View

switch
from
...
to
 
Commits (3)
  • 3a9ab9cd   Decouple robot control (#404) ... Browse Code » 
    * INIT testing..

* REM: Decouple robot

* FIX: trck_init gets the first value

* INIT testing..

* REM: Decouple robot

* FIX: trck_init gets the first value

* FIX: append tracker init robot

* FIX: transform robot coordinates to tracker coordinates system

* ENH: robot matrix estimation outside invesalius

* ENH: remove robot coordinates and target matrix from invesalius

* FIX: disconnected

* FIX: tracker connection

* ENH: Cleaning

* CLN: remove unused imports

* ENH: checks with target angle is zero/none to enable send target to robot

* more cleaning

* FIX: destroys robot dialog if not possible to connect to the robot

* ENH: deal with robot resetting

* ADD: function to estimate transformation matrix

-navigation starts after ICP box

* FIX: deals with tracker devices not connected

* FIX: transformation matrix to list

* ADD: send target to robot from image

-need tests

* FIX: image_to_tracker transformation

* ENH: apply icp matrix to target

* ENH: Show robot menu item only with tracker is robot

* FIX: typo and comments

* FIX: exception for BadNamespaceError

* ADD: set metaclass to navigation and icp

-allows access for plugins

* CLEAN: remove unused imports
    Renan
     
  • df0e6ca1   Added requirements to apple silicon Browse Code »
    Thiago Franco de Moraes
     
  • e971a699   ADD: Send coil mesh using NeuronavigationApi (#418) ... Browse Code » 
    * ADD: Send coil mesh using NeuronavigationApi

* Review comments, some tweaks to save memory and speed up
    Olli-Pekka Kahilakoski
     
Showing 16 changed files   Show diff stats
invesalius/constants.py
  Diff comments   View file @e971a69
... ... @@ -832,7 +832,6 @@ SEED_RADIUS = 1.5
832 832 # Increased the default sleep parameter from 0.1 to 0.15 to decrease CPU load during navigation.
833 833 SLEEP_NAVIGATION = 0.2
834 834 SLEEP_COORDINATES = 0.05
835   -SLEEP_ROBOT = 0.01
836 835  
837 836 BRAIN_OPACITY = 0.6
838 837 N_CPU = psutil.cpu_count()
... ... @@ -864,14 +863,3 @@ PULSE_DURATION_IN_MILLISECONDS = 0.2
864 863  
865 864 #Robot
866 865 ROBOT_ElFIN_IP = ['143.107.220.251', '169.254.153.251', '127.0.0.1']
867   -ROBOT_ElFIN_PORT = 10003
868   -ROBOT_MOTIONS = {"normal": 0, "linear out": 1, "arc": 2}
869   -ROBOT_HEAD_VELOCITY_THRESHOLD = 10 #mm/s
870   -ROBOT_ARC_THRESHOLD_DISTANCE = 100 #mm
871   -ROBOT_VERSOR_SCALE_FACTOR = 70
872   -#Robot Working Space is defined as 800mm in Elfin manual. For safety, the value is reduced by 5%.
873   -ROBOT_WORKING_SPACE = 760 #mm
874   -ROBOT_MOVE_STATE = {"free to move": 0,
875   - "in motion": 1009,
876   - "waiting for execution": 1013,
877   - "error": 1025}
... ...
invesalius/data/bases.py
  Diff comments   View file @e971a69
... ... @@ -163,6 +163,12 @@ def transform_icp(m_img, m_icp):
163 163  
164 164 return m_img
165 165  
  166 +def inverse_transform_icp(m_img, m_icp):
  167 + coord_img = [m_img[0, -1], -m_img[1, -1], m_img[2, -1], 1]
  168 + m_img[0, -1], m_img[1, -1], m_img[2, -1], _ = np.linalg.inv(m_icp) @ coord_img
  169 + m_img[0, -1], m_img[1, -1], m_img[2, -1] = m_img[0, -1], -m_img[1, -1], m_img[2, -1]
  170 +
  171 + return m_img
166 172  
167 173 def object_registration(fiducials, orients, coord_raw, m_change):
168 174 """
... ... @@ -244,47 +250,3 @@ def object_registration(fiducials, orients, coord_raw, m_change):
244 250 )
245 251  
246 252 return t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img
247   -
248   -def compute_robot_to_head_matrix(raw_target_robot):
249   - """
250   - :param head: nx6 array of head coordinates from tracking device in robot space
251   - :param robot: nx6 array of robot coordinates
252   -
253   - :return: target_robot_matrix: 3x3 array representing change of basis from robot to head in robot system
254   - """
255   - head, robot = raw_target_robot
256   - # compute head target matrix
257   - m_head_target = dco.coordinates_to_transformation_matrix(
258   - position=head[:3],
259   - orientation=head[3:],
260   - axes='rzyx',
261   - )
262   -
263   - # compute robot target matrix
264   - m_robot_target = dco.coordinates_to_transformation_matrix(
265   - position=robot[:3],
266   - orientation=robot[3:],
267   - axes='rzyx',
268   - )
269   - robot_to_head_matrix = np.linalg.inv(m_head_target) @ m_robot_target
270   -
271   - return robot_to_head_matrix
272   -
273   -
274   -class transform_tracker_to_robot(object):
275   - M_tracker_to_robot = np.array([])
276   - def transformation_tracker_to_robot(self, tracker_coord):
277   - if not transform_tracker_to_robot.M_tracker_to_robot.any():
278   - return None
279   -
280   - M_tracker = dco.coordinates_to_transformation_matrix(
281   - position=tracker_coord[:3],
282   - orientation=tracker_coord[3:6],
283   - axes='rzyx',
284   - )
285   - M_tracker_in_robot = transform_tracker_to_robot.M_tracker_to_robot @ M_tracker
286   -
287   - translation, angles_as_deg = dco.transformation_matrix_to_coordinates(M_tracker_in_robot, axes='rzyx')
288   - tracker_in_robot = list(translation) + list(angles_as_deg)
289   -
290   - return tracker_in_robot
... ...
invesalius/data/coordinates.py
  Diff comments   View file @e971a69
... ... @@ -22,7 +22,6 @@ import numpy as np
22 22 import threading
23 23 import wx
24 24  
25   -import invesalius.data.bases as db
26 25 import invesalius.data.transformations as tr
27 26 import invesalius.constants as const
28 27  
... ... @@ -47,14 +46,20 @@ class TrackerCoordinates():
47 46 def SetCoordinates(self, coord, markers_flag):
48 47 self.coord = coord
49 48 self.markers_flag = markers_flag
50   - if self.previous_markers_flag != self.markers_flag and not self.nav_status:
51   - wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag)
52   - self.previous_markers_flag = self.markers_flag
  49 + if not self.nav_status:
  50 + wx.CallAfter(Publisher.sendMessage, 'Update tracker coordinates',
  51 + coord=self.coord.tolist(), markers_flag=self.markers_flag)
  52 + if self.previous_markers_flag != self.markers_flag:
  53 + wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag)
  54 + self.previous_markers_flag = self.markers_flag
53 55  
54 56 def GetCoordinates(self):
55   - if self.previous_markers_flag != self.markers_flag and self.nav_status:
56   - wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag)
57   - self.previous_markers_flag = self.markers_flag
  57 + if self.nav_status:
  58 + wx.CallAfter(Publisher.sendMessage, 'Update tracker coordinates',
  59 + coord=self.coord.tolist(), markers_flag=self.markers_flag)
  60 + if self.previous_markers_flag != self.markers_flag:
  61 + wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag)
  62 + self.previous_markers_flag = self.markers_flag
58 63  
59 64 return self.coord, self.markers_flag
60 65  
... ... @@ -195,16 +200,6 @@ def PolarisCoord(trck_init, trck_id, ref_mode):
195 200  
196 201 return coord, [trck.probeID, trck.refID, trck.objID]
197 202  
198   -def ElfinCoord(trck_init):
199   - if len(trck_init) > 2:
200   - robotcoordinates = trck_init[-1]
201   - coord = robotcoordinates.GetRobotCoordinates()
202   - if coord is None:
203   - coord = np.array([0, 0, 0, 0, 0, 0])
204   - else:
205   - coord = np.array([0, 0, 0, 0, 0, 0])
206   -
207   - return coord
208 203  
209 204 def CameraCoord(trck_init, trck_id, ref_mode):
210 205 trck = trck_init[0]
... ... @@ -346,6 +341,13 @@ def PolhemusSerialCoord(trck_init, trck_id, ref_mode):
346 341  
347 342 return coord
348 343  
  344 +def RobotCoord(trk_init, trck_id, ref_mode):
  345 + #trck_id is the tracker related to the robot ID. To get the tracker ID, combined with the robot,
  346 + # it is required to get trk_init[1]
  347 + tracker_id = trk_init[1]
  348 + coord_tracker, markers_flag = GetCoordinatesForThread(trk_init[0], tracker_id, ref_mode)
  349 +
  350 + return np.vstack([coord_tracker[0], coord_tracker[1], coord_tracker[2]]), markers_flag
349 351  
350 352 def DebugCoordRandom(trk_init, trck_id, ref_mode):
351 353 """
... ... @@ -497,22 +499,6 @@ def dynamic_reference_m(probe, reference):
497 499 return coord_rot
498 500  
499 501  
500   -def RobotCoord(trk_init, trck_id, ref_mode):
501   - coord_tracker, markers_flag = GetCoordinatesForThread(trk_init[0][0], trk_init[1], ref_mode)
502   - coord_robot = ElfinCoord([trk_init[0][1]]+trk_init[1:])
503   -
504   - probe_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[0])
505   - ref_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[1])
506   - obj_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[2])
507   -
508   - if probe_tracker_in_robot is None:
509   - probe_tracker_in_robot = coord_tracker[0]
510   - ref_tracker_in_robot = coord_tracker[1]
511   - obj_tracker_in_robot = coord_tracker[2]
512   -
513   - return np.vstack([probe_tracker_in_robot, ref_tracker_in_robot, coord_robot, obj_tracker_in_robot]), markers_flag
514   -
515   -
516 502 def dynamic_reference_m2(probe, reference):
517 503 """
518 504 Apply dynamic reference correction to probe coordinates. Uses the alpha, beta and gama
... ... @@ -590,4 +576,4 @@ class ReceiveCoordinates(threading.Thread):
590 576 while not self.event.is_set():
591 577 coord_raw, markers_flag = GetCoordinatesForThread(self.trck_init, self.trck_id, const.DEFAULT_REF_MODE)
592 578 self.TrackerCoordinates.SetCoordinates(coord_raw, markers_flag)
593   - sleep(const.SLEEP_COORDINATES)
594 579 \ No newline at end of file
  580 + sleep(const.SLEEP_COORDINATES)
... ...
invesalius/data/coregistration.py
  Diff comments   View file @e971a69
... ... @@ -101,6 +101,32 @@ def tracker_to_image(m_change, m_probe_ref, r_obj_img, m_obj_raw, s0_dyn):
101 101 m_img[:3, :3] = r_obj[:3, :3]
102 102 return m_img
103 103  
  104 +def image_to_tracker(m_change, target, icp):
  105 + """Compute the transformation matrix to the tracker coordinate system
  106 +
  107 + :param m_change: Corregistration transformation obtained from fiducials
  108 + :type m_change: numpy.ndarray
  109 + :param target: Target in invesalius coordinate system
  110 + :type target: numpy.ndarray
  111 + :param icp: ICP transformation matrix
  112 + :type icp: numpy.ndarray
  113 +
  114 + :return: 4 x 4 numpy double array
  115 + :rtype: numpy.ndarray
  116 + """
  117 + m_target_in_image = dco.coordinates_to_transformation_matrix(
  118 + position=target[:3],
  119 + orientation=[0, 0, 0],
  120 + axes='sxyz',
  121 + )
  122 + if icp.use_icp:
  123 + m_target_in_image = bases.inverse_transform_icp(m_target_in_image, icp.m_icp)
  124 + m_target_in_tracker = np.linalg.inv(m_change) @ m_target_in_image
  125 +
  126 + # invert y coordinate
  127 + m_target_in_tracker[2, -1] = -m_target_in_tracker[2, -1]
  128 +
  129 + return m_target_in_tracker
104 130  
105 131 def corregistrate_object_dynamic(inp, coord_raw, ref_mode_id, icp):
106 132  
... ... @@ -193,7 +219,6 @@ class CoordinateCorregistrate(threading.Thread):
193 219 self.object_at_target_queue = queues[3]
194 220 self.icp = None
195 221 self.m_icp = None
196   - self.robot_tracker_flag = None
197 222 self.last_coord = None
198 223 self.tracker_id = tracker_id
199 224 self.target = target
... ... @@ -254,7 +279,7 @@ class CoordinateCorregistrate(threading.Thread):
254 279 if self.icp:
255 280 m_img = bases.transform_icp(m_img, self.m_icp)
256 281  
257   - self.coord_queue.put_nowait([coord, [coord_raw, markers_flag], m_img, view_obj])
  282 + self.coord_queue.put_nowait([coord, markers_flag, m_img, view_obj])
258 283 # print('CoordCoreg: put {}'.format(count))
259 284 # count += 1
260 285  
... ... @@ -307,7 +332,7 @@ class CoordinateCorregistrateNoObject(threading.Thread):
307 332 if self.icp:
308 333 m_img = bases.transform_icp(m_img, self.m_icp)
309 334  
310   - self.coord_queue.put_nowait([coord, [coord_raw, markers_flag], m_img, view_obj])
  335 + self.coord_queue.put_nowait([coord, markers_flag, m_img, view_obj])
311 336  
312 337 if self.view_tracts:
313 338 self.coord_tracts_queue.put_nowait(m_img_flip)
... ...
invesalius/data/trackers.py
  Diff comments   View file @e971a69
... ... @@ -18,6 +18,7 @@
18 18 #--------------------------------------------------------------------------
19 19 import invesalius.constants as const
20 20 import invesalius.gui.dialogs as dlg
  21 +from invesalius.pubsub import pub as Publisher
21 22 # TODO: Disconnect tracker when a new one is connected
22 23 # TODO: Test if there are too many prints when connection fails
23 24 # TODO: Redesign error messages. No point in having "Could not connect to default tracker" in all trackers
... ... @@ -223,42 +224,23 @@ def OptitrackTracker(tracker_id):
223 224 print('#####')
224 225 return trck_init, lib_mode
225 226  
226   -def ElfinRobot(robot_IP):
227   - trck_init = None
228   - try:
229   - import invesalius.data.elfin as elfin
230   - print("Trying to connect Robot via: ", robot_IP)
231   - trck_init = elfin.Elfin_Server(robot_IP, const.ROBOT_ElFIN_PORT)
232   - trck_init.Initialize()
233   - lib_mode = 'wrapper'
234   - print('Connect to elfin robot tracking device.')
235   -
236   - except:
237   - lib_mode = 'error'
238   - trck_init = None
239   - print('Could not connect to elfin robot tracker.')
240   -
241   - # return tracker initialization variable and type of connection
242   - return trck_init, lib_mode
243   -
244 227 def RobotTracker(tracker_id):
245 228 from wx import ID_OK
246   - trck_init = None
247   - trck_init_robot = None
248   - tracker_id = None
  229 +
249 230 dlg_device = dlg.SetTrackerDeviceToRobot()
250 231 if dlg_device.ShowModal() == ID_OK:
251 232 tracker_id = dlg_device.GetValue()
252 233 if tracker_id:
253   - trck_connection = TrackerConnection(tracker_id, None, 'connect')
254   - if trck_connection[0]:
  234 + trck_init = TrackerConnection(tracker_id, None, 'connect')
  235 + if trck_init[0]:
255 236 dlg_ip = dlg.SetRobotIP()
256 237 if dlg_ip.ShowModal() == ID_OK:
257 238 robot_IP = dlg_ip.GetValue()
258   - trck_init = trck_connection
259   - trck_init_robot = ElfinRobot(robot_IP)
  239 + Publisher.sendMessage('Connect to robot', robot_IP=robot_IP)
  240 +
  241 + return trck_init, tracker_id
260 242  
261   - return [(trck_init, trck_init_robot), tracker_id]
  243 + return None, None
262 244  
263 245 def DebugTrackerRandom(tracker_id):
264 246 trck_init = True
... ... @@ -392,8 +374,9 @@ def DisconnectTracker(tracker_id, trck_init):
392 374 lib_mode = 'serial'
393 375 print('Tracker disconnected.')
394 376 elif tracker_id == const.ROBOT:
395   - trck_init[0][0].Close()
396   - trck_init[1][0].Close()
  377 + Publisher.sendMessage('Reset robot', data=None)
  378 + if trck_init[1] == const.DEBUGTRACKRANDOM or trck_init[1] == const.DEBUGTRACKAPPROACH:
  379 + trck_init[0].Close()
397 380 trck_init = False
398 381 lib_mode = 'wrapper'
399 382 print('Tracker disconnected.')
... ...
invesalius/gui/dialogs.py
  Diff comments   View file @e971a69
... ... @@ -57,8 +57,6 @@ except ImportError:
57 57 from wx import AboutDialogInfo, AboutBox
58 58  
59 59 import invesalius.constants as const
60   -import invesalius.data.coordinates as dco
61   -import invesalius.data.transformations as tr
62 60 import invesalius.gui.widgets.gradient as grad
63 61 import invesalius.session as ses
64 62 import invesalius.utils as utils
... ... @@ -3597,11 +3595,7 @@ class ObjectCalibrationDialog(wx.Dialog):
3597 3595 # and not change the function to the point of potentially breaking it.)
3598 3596 #
3599 3597 if self.obj_ref_id and fiducial_index == 4:
3600   - if self.tracker_id == const.ROBOT:
3601   - trck_init_robot = self.trk_init[1][0]
3602   - coord = trck_init_robot.Run()
3603   - else:
3604   - coord = coord_raw[self.obj_ref_id, :]
  3598 + coord = coord_raw[self.obj_ref_id, :]
3605 3599 else:
3606 3600 coord = coord_raw[0, :]
3607 3601  
... ... @@ -4323,7 +4317,7 @@ class SetTrackerDeviceToRobot(wx.Dialog):
4323 4317 def _init_gui(self):
4324 4318 # ComboBox for spatial tracker device selection
4325 4319 tooltip = wx.ToolTip(_("Choose the tracking device"))
4326   - trackers = const.TRACKERS
  4320 + trackers = const.TRACKERS.copy()
4327 4321 if not ses.Session().debug:
4328 4322 del trackers[-3:]
4329 4323 tracker_options = [_("Select tracker:")] + trackers
... ... @@ -4425,10 +4419,8 @@ class CreateTransformationMatrixRobot(wx.Dialog):
4425 4419 M_robot_2_tracker is created by an affine transformation. Robot TCP should be calibrated to the center of the tracker marker
4426 4420 '''
4427 4421 #TODO: make aboutbox
4428   - self.tracker_coord = []
4429   - self.tracker_angles = []
4430   - self.robot_coord = []
4431   - self.robot_angles = []
  4422 + self.matrix_tracker_to_robot = []
  4423 + self.robot_status = False
4432 4424  
4433 4425 self.tracker = tracker
4434 4426  
... ... @@ -4470,6 +4462,8 @@ class CreateTransformationMatrixRobot(wx.Dialog):
4470 4462  
4471 4463 btn_load = wx.Button(self, -1, label=_('Load'), size=wx.Size(65, 23))
4472 4464 btn_load.Bind(wx.EVT_BUTTON, self.OnLoadReg)
  4465 + btn_load.Enable(False)
  4466 + self.btn_load = btn_load
4473 4467  
4474 4468 # Create a horizontal sizers
4475 4469 border = 1
... ... @@ -4524,16 +4518,17 @@ class CreateTransformationMatrixRobot(wx.Dialog):
4524 4518 main_sizer.Fit(self)
4525 4519  
4526 4520 self.CenterOnParent()
  4521 + self.__bind_events()
4527 4522  
4528   - def affine_correg(self, tracker, robot):
4529   - m_change = tr.affine_matrix_from_points(robot[:].T, tracker[:].T,
4530   - shear=False, scale=False, usesvd=False)
4531   - return m_change
  4523 + def __bind_events(self):
  4524 + Publisher.subscribe(self.OnUpdateTransformationMatrix, 'Update robot transformation matrix')
  4525 + Publisher.subscribe(self.OnCoordinatesAdquired, 'Coordinates for the robot transformation matrix collected')
  4526 + Publisher.subscribe(self.OnRobotConnectionStatus, 'Robot connection status')
4532 4527  
4533 4528 def OnContinuousAcquisition(self, evt=None, btn=None):
4534 4529 value = btn.GetValue()
4535 4530 if value:
4536   - self.timer.Start(500)
  4531 + self.timer.Start(100)
4537 4532 else:
4538 4533 self.timer.Stop()
4539 4534  
... ... @@ -4541,56 +4536,50 @@ class CreateTransformationMatrixRobot(wx.Dialog):
4541 4536 self.OnCreatePoint(evt=None)
4542 4537  
4543 4538 def OnCreatePoint(self, evt):
4544   - coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates()
4545   - #robot thread is not initialized yet
4546   - coord_raw_robot = self.tracker.trk_init[0][1][0].Run()
4547   - coord_raw_tracker_obj = coord_raw[3]
4548   -
4549   - if markers_flag[2]:
4550   - self.tracker_coord.append(coord_raw_tracker_obj[:3])
4551   - self.tracker_angles.append(coord_raw_tracker_obj[3:])
4552   - self.robot_coord.append(coord_raw_robot[:3])
4553   - self.robot_angles.append(coord_raw_robot[3:])
4554   - self.txt_number.SetLabel(str(int(self.txt_number.GetLabel())+1))
4555   - else:
4556   - print('Cannot detect the coil markers, pls try again')
  4539 + Publisher.sendMessage('Collect coordinates for the robot transformation matrix', data=None)
  4540 +
  4541 + def OnCoordinatesAdquired(self):
  4542 + self.txt_number.SetLabel(str(int(self.txt_number.GetLabel())+1))
4557 4543  
4558   - if len(self.tracker_coord) >= 3:
  4544 + if self.robot_status and int(self.txt_number.GetLabel()) >= 3:
4559 4545 self.btn_apply_reg.Enable(True)
4560 4546  
  4547 + def OnRobotConnectionStatus(self, data):
  4548 + self.robot_status = data
  4549 + if self.robot_status:
  4550 + self.btn_load.Enable(True)
  4551 + if int(self.txt_number.GetLabel()) >= 3:
  4552 + self.btn_apply_reg.Enable(True)
  4553 +
4561 4554 def OnReset(self, evt):
  4555 + Publisher.sendMessage('Reset coordinates collection for the robot transformation matrix', data=None)
4562 4556 if self.btn_cont_point:
4563 4557 self.btn_cont_point.SetValue(False)
4564 4558 self.OnContinuousAcquisition(evt=None, btn=self.btn_cont_point)
4565 4559  
4566   - self.tracker_coord = []
4567   - self.tracker_angles = []
4568   - self.robot_coord = []
4569   - self.robot_angles = []
4570   - self.M_tracker_2_robot = []
4571 4560 self.txt_number.SetLabel('0')
4572 4561  
4573 4562 self.btn_apply_reg.Enable(False)
4574 4563 self.btn_save.Enable(False)
4575 4564 self.btn_ok.Enable(False)
4576 4565  
  4566 + self.matrix_tracker_to_robot = []
  4567 +
4577 4568 def OnApply(self, evt):
4578 4569 if self.btn_cont_point:
4579 4570 self.btn_cont_point.SetValue(False)
4580 4571 self.OnContinuousAcquisition(evt=None, btn=self.btn_cont_point)
4581 4572  
4582   - tracker_coord = np.array(self.tracker_coord)
4583   - robot_coord = np.array(self.robot_coord)
4584   -
4585   - M_robot_2_tracker = self.affine_correg(tracker_coord, robot_coord)
4586   - M_tracker_2_robot = tr.inverse_matrix(M_robot_2_tracker)
4587   - self.M_tracker_2_robot = M_tracker_2_robot
  4573 + Publisher.sendMessage('Robot transformation matrix estimation', data=None)
4588 4574  
4589 4575 self.btn_save.Enable(True)
4590 4576 self.btn_ok.Enable(True)
4591 4577  
4592 4578 #TODO: make a colored circle to sinalize that the transformation was made (green) (red if not)
4593 4579  
  4580 + def OnUpdateTransformationMatrix(self, data):
  4581 + self.matrix_tracker_to_robot = np.array(data)
  4582 +
4594 4583 def OnSaveReg(self, evt):
4595 4584 filename = ShowLoadSaveDialog(message=_(u"Save robot transformation file as..."),
4596 4585 wildcard=_("Robot transformation files (*.rbtf)|*.rbtf"),
... ... @@ -4598,10 +4587,10 @@ class CreateTransformationMatrixRobot(wx.Dialog):
4598 4587 default_filename="robottransform.rbtf", save_ext="rbtf")
4599 4588  
4600 4589 if filename:
4601   - if self.M_tracker_2_robot is not None:
  4590 + if self.matrix_tracker_to_robot is not None:
4602 4591 with open(filename, 'w', newline='') as file:
4603 4592 writer = csv.writer(file, delimiter='\t')
4604   - writer.writerows(self.M_tracker_2_robot)
  4593 + writer.writerows(np.vstack(self.matrix_tracker_to_robot).tolist())
4605 4594  
4606 4595 def OnLoadReg(self, evt):
4607 4596 filename = ShowLoadSaveDialog(message=_(u"Load robot transformation"),
... ... @@ -4611,12 +4600,12 @@ class CreateTransformationMatrixRobot(wx.Dialog):
4611 4600 reader = csv.reader(file, delimiter='\t')
4612 4601 content = [row for row in reader]
4613 4602  
4614   - self.M_tracker_2_robot = np.vstack(list(np.float_(content)))
4615   - print("Matrix tracker to robot:", self.M_tracker_2_robot)
4616   - self.btn_ok.Enable(True)
  4603 + self.matrix_tracker_to_robot = np.vstack(list(np.float_(content)))
  4604 + print("Matrix tracker to robot:", self.matrix_tracker_to_robot)
  4605 + Publisher.sendMessage('Load robot transformation matrix', data=self.matrix_tracker_to_robot.tolist())
  4606 + if self.robot_status:
  4607 + self.btn_ok.Enable(True)
4617 4608  
4618   - def GetValue(self):
4619   - return self.M_tracker_2_robot
4620 4609  
4621 4610 class SetNDIconfigs(wx.Dialog):
4622 4611 def __init__(self, title=_("Setting NDI polaris configs:")):
... ...
invesalius/gui/task_navigator.py
  Diff comments   View file @e971a69
... ... @@ -20,10 +20,7 @@
20 20 import dataclasses
21 21 from functools import partial
22 22 import itertools
23   -import csv
24   -import queue
25 23 import time
26   -import threading
27 24  
28 25 import nibabel as nb
29 26 import numpy as np
... ... @@ -41,7 +38,6 @@ except ImportError:
41 38 has_robot = False
42 39  
43 40 import wx
44   -import vtk
45 41  
46 42 try:
47 43 import wx.lib.agw.foldpanelbar as fpb
... ... @@ -51,7 +47,6 @@ except ImportError:
51 47 import wx.lib.colourselect as csel
52 48 import wx.lib.masked.numctrl
53 49 from invesalius.pubsub import pub as Publisher
54   -from time import sleep
55 50  
56 51 import invesalius.constants as const
57 52  
... ... @@ -64,6 +59,7 @@ import invesalius.data.tractography as dti
64 59 import invesalius.data.record_coords as rec
65 60 import invesalius.data.vtk_utils as vtk_utils
66 61 import invesalius.data.bases as db
  62 +import invesalius.data.coregistration as dcr
67 63 import invesalius.gui.dialogs as dlg
68 64 import invesalius.project as prj
69 65 import invesalius.session as ses
... ... @@ -73,7 +69,6 @@ from invesalius.gui import utils as gui_utils
73 69 from invesalius.navigation.icp import ICP
74 70 from invesalius.navigation.navigation import Navigation
75 71 from invesalius.navigation.tracker import Tracker
76   -from invesalius.navigation.robot import Robot
77 72 from invesalius.data.converters import to_vtk
78 73  
79 74 from invesalius.net.neuronavigation_api import NeuronavigationApi
... ... @@ -176,7 +171,7 @@ class InnerFoldPanel(wx.Panel):
176 171 #
177 172 tracker = Tracker()
178 173 pedal_connection = PedalConnection() if HAS_PEDAL_CONNECTION else None
179   -
  174 + icp = ICP()
180 175 neuronavigation_api = NeuronavigationApi()
181 176 navigation = Navigation(
182 177 pedal_connection=pedal_connection,
... ... @@ -194,6 +189,7 @@ class InnerFoldPanel(wx.Panel):
194 189 parent=item,
195 190 navigation=navigation,
196 191 tracker=tracker,
  192 + icp=icp,
197 193 pedal_connection=pedal_connection,
198 194 neuronavigation_api=neuronavigation_api,
199 195 )
... ... @@ -205,7 +201,12 @@ class InnerFoldPanel(wx.Panel):
205 201  
206 202 # Fold 2 - Object registration panel
207 203 item = fold_panel.AddFoldPanel(_("Object registration"), collapsed=True)
208   - otw = ObjectRegistrationPanel(item, tracker, pedal_connection)
  204 + otw = ObjectRegistrationPanel(
  205 + parent=item,
  206 + tracker=tracker,
  207 + pedal_connection=pedal_connection,
  208 + neuronavigation_api=neuronavigation_api,
  209 + )
209 210  
210 211 fold_panel.ApplyCaptionStyle(item, style)
211 212 fold_panel.AddFoldPanelWindow(item, otw, spacing=0,
... ... @@ -213,7 +214,7 @@ class InnerFoldPanel(wx.Panel):
213 214  
214 215 # Fold 3 - Markers panel
215 216 item = fold_panel.AddFoldPanel(_("Markers"), collapsed=True)
216   - mtw = MarkersPanel(item, navigation, tracker)
  217 + mtw = MarkersPanel(item, navigation, tracker, icp)
217 218  
218 219 fold_panel.ApplyCaptionStyle(item, style)
219 220 fold_panel.AddFoldPanelWindow(item, mtw, spacing= 0,
... ... @@ -352,7 +353,7 @@ class InnerFoldPanel(wx.Panel):
352 353  
353 354  
354 355 class NeuronavigationPanel(wx.Panel):
355   - def __init__(self, parent, navigation, tracker, pedal_connection, neuronavigation_api):
  356 + def __init__(self, parent, navigation, tracker, icp, pedal_connection, neuronavigation_api):
356 357 wx.Panel.__init__(self, parent)
357 358 try:
358 359 default_colour = wx.SystemSettings.GetColour(wx.SYS_COLOUR_MENUBAR)
... ... @@ -369,9 +370,8 @@ class NeuronavigationPanel(wx.Panel):
369 370 self.neuronavigation_api = neuronavigation_api
370 371  
371 372 self.navigation = navigation
372   - self.icp = ICP()
  373 + self.icp = icp
373 374 self.tracker = tracker
374   - self.robot = Robot(tracker)
375 375  
376 376 self.nav_status = False
377 377 self.tracker_fiducial_being_set = None
... ... @@ -558,6 +558,7 @@ class NeuronavigationPanel(wx.Panel):
558 558 Publisher.subscribe(self.UpdateTarget, 'Update target')
559 559 Publisher.subscribe(self.OnStartNavigation, 'Start navigation')
560 560 Publisher.subscribe(self.OnStopNavigation, 'Stop navigation')
  561 + Publisher.subscribe(self.OnDialogRobotDestroy, 'Dialog robot destroy')
561 562  
562 563 def LoadImageFiducials(self, label, coord):
563 564 fiducial = self.GetFiducialByAttribute(const.IMAGE_FIDUCIALS, 'label', label)
... ... @@ -671,8 +672,6 @@ class NeuronavigationPanel(wx.Panel):
671 672 self.checkbox_icp.SetValue(False)
672 673  
673 674 def OnDisconnectTracker(self):
674   - if self.tracker.tracker_id == const.ROBOT:
675   - self.robot.StopRobotThreadNavigation()
676 675 self.tracker.DisconnectTracker()
677 676 self.ResetICP()
678 677 self.tracker.UpdateUI(self.select_tracker_elem, self.numctrls_fiducial[3:6], self.txtctrl_fre)
... ... @@ -691,7 +690,12 @@ class NeuronavigationPanel(wx.Panel):
691 690  
692 691 self.tracker.SetTracker(choice)
693 692 if self.tracker.tracker_id == const.ROBOT:
694   - self.tracker.ConnectToRobot(self.navigation, self.tracker, self.robot)
  693 + self.dlg_correg_robot = dlg.CreateTransformationMatrixRobot(self.tracker)
  694 + if self.dlg_correg_robot.ShowModal() == wx.ID_OK:
  695 + Publisher.sendMessage('Robot navigation mode', robot_mode=True)
  696 + else:
  697 + Publisher.sendMessage('Disconnect tracker')
  698 + wx.MessageBox(_("Not possible to connect to the robot."), _("InVesalius 3"))
695 699  
696 700 self.ResetICP()
697 701 self.tracker.UpdateUI(ctrl, self.numctrls_fiducial[3:6], self.txtctrl_fre)
... ... @@ -776,8 +780,8 @@ class NeuronavigationPanel(wx.Panel):
776 780  
777 781 self.navigation.StopNavigation()
778 782 if self.tracker.tracker_id == const.ROBOT:
779   - Publisher.sendMessage('Robot target matrix', robot_tracker_flag=False,
780   - m_change_robot_to_head=None)
  783 + Publisher.sendMessage('Update robot target', robot_tracker_flag=False,
  784 + target_index=None, target=None)
781 785  
782 786 # Enable all navigation buttons
783 787 choice_ref.Enable(True)
... ... @@ -786,6 +790,10 @@ class NeuronavigationPanel(wx.Panel):
786 790 for btn_c in self.btns_set_fiducial:
787 791 btn_c.Enable(True)
788 792  
  793 + def OnDialogRobotDestroy(self):
  794 + if self.dlg_correg_robot:
  795 + self.dlg_correg_robot.Destroy()
  796 +
789 797 def CheckFiducialRegistrationError(self):
790 798 self.navigation.UpdateFiducialRegistrationError(self.tracker)
791 799 fre, fre_ok = self.navigation.GetFiducialRegistrationError(self.icp)
... ... @@ -820,7 +828,7 @@ class NeuronavigationPanel(wx.Panel):
820 828 for btn_c in self.btns_set_fiducial:
821 829 btn_c.Enable(False)
822 830  
823   - self.navigation.StartNavigation(self.tracker)
  831 + self.navigation.EstimateTrackerToInVTransformationMatrix(self.tracker)
824 832  
825 833 if not self.CheckFiducialRegistrationError():
826 834 # TODO: Exhibit FRE in a warning dialog and only starts navigation after user clicks ok
... ... @@ -830,9 +838,11 @@ class NeuronavigationPanel(wx.Panel):
830 838 if self.icp.use_icp:
831 839 self.checkbox_icp.Enable(True)
832 840 self.checkbox_icp.SetValue(True)
833   - # Update FRE once more after starting the navigation, due to the optional use of ICP,
834   - # which improves FRE.
835   - self.CheckFiducialRegistrationError()
  841 + # Update FRE once more after starting the navigation, due to the optional use of ICP,
  842 + # which improves FRE.
  843 + self.CheckFiducialRegistrationError()
  844 +
  845 + self.navigation.StartNavigation(self.tracker)
836 846  
837 847 def OnNavigate(self, evt, btn_nav):
838 848 select_tracker_elem = self.select_tracker_elem
... ... @@ -881,11 +891,10 @@ class NeuronavigationPanel(wx.Panel):
881 891 )
882 892 self.tracker.__init__()
883 893 self.icp.__init__()
884   - self.robot.__init__(self.tracker)
885 894  
886 895  
887 896 class ObjectRegistrationPanel(wx.Panel):
888   - def __init__(self, parent, tracker, pedal_connection):
  897 + def __init__(self, parent, tracker, pedal_connection, neuronavigation_api):
889 898 wx.Panel.__init__(self, parent)
890 899 try:
891 900 default_colour = wx.SystemSettings.GetColour(wx.SYS_COLOUR_MENUBAR)
... ... @@ -897,6 +906,7 @@ class ObjectRegistrationPanel(wx.Panel):
897 906  
898 907 self.tracker = tracker
899 908 self.pedal_connection = pedal_connection
  909 + self.neuronavigation_api = neuronavigation_api
900 910  
901 911 self.nav_prop = None
902 912 self.obj_fiducials = None
... ... @@ -913,7 +923,7 @@ class ObjectRegistrationPanel(wx.Panel):
913 923 btn_new = wx.Button(self, -1, _("New"), size=wx.Size(65, 23))
914 924 btn_new.SetToolTip(tooltip)
915 925 btn_new.Enable(1)
916   - btn_new.Bind(wx.EVT_BUTTON, self.OnLinkCreate)
  926 + btn_new.Bind(wx.EVT_BUTTON, self.OnCreateNewCoil)
917 927 self.btn_new = btn_new
918 928  
919 929 # Button for import config coil file
... ... @@ -1056,13 +1066,16 @@ class ObjectRegistrationPanel(wx.Panel):
1056 1066 coil_index = evt.GetSelection()
1057 1067 Publisher.sendMessage('Change selected coil', self.coil_list[coil_index][1])
1058 1068  
1059   - def OnLinkCreate(self, event=None):
  1069 + def OnCreateNewCoil(self, event=None):
1060 1070  
1061 1071 if self.tracker.IsTrackerInitialized():
1062 1072 dialog = dlg.ObjectCalibrationDialog(self.tracker, self.pedal_connection)
1063 1073 try:
1064 1074 if dialog.ShowModal() == wx.ID_OK:
1065 1075 self.obj_fiducials, self.obj_orients, self.obj_ref_mode, self.obj_name, polydata, use_default_object = dialog.GetValue()
  1076 +
  1077 + self.neuronavigation_api.update_coil_mesh(polydata)
  1078 +
1066 1079 if np.isfinite(self.obj_fiducials).all() and np.isfinite(self.obj_orients).all():
1067 1080 self.checktrack.Enable(1)
1068 1081 Publisher.sendMessage('Update object registration',
... ... @@ -1248,20 +1261,7 @@ class MarkersPanel(wx.Panel):
1248 1261 if field.type is bool:
1249 1262 setattr(self, field.name, str_val=='True')
1250 1263  
1251   - @dataclasses.dataclass
1252   - class Robot_Marker:
1253   - """Class for storing robot target."""
1254   - m_robot_target : list = None
1255   -
1256   - @property
1257   - def robot_target_matrix(self):
1258   - return self.m_robot_target
1259   -
1260   - @robot_target_matrix.setter
1261   - def robot_target_matrix(self, new_m_robot_target):
1262   - self.m_robot_target = new_m_robot_target
1263   -
1264   - def __init__(self, parent, navigation, tracker):
  1264 + def __init__(self, parent, navigation, tracker, icp):
1265 1265 wx.Panel.__init__(self, parent)
1266 1266 try:
1267 1267 default_colour = wx.SystemSettings.GetColour(wx.SYS_COLOUR_MENUBAR)
... ... @@ -1273,6 +1273,7 @@ class MarkersPanel(wx.Panel):
1273 1273  
1274 1274 self.navigation = navigation
1275 1275 self.tracker = tracker
  1276 + self.icp = icp
1276 1277  
1277 1278 self.__bind_events()
1278 1279  
... ... @@ -1280,11 +1281,9 @@ class MarkersPanel(wx.Panel):
1280 1281  
1281 1282 self.current_coord = [0, 0, 0, None, None, None]
1282 1283 self.current_seed = 0, 0, 0
1283   - self.current_robot_target_matrix = [None] * 9
  1284 +
1284 1285 self.markers = []
1285   - self.robot_markers = []
1286 1286 self.nav_status = False
1287   - self.raw_target_robot = None, None
1288 1287  
1289 1288 self.marker_colour = const.MARKER_COLOUR
1290 1289 self.marker_size = const.MARKER_SIZE
... ... @@ -1383,7 +1382,6 @@ class MarkersPanel(wx.Panel):
1383 1382 Publisher.subscribe(self.UpdateNavigationStatus, 'Navigation status')
1384 1383 Publisher.subscribe(self.UpdateSeedCoordinates, 'Update tracts')
1385 1384 Publisher.subscribe(self.OnChangeCurrentSession, 'Current session changed')
1386   - Publisher.subscribe(self.UpdateRobotCoordinates, 'Update raw coordinates')
1387 1385  
1388 1386 def __find_target_marker(self):
1389 1387 """
... ... @@ -1417,7 +1415,6 @@ class MarkersPanel(wx.Panel):
1417 1415 """
1418 1416 for i in reversed(index):
1419 1417 del self.markers[i]
1420   - del self.robot_markers[i]
1421 1418 self.lc.DeleteItem(i)
1422 1419 for n in range(0, self.lc.GetItemCount()):
1423 1420 self.lc.SetItem(n, 0, str(n + 1))
... ... @@ -1470,34 +1467,37 @@ class MarkersPanel(wx.Panel):
1470 1467 def UpdateSeedCoordinates(self, root=None, affine_vtk=None, coord_offset=(0, 0, 0), coord_offset_w=(0, 0, 0)):
1471 1468 self.current_seed = coord_offset_w
1472 1469  
1473   - def UpdateRobotCoordinates(self, coordinates_raw, markers_flag):
1474   - self.raw_target_robot = coordinates_raw[1], coordinates_raw[2]
1475   -
1476 1470 def OnMouseRightDown(self, evt):
1477 1471 # TODO: Enable the "Set as target" only when target is created with registered object
1478 1472 menu_id = wx.Menu()
1479 1473 edit_id = menu_id.Append(0, _('Edit label'))
1480 1474 menu_id.Bind(wx.EVT_MENU, self.OnMenuEditMarkerLabel, edit_id)
1481   - color_id = menu_id.Append(2, _('Edit color'))
  1475 + color_id = menu_id.Append(1, _('Edit color'))
1482 1476 menu_id.Bind(wx.EVT_MENU, self.OnMenuSetColor, color_id)
1483 1477 menu_id.AppendSeparator()
1484   - target_menu = menu_id.Append(1, _('Set as target'))
  1478 + target_menu = menu_id.Append(2, _('Set as target'))
1485 1479 menu_id.Bind(wx.EVT_MENU, self.OnMenuSetTarget, target_menu)
1486 1480 menu_id.AppendSeparator()
1487   - send_target_to_robot = menu_id.Append(3, _('Send target to robot'))
1488   - menu_id.Bind(wx.EVT_MENU, self.OnMenuSendTargetToRobot, send_target_to_robot)
1489 1481  
1490   - if all([elem is not None for elem in self.markers[self.lc.GetFocusedItem()].coord[3:]]):
  1482 + check_target_angles = all([elem is not None for elem in self.markers[self.lc.GetFocusedItem()].coord[3:]])
  1483 + # Enable "Send target to robot" button only if tracker is robot, if navigation is on and if target is not none
  1484 + if self.tracker.tracker_id == const.ROBOT:
  1485 + send_target_to_robot_compensation = menu_id.Append(3, _('Sets target to robot head move compensation'))
  1486 + menu_id.Bind(wx.EVT_MENU, self.OnMenuSetRobotCompensation, send_target_to_robot_compensation)
  1487 + send_target_to_robot = menu_id.Append(4, _('Send target from InVesalius to robot'))
  1488 + menu_id.Bind(wx.EVT_MENU, self.OnMenuSendTargetToRobot, send_target_to_robot)
  1489 + if self.nav_status and check_target_angles:
  1490 + send_target_to_robot_compensation.Enable(True)
  1491 + send_target_to_robot.Enable(True)
  1492 + else:
  1493 + send_target_to_robot_compensation.Enable(False)
  1494 + send_target_to_robot.Enable(False)
  1495 +
  1496 + if check_target_angles:
1491 1497 target_menu.Enable(True)
1492 1498 else:
1493 1499 target_menu.Enable(False)
1494 1500  
1495   - # Enable "Send target to robot" button only if tracker is robot, if navigation is on and if target is not none
1496   - m_target_robot = np.array([self.robot_markers[self.lc.GetFocusedItem()].robot_target_matrix])
1497   - if self.tracker.tracker_id == const.ROBOT and self.nav_status and m_target_robot.any():
1498   - send_target_to_robot.Enable(True)
1499   - else:
1500   - send_target_to_robot.Enable(False)
1501 1501 # TODO: Create the remove target option so the user can disable the target without removing the marker
1502 1502 # target_menu_rem = menu_id.Append(3, _('Remove target'))
1503 1503 # menu_id.Bind(wx.EVT_MENU, self.OnMenuRemoveTarget, target_menu_rem)
... ... @@ -1547,14 +1547,33 @@ class MarkersPanel(wx.Panel):
1547 1547  
1548 1548 Publisher.sendMessage('Set new color', index=index, color=color_new)
1549 1549  
  1550 + def OnMenuSetRobotCompensation(self, evt):
  1551 + if isinstance(evt, int):
  1552 + self.lc.Focus(evt)
  1553 +
  1554 + Publisher.sendMessage('Reset robot process', data=None)
  1555 + matrix_tracker_fiducials = self.tracker.GetMatrixTrackerFiducials()
  1556 + Publisher.sendMessage('Update tracker fiducials matrix',
  1557 + matrix_tracker_fiducials=matrix_tracker_fiducials)
  1558 + Publisher.sendMessage('Update robot target', robot_tracker_flag=True, target_index=self.lc.GetFocusedItem(), target=None)
  1559 +
1550 1560 def OnMenuSendTargetToRobot(self, evt):
1551 1561 if isinstance(evt, int):
1552 1562 self.lc.Focus(evt)
  1563 + index = self.lc.GetFocusedItem()
  1564 + if index == -1:
  1565 + wx.MessageBox(_("No data selected."), _("InVesalius 3"))
  1566 + return
  1567 +
  1568 + Publisher.sendMessage('Reset robot process', data=None)
  1569 + matrix_tracker_fiducials = self.tracker.GetMatrixTrackerFiducials()
  1570 + Publisher.sendMessage('Update tracker fiducials matrix',
  1571 + matrix_tracker_fiducials=matrix_tracker_fiducials)
1553 1572  
1554   - m_target_robot = self.robot_markers[self.lc.GetFocusedItem()].robot_target_matrix
  1573 + target_coord = self.markers[index].coord[:3]
  1574 + target = dcr.image_to_tracker(self.navigation.m_change, target_coord, self.icp)
1555 1575  
1556   - Publisher.sendMessage('Reset robot process')
1557   - Publisher.sendMessage('Robot target matrix', robot_tracker_flag=True, m_change_robot_to_head=m_target_robot)
  1576 + Publisher.sendMessage('Update robot target', robot_tracker_flag=True, target_index=self.lc.GetFocusedItem(), target=target.tolist())
1558 1577  
1559 1578 def OnDeleteAllMarkers(self, evt=None):
1560 1579 if evt is not None:
... ... @@ -1566,9 +1585,11 @@ class MarkersPanel(wx.Panel):
1566 1585 Publisher.sendMessage('Disable or enable coil tracker', status=False)
1567 1586 if evt is not None:
1568 1587 wx.MessageBox(_("Target deleted."), _("InVesalius 3"))
  1588 + if self.tracker.tracker_id == const.ROBOT:
  1589 + Publisher.sendMessage('Update robot target', robot_tracker_flag=False,
  1590 + target_index=None, target=None)
1569 1591  
1570 1592 self.markers = []
1571   - self.robot_markers = []
1572 1593 Publisher.sendMessage('Remove all markers', indexes=self.lc.GetItemCount())
1573 1594 self.lc.DeleteAllItems()
1574 1595 Publisher.sendMessage('Stop Blink Marker', index='DeleteAll')
... ... @@ -1596,8 +1617,8 @@ class MarkersPanel(wx.Panel):
1596 1617 if self.__find_target_marker() in index:
1597 1618 Publisher.sendMessage('Disable or enable coil tracker', status=False)
1598 1619 if self.tracker.tracker_id == const.ROBOT:
1599   - Publisher.sendMessage('Robot target matrix', robot_tracker_flag=False,
1600   - m_change_robot_to_head=[])
  1620 + Publisher.sendMessage('Update robot target', robot_tracker_flag=False,
  1621 + target_index=None, target=None)
1601 1622 wx.MessageBox(_("Target deleted."), _("InVesalius 3"))
1602 1623  
1603 1624 self.__delete_multiple_markers(index)
... ... @@ -1702,12 +1723,11 @@ class MarkersPanel(wx.Panel):
1702 1723 new_marker.session_id = session_id or self.current_session
1703 1724  
1704 1725 if self.tracker.tracker_id == const.ROBOT and self.nav_status:
1705   - self.current_robot_target_matrix = db.compute_robot_to_head_matrix(self.raw_target_robot)
  1726 + current_head_robot_target_status = True
1706 1727 else:
1707   - self.current_robot_target_matrix = [None] * 9
  1728 + current_head_robot_target_status = False
1708 1729  
1709   - new_robot_marker = self.Robot_Marker()
1710   - new_robot_marker.robot_target_matrix = self.current_robot_target_matrix
  1730 + Publisher.sendMessage('Add marker to robot control', data=current_head_robot_target_status)
1711 1731  
1712 1732 # Note that ball_id is zero-based, so we assign it len(self.markers) before the new marker is added
1713 1733 if all([elem is not None for elem in new_marker.coord[3:]]):
... ... @@ -1723,7 +1743,6 @@ class MarkersPanel(wx.Panel):
1723 1743  
1724 1744  
1725 1745 self.markers.append(new_marker)
1726   - self.robot_markers.append(new_robot_marker)
1727 1746  
1728 1747 # Add item to list control in panel
1729 1748 num_items = self.lc.GetItemCount()
... ...
invesalius/navigation/icp.py
  Diff comments   View file @e971a69
... ... @@ -21,10 +21,10 @@ import wx
21 21  
22 22 import invesalius.data.bases as db
23 23 import invesalius.gui.dialogs as dlg
24   -from invesalius.pubsub import pub as Publisher
25 24  
  25 +from invesalius.utils import Singleton
26 26  
27   -class ICP():
  27 +class ICP(metaclass=Singleton):
28 28 def __init__(self):
29 29 self.use_icp = False
30 30 self.m_icp = None
... ... @@ -35,7 +35,6 @@ class ICP():
35 35  
36 36 if not self.use_icp:
37 37 if dlg.ICPcorregistration(navigation.fre):
38   - Publisher.sendMessage('Stop navigation')
39 38 use_icp, self.m_icp = self.OnICP(navigation, tracker, navigation.m_change)
40 39 if use_icp:
41 40 self.icp_fre = db.calculate_fre(tracker.tracker_fiducials_raw, navigation.all_fiducials,
... ... @@ -43,7 +42,6 @@ class ICP():
43 42 self.SetICP(navigation, use_icp)
44 43 else:
45 44 print("ICP canceled")
46   - Publisher.sendMessage('Start navigation')
47 45  
48 46 def OnICP(self, navigation, tracker, m_change):
49 47 ref_mode_id = navigation.GetReferenceMode()
... ...
invesalius/navigation/navigation.py
  Diff comments   View file @e971a69
... ... @@ -34,6 +34,7 @@ import invesalius.data.tractography as dti
34 34 import invesalius.data.transformations as tr
35 35 import invesalius.data.vtk_utils as vtk_utils
36 36 from invesalius.pubsub import pub as Publisher
  37 +from invesalius.utils import Singleton
37 38  
38 39  
39 40 class QueueCustom(queue.Queue):
... ... @@ -83,7 +84,7 @@ class UpdateNavigationScene(threading.Thread):
83 84  
84 85 threading.Thread.__init__(self, name='UpdateScene')
85 86 self.serial_port_enabled, self.view_tracts, self.peel_loaded = vis_components
86   - self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue, self.robot_target_queue = vis_queues
  87 + self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue = vis_queues
87 88 self.sle = sle
88 89 self.event = event
89 90 self.neuronavigation_api = neuronavigation_api
... ... @@ -93,7 +94,7 @@ class UpdateNavigationScene(threading.Thread):
93 94 while not self.event.is_set():
94 95 got_coords = False
95 96 try:
96   - coord, [coordinates_raw, markers_flag], m_img, view_obj = self.coord_queue.get_nowait()
  97 + coord, markers_flag, m_img, view_obj = self.coord_queue.get_nowait()
97 98 got_coords = True
98 99  
99 100 # print('UpdateScene: get {}'.format(count))
... ... @@ -117,7 +118,6 @@ class UpdateNavigationScene(threading.Thread):
117 118 # see the red cross in the position of the offset marker
118 119 wx.CallAfter(Publisher.sendMessage, 'Update slices position', position=coord[:3])
119 120 wx.CallAfter(Publisher.sendMessage, 'Set cross focal point', position=coord)
120   - wx.CallAfter(Publisher.sendMessage, 'Update raw coordinates', coordinates_raw=coordinates_raw, markers_flag=markers_flag)
121 121 wx.CallAfter(Publisher.sendMessage, 'Update slice viewer')
122 122 wx.CallAfter(Publisher.sendMessage, 'Sensor ID', markers_flag=markers_flag)
123 123  
... ... @@ -140,7 +140,7 @@ class UpdateNavigationScene(threading.Thread):
140 140 self.coord_queue.task_done()
141 141  
142 142  
143   -class Navigation():
  143 +class Navigation(metaclass=Singleton):
144 144 def __init__(self, pedal_connection, neuronavigation_api):
145 145 self.pedal_connection = pedal_connection
146 146 self.neuronavigation_api = neuronavigation_api
... ... @@ -157,7 +157,6 @@ class Navigation():
157 157 self.coord_queue = QueueCustom(maxsize=1)
158 158 self.icp_queue = QueueCustom(maxsize=1)
159 159 self.object_at_target_queue = QueueCustom(maxsize=1)
160   - self.robot_target_queue = QueueCustom(maxsize=1)
161 160 # self.visualization_queue = QueueCustom(maxsize=1)
162 161 self.serial_port_queue = QueueCustom(maxsize=1)
163 162 self.coord_tracts_queue = QueueCustom(maxsize=1)
... ... @@ -245,9 +244,14 @@ class Navigation():
245 244 if state and permission_to_stimulate:
246 245 self.serial_port_connection.SendPulse()
247 246  
248   - def StartNavigation(self, tracker):
  247 + def EstimateTrackerToInVTransformationMatrix(self, tracker):
249 248 tracker_fiducials, tracker_fiducials_raw = tracker.GetTrackerFiducials()
  249 + self.all_fiducials = np.vstack([self.image_fiducials, tracker_fiducials])
250 250  
  251 + self.m_change = tr.affine_matrix_from_points(self.all_fiducials[3:, :].T, self.all_fiducials[:3, :].T,
  252 + shear=False, scale=False)
  253 +
  254 + def StartNavigation(self, tracker):
251 255 # initialize jobs list
252 256 jobs_list = []
253 257  
... ... @@ -255,17 +259,9 @@ class Navigation():
255 259 self.event.clear()
256 260  
257 261 vis_components = [self.serial_port_in_use, self.view_tracts, self.peel_loaded]
258   - vis_queues = [self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue, self.robot_target_queue]
  262 + vis_queues = [self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue]
259 263  
260 264 Publisher.sendMessage("Navigation status", nav_status=True, vis_status=vis_components)
261   -
262   - self.all_fiducials = np.vstack([self.image_fiducials, tracker_fiducials])
263   -
264   - # fiducials matrix
265   - m_change = tr.affine_matrix_from_points(self.all_fiducials[3:, :].T, self.all_fiducials[:3, :].T,
266   - shear=False, scale=False)
267   - self.m_change = m_change
268   -
269 265 errors = False
270 266  
271 267 if self.track_obj:
... ... @@ -279,14 +275,14 @@ class Navigation():
279 275 # obj_reg[0] is object 3x3 fiducial matrix and obj_reg[1] is 3x3 orientation matrix
280 276 obj_fiducials, obj_orients, obj_ref_mode, obj_name = self.obj_reg
281 277  
282   - coreg_data = [m_change, obj_ref_mode]
  278 + coreg_data = [self.m_change, obj_ref_mode]
283 279  
284 280 if self.ref_mode_id:
285 281 coord_raw, markers_flag = tracker.TrackerCoordinates.GetCoordinates()
286 282 else:
287 283 coord_raw = np.array([None])
288 284  
289   - obj_data = db.object_registration(obj_fiducials, obj_orients, coord_raw, m_change)
  285 + obj_data = db.object_registration(obj_fiducials, obj_orients, coord_raw, self.m_change)
290 286 coreg_data.extend(obj_data)
291 287  
292 288 queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue, self.object_at_target_queue]
... ... @@ -295,7 +291,7 @@ class Navigation():
295 291 self.event, self.sleep_nav, tracker.tracker_id,
296 292 self.target))
297 293 else:
298   - coreg_data = (m_change, 0)
  294 + coreg_data = (self.m_change, 0)
299 295 queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue]
300 296 jobs_list.append(dcr.CoordinateCorregistrateNoObject(self.ref_mode_id, tracker, coreg_data,
301 297 self.view_tracts, queues,
... ...
invesalius/navigation/tracker.py
  Diff comments   View file @e971a69
... ... @@ -42,7 +42,6 @@ class Tracker():
42 42 self.thread_coord = None
43 43  
44 44 self.event_coord = threading.Event()
45   - self.event_robot = threading.Event()
46 45  
47 46 self.TrackerCoordinates = dco.TrackerCoordinates()
48 47  
... ... @@ -78,11 +77,8 @@ class Tracker():
78 77  
79 78 if self.thread_coord:
80 79 self.event_coord.set()
81   - self.event_robot.set()
82 80 self.thread_coord.join()
83 81 self.event_coord.clear()
84   - self.event_robot.clear()
85   -
86 82  
87 83 Publisher.sendMessage('Update status text in GUI',
88 84 label=_("Tracker disconnected"))
... ... @@ -92,13 +88,6 @@ class Tracker():
92 88 label=_("Tracker still connected"))
93 89 print("Tracker still connected!")
94 90  
95   - def ConnectToRobot(self, navigation, tracker, robot):
96   - robot.SetRobotQueues([navigation.robot_target_queue, navigation.object_at_target_queue])
97   - robot.OnRobotConnection()
98   - trk_init_robot = self.trk_init[0][1][0]
99   - if trk_init_robot:
100   - robot.StartRobotThreadNavigation(tracker, navigation.coord_queue)
101   - Publisher.sendMessage('Robot navigation mode', robot_mode=True)
102 91  
103 92 def IsTrackerInitialized(self):
104 93 return self.trk_init and self.tracker_id and self.tracker_connected
... ... @@ -158,7 +147,7 @@ class Tracker():
158 147 m_probe_ref_right = np.linalg.inv(self.m_tracker_fiducials_raw[3]) @ self.m_tracker_fiducials_raw[2]
159 148 m_probe_ref_nasion = np.linalg.inv(self.m_tracker_fiducials_raw[5]) @ self.m_tracker_fiducials_raw[4]
160 149  
161   - return [m_probe_ref_left, m_probe_ref_right, m_probe_ref_nasion]
  150 + return [m_probe_ref_left.tolist(), m_probe_ref_right.tolist(), m_probe_ref_nasion.tolist()]
162 151  
163 152 def GetTrackerInfo(self):
164 153 return self.trk_init, self.tracker_id
... ...
invesalius/net/neuronavigation_api.py
  Diff comments   View file @e971a69
... ... @@ -20,6 +20,9 @@
20 20 from invesalius.pubsub import pub as Publisher
21 21 from invesalius.utils import Singleton
22 22  
  23 +import numpy as np
  24 +from vtk.numpy_interface import dataset_adapter
  25 +
23 26 class NeuronavigationApi(metaclass=Singleton):
24 27 """
25 28 An API used internally in InVesalius to communicate with the
... ... @@ -34,6 +37,8 @@ class NeuronavigationApi(metaclass=Singleton):
34 37  
35 38 If connection object is not given or it is None, skip doing the updates.
36 39 """
  40 + N_VERTICES_IN_POLYGON = 3
  41 +
37 42 def __init__(self, connection=None):
38 43 if connection is not None:
39 44 assert self._hasmethod(connection, 'update_coil_pose')
... ... @@ -65,3 +70,28 @@ class NeuronavigationApi(metaclass=Singleton):
65 70 position=position,
66 71 orientation=orientation,
67 72 )
  73 +
  74 + def update_coil_mesh(self, polydata):
  75 + wrapped = dataset_adapter.WrapDataObject(polydata)
  76 +
  77 + points = np.asarray(wrapped.Points)
  78 + polygons_raw = np.asarray(wrapped.Polygons)
  79 +
  80 + # The polygons are returned as 1d-array of the form
  81 + #
  82 + # [n_0, id_0(0), id_0(1), ..., id_0(n_0),
  83 + # n_1, id_1(0), id_1(1), ..., id_1(n_1),
  84 + # ...]
  85 + #
  86 + # where n_i is the number of vertices in polygon i, and id_i's are indices to the vertex list.
  87 + #
  88 + # Assert that all polygons have an equal number of vertices, reshape the array, and drop n_i's.
  89 + #
  90 + assert np.all(polygons_raw[0::self.N_VERTICES_IN_POLYGON + 1] == self.N_VERTICES_IN_POLYGON)
  91 +
  92 + polygons = polygons_raw.reshape(-1, self.N_VERTICES_IN_POLYGON + 1)[:, 1:]
  93 +
  94 + self.connection.update_coil_mesh(
  95 + points=points,
  96 + polygons=polygons,
  97 + )
... ...
invesalius/net/remote_control.py
  Diff comments   View file @e971a69
... ... @@ -79,5 +79,7 @@ class RemoteControl:
79 79 })
80 80 except TypeError:
81 81 pass
  82 + except socketio.exceptions.BadNamespaceError:
  83 + pass
82 84  
83 85 Publisher.add_sendMessage_hook(_emit)
... ...
requirements_m1.txt 0 → 100644
  Diff comments   View file @e971a69
... ... @@ -0,0 +1,17 @@
  1 +Cython==0.29.24
  2 +Pillow==9.0.0
  3 +Pypubsub==4.0.3
  4 +configparser==5.0.1
  5 +h5py==3.6.0
  6 +imageio==2.9.0
  7 +nibabel==3.2.1
  8 +numpy==1.22.1
  9 +psutil==5.8.0
  10 +pyserial==3.5
  11 +python-gdcm==3.0.9.1
  12 +scikit-image==0.19.1
  13 +scipy==1.7.3
  14 +vtk==9.0.3
  15 +wxPython==4.1.1
  16 +torch==1.10.0
  17 +pyacvd==0.2.7
... ...