Download as
Browse Code »

Commit 55ce120595561b4806a4aed610e1379d82ac69f6

Authored by Olli-Pekka Kahilakoski
1 parent 9d79c88d
Exists in master

MOD: Refactor task_navigator.py 

- Some steps towards decoupling GUI and domain logic, namely, by adding
  classes Navigation and Tracker, which take some of the responsibility
  previously handled by NeuronavigationPanel.

- This is done to prepare to do some changes in the GUI, such as
  moving "Navigate" button elsewhere -- previously, it was hard
  to do due to the elements in the GUI being tightly coupled with the
  implementation of navigation.

- Also some other minor clean-up and style improvements, such as
  using more consistent and more descriptive variable names
Showing 3 changed files with 490 additions and 432 deletions   Show diff stats
invesalius/constants.py
  Diff comments   View file @55ce120
... ... @@ -675,12 +675,12 @@ DEFAULT_TRACKER = SELECT
675 675  
676 676 NDICOMPORT = b'COM1'
677 677  
678   -TRACKER = [_("Select tracker:"), _("Claron MicronTracker"),
679   - _("Polhemus FASTRAK"), _("Polhemus ISOTRAK II"),
680   - _("Polhemus PATRIOT"), _("Camera tracker"),
681   - _("NDI Polaris"), _("NDI Polaris P4"),
682   - _("Optitrack"), _("Debug tracker (random)"),
683   - _("Debug tracker (approach)"), _("Disconnect tracker")]
  678 +TRACKERS = [_("Claron MicronTracker"),
  679 + _("Polhemus FASTRAK"), _("Polhemus ISOTRAK II"),
  680 + _("Polhemus PATRIOT"), _("Camera tracker"),
  681 + _("NDI Polaris"), _("NDI Polaris P4"),
  682 + _("Optitrack"), _("Debug tracker (random)"),
  683 + _("Debug tracker (approach)"), _("Disconnect tracker")]
684 684  
685 685 STATIC_REF = 0
686 686 DYNAMIC_REF = 1
... ... @@ -729,21 +729,21 @@ TRACKER_FIDUCIALS = [
729 729 'button_id': TR1,
730 730 'label': 'LET',
731 731 'fiducial_name': 'LE',
732   - 'fiducial_index': 3,
  732 + 'fiducial_index': 0,
733 733 'tip': _("Select left ear with spatial tracker"),
734 734 },
735 735 {
736 736 'button_id': TR2,
737 737 'label': 'RET',
738 738 'fiducial_name': 'RE',
739   - 'fiducial_index': 4,
  739 + 'fiducial_index': 1,
740 740 'tip': _("Select right ear with spatial tracker"),
741 741 },
742 742 {
743 743 'button_id': TR3,
744 744 'label': 'NAT',
745 745 'fiducial_name': 'NA',
746   - 'fiducial_index': 5,
  746 + 'fiducial_index': 2,
747 747 'tip': _("Select nasion with spatial tracker"),
748 748 },
749 749 ]
... ...
invesalius/data/coregistration.py
  Diff comments   View file @55ce120
... ... @@ -171,10 +171,10 @@ def corregistrate_dynamic(inp, coord_raw, ref_mode_id, icp):
171 171  
172 172  
173 173 class CoordinateCorregistrate(threading.Thread):
174   - def __init__(self, ref_mode_id, trck_info, coreg_data, view_tracts, queues, event, sle, tracker_id, target):
  174 + def __init__(self, ref_mode_id, tracker, coreg_data, view_tracts, queues, event, sle, tracker_id, target):
175 175 threading.Thread.__init__(self, name='CoordCoregObject')
176 176 self.ref_mode_id = ref_mode_id
177   - self.trck_info = trck_info
  177 + self.tracker = tracker
178 178 self.coreg_data = coreg_data
179 179 self.coord_queue = queues[0]
180 180 self.view_tracts = view_tracts
... ... @@ -198,11 +198,11 @@ class CoordinateCorregistrate(threading.Thread):
198 198 self.target[1] = -self.target[1]
199 199  
200 200 def run(self):
201   - trck_info = self.trck_info
202 201 coreg_data = self.coreg_data
203 202 view_obj = 1
204 203  
205   - trck_init, trck_id, trck_mode = trck_info
  204 + trck_init, trck_id, trck_mode = self.tracker.GetTrackerInfo()
  205 +
206 206 # print('CoordCoreg: event {}'.format(self.event.is_set()))
207 207 while not self.event.is_set():
208 208 try:
... ... @@ -258,10 +258,10 @@ class CoordinateCorregistrate(threading.Thread):
258 258  
259 259  
260 260 class CoordinateCorregistrateNoObject(threading.Thread):
261   - def __init__(self, ref_mode_id, trck_info, coreg_data, view_tracts, queues, event, sle):
  261 + def __init__(self, ref_mode_id, tracker, coreg_data, view_tracts, queues, event, sle):
262 262 threading.Thread.__init__(self, name='CoordCoregNoObject')
263 263 self.ref_mode_id = ref_mode_id
264   - self.trck_info = trck_info
  264 + self.tracker = tracker
265 265 self.coreg_data = coreg_data
266 266 self.coord_queue = queues[0]
267 267 self.view_tracts = view_tracts
... ... @@ -273,11 +273,10 @@ class CoordinateCorregistrateNoObject(threading.Thread):
273 273 self.m_icp = None
274 274  
275 275 def run(self):
276   - trck_info = self.trck_info
277 276 coreg_data = self.coreg_data
278 277 view_obj = 0
279 278  
280   - trck_init, trck_id, trck_mode = trck_info
  279 + trck_init, trck_id, trck_mode = self.tracker.GetTrackerInfo()
281 280 # print('CoordCoreg: event {}'.format(self.event.is_set()))
282 281 while not self.event.is_set():
283 282 try:
... ...
invesalius/gui/task_navigator.py
  Diff comments   View file @55ce120
... ... @@ -300,39 +300,18 @@ class InnerFoldPanel(wx.Panel):
300 300 Publisher.sendMessage('Update volume camera state', camera_state=self.checkcamera.GetValue())
301 301  
302 302  
303   -class NeuronavigationPanel(wx.Panel):
304   - def __init__(self, parent):
305   - wx.Panel.__init__(self, parent)
306   - try:
307   - default_colour = wx.SystemSettings.GetColour(wx.SYS_COLOUR_MENUBAR)
308   - except AttributeError:
309   - default_colour = wx.SystemSettings_GetColour(wx.SYS_COLOUR_MENUBAR)
310   - self.SetBackgroundColour(default_colour)
311   -
312   - self.SetAutoLayout(1)
313   -
314   - self.__bind_events()
315   -
316   - # Initialize global variables
317   - self.pedal_connection = PedalConnection() if HAS_PEDAL_CONNECTION else None
318   - self.fiducials = np.full([6, 3], np.nan)
319   - self.fiducials_raw = np.zeros((6, 6))
  303 +class Navigation():
  304 + def __init__(self):
  305 + self.image_fiducials = np.full([3, 3], np.nan)
320 306 self.correg = None
321 307 self.current_coord = 0, 0, 0
322   - self.trk_init = None
323   - self.nav_status = False
324 308 self.target = None
325 309 self.trigger = None
326 310 self.trigger_state = False
327 311 self.obj_reg = None
328   - self.obj_reg_status = False
329 312 self.track_obj = False
330   - self.m_icp = None
331   - self.fre = None
332   - self.icp_fre = None
333   - self.icp = False
334   - self.event = threading.Event()
335 313  
  314 + self.event = threading.Event()
336 315 self.coord_queue = QueueCustom(maxsize=1)
337 316 self.icp_queue = QueueCustom(maxsize=1)
338 317 # self.visualization_queue = QueueCustom(maxsize=1)
... ... @@ -352,21 +331,400 @@ class NeuronavigationPanel(wx.Panel):
352 331 self.seed_radius = const.SEED_RADIUS
353 332 self.sleep_nav = const.SLEEP_NAVIGATION
354 333  
  334 + # ICP related variables
  335 + self.use_icp = False
  336 + self.m_icp = None
  337 + self.fre = None
  338 + self.icp_fre = None
  339 +
  340 + def SetImageFiducial(self, fiducial_index, coord):
  341 + self.image_fiducials[fiducial_index, :] = coord
  342 +
  343 + print("Set image fiducial {} to coordinates {}".format(fiducial_index, coord))
  344 +
  345 + def AreImageFiducialsSet(self):
  346 + return not np.isnan(self.image_fiducials).any()
  347 +
  348 + def UpdateFiducialRegistrationError(self, tracker):
  349 + tracker_fiducials, tracker_fiducials_raw = tracker.GetTrackerFiducials()
  350 + ref_mode_id = tracker.GetReferenceMode()
  351 +
  352 + all_fiducials = np.vstack([self.image_fiducials, tracker_fiducials])
  353 +
  354 + # fiducials matrix
  355 + m_change = tr.affine_matrix_from_points(all_fiducials[3:, :].T, all_fiducials[:3, :].T,
  356 + shear=False, scale=False)
  357 +
  358 + self.fre = db.calculate_fre(tracker_fiducials_raw, all_fiducials, ref_mode_id, m_change)
  359 +
  360 + def GetFiducialRegistrationError(self):
  361 + fre = self.icp_fre if self.use_icp else self.fre
  362 + return fre, fre <= 3
  363 +
  364 + def StartNavigation(self, tracker):
  365 + tracker_fiducials, tracker_fiducials_raw = tracker.GetTrackerFiducials()
  366 + ref_mode_id = tracker.GetReferenceMode()
  367 +
  368 + # initialize jobs list
  369 + jobs_list = []
  370 +
  371 + if self.event.is_set():
  372 + self.event.clear()
  373 +
  374 + vis_components = [self.trigger_state, self.view_tracts]
  375 + vis_queues = [self.coord_queue, self.trigger_queue, self.tracts_queue, self.icp_queue]
  376 +
  377 + Publisher.sendMessage("Navigation status", nav_status=True, vis_status=vis_components)
  378 +
  379 + all_fiducials = np.vstack([self.image_fiducials, tracker_fiducials])
  380 +
  381 + # fiducials matrix
  382 + m_change = tr.affine_matrix_from_points(all_fiducials[3:, :].T, all_fiducials[:3, :].T,
  383 + shear=False, scale=False)
  384 +
  385 + errors = False
  386 +
  387 + if self.track_obj:
  388 + # if object tracking is selected
  389 + if self.obj_reg is None:
  390 + # check if object registration was performed
  391 + wx.MessageBox(_("Perform coil registration before navigation."), _("InVesalius 3"))
  392 + errors = True
  393 + else:
  394 + # if object registration was correctly performed continue with navigation
  395 + # obj_reg[0] is object 3x3 fiducial matrix and obj_reg[1] is 3x3 orientation matrix
  396 + obj_fiducials, obj_orients, obj_ref_mode, obj_name = self.obj_reg
  397 +
  398 + coreg_data = [m_change, obj_ref_mode]
  399 +
  400 + if ref_mode_id:
  401 + coord_raw = dco.GetCoordinates(tracker.trk_init, tracker.tracker_id, ref_mode_id)
  402 + else:
  403 + coord_raw = np.array([None])
  404 +
  405 + obj_data = db.object_registration(obj_fiducials, obj_orients, coord_raw, m_change)
  406 + coreg_data.extend(obj_data)
  407 +
  408 + queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue]
  409 + jobs_list.append(dcr.CoordinateCorregistrate(ref_mode_id, tracker, coreg_data,
  410 + self.view_tracts, queues,
  411 + self.event, self.sleep_nav, tracker.tracker_id,
  412 + self.target))
  413 + else:
  414 + coreg_data = (m_change, 0)
  415 + queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue]
  416 + jobs_list.append(dcr.CoordinateCorregistrateNoObject(ref_mode_id, tracker, coreg_data,
  417 + self.view_tracts, queues,
  418 + self.event, self.sleep_nav))
  419 +
  420 + if not errors:
  421 + #TODO: Test the trigger thread
  422 + if self.trigger_state:
  423 + # self.trigger = trig.Trigger(nav_id)
  424 + jobs_list.append(trig.TriggerNew(self.trigger_queue, self.event, self.sleep_nav))
  425 +
  426 + if self.view_tracts:
  427 + # initialize Trekker parameters
  428 + slic = sl.Slice()
  429 + prj_data = prj.Project()
  430 + matrix_shape = tuple(prj_data.matrix_shape)
  431 + affine = slic.affine.copy()
  432 + affine[1, -1] -= matrix_shape[1]
  433 + affine_vtk = vtk_utils.numpy_to_vtkMatrix4x4(affine)
  434 + Publisher.sendMessage("Update marker offset state", create=True)
  435 + self.trk_inp = self.trekker, affine, self.seed_offset, self.n_tracts, self.seed_radius,\
  436 + self.n_threads, self.act_data, affine_vtk, matrix_shape[1]
  437 + # print("Appending the tract computation thread!")
  438 + queues = [self.coord_tracts_queue, self.tracts_queue]
  439 + if self.enable_act:
  440 + jobs_list.append(dti.ComputeTractsACTThread(self.trk_inp, queues, self.event, self.sleep_nav))
  441 + else:
  442 + jobs_list.append(dti.ComputeTractsThread(self.trk_inp, queues, self.event, self.sleep_nav))
  443 +
  444 + jobs_list.append(UpdateNavigationScene(vis_queues, vis_components,
  445 + self.event, self.sleep_nav))
  446 +
  447 + for jobs in jobs_list:
  448 + # jobs.daemon = True
  449 + jobs.start()
  450 + # del jobs
  451 +
  452 + # TODO: Separate ICP-related code from Navigation class. The first step would be to have this stuff
  453 + # in a separate function or so.
  454 + #
  455 + if not self.use_icp:
  456 + if dlg.ICPcorregistration(self.fre):
  457 + m_icp = self.OnICP(tracker, m_change)
  458 + self.icp_fre = db.calculate_fre(tracker_fiducials_raw, all_fiducials, ref_mode_id,
  459 + m_change, m_icp)
  460 + self.SetICP(True)
  461 +
  462 + def StopNavigation(self):
  463 + self.event.set()
  464 +
  465 + self.coord_queue.clear()
  466 + self.coord_queue.join()
  467 +
  468 + if self.trigger_state:
  469 + self.trigger_queue.clear()
  470 + self.trigger_queue.join()
  471 + if self.view_tracts:
  472 + self.coord_tracts_queue.clear()
  473 + self.coord_tracts_queue.join()
  474 +
  475 + self.tracts_queue.clear()
  476 + self.tracts_queue.join()
  477 +
  478 + vis_components = [self.trigger_state, self.view_tracts]
  479 + Publisher.sendMessage("Navigation status", nav_status=False, vis_status=vis_components)
  480 +
  481 + # ICP-related functions
  482 + #
  483 + # TODO: Refactor to preferably their own class.
  484 + #
  485 + def OnICP(self, tracker, m_change):
  486 + ref_mode_id = tracker.GetReferenceMode()
  487 +
  488 + dialog = dlg.ICPCorregistrationDialog(nav_prop=(m_change, tracker.tracker_id, tracker.trk_init, ref_mode_id))
  489 +
  490 + if dialog.ShowModal() == wx.ID_OK:
  491 + self.m_icp, point_coord, transformed_points, prev_error, final_error = dialog.GetValue()
  492 + # TODO: checkbox in the dialog to transfer the icp points to 3D viewer
  493 + #create markers
  494 + # for i in range(len(point_coord)):
  495 + # img_coord = point_coord[i][0],-point_coord[i][1],point_coord[i][2], 0, 0, 0
  496 + # transf_coord = transformed_points[i][0],-transformed_points[i][1],transformed_points[i][2], 0, 0, 0
  497 + # Publisher.sendMessage('Create marker', coord=img_coord, marker_id=None, colour=(1,0,0))
  498 + # Publisher.sendMessage('Create marker', coord=transf_coord, marker_id=None, colour=(0,0,1))
  499 + if self.m_icp is not None:
  500 + dlg.ReportICPerror(prev_error, final_error)
  501 + self.use_icp = True
  502 + else:
  503 + self.use_icp = False
  504 +
  505 + return self.m_icp
  506 +
  507 + def SetICP(self, use_icp):
  508 + self.use_icp = use_icp
  509 + self.icp_queue.put_nowait([self.use_icp, self.m_icp])
  510 +
  511 + def ResetICP(self):
  512 + self.use_icp = False
  513 + self.m_icp = None
  514 + self.fre = None
  515 + self.icp_fre = None
  516 +
  517 +
  518 +class Tracker():
  519 + def __init__(self):
  520 + self.trk_init = None
355 521 self.tracker_id = const.DEFAULT_TRACKER
356 522 self.ref_mode_id = const.DEFAULT_REF_MODE
357 523  
  524 + self.tracker_fiducials = np.full([3, 3], np.nan)
  525 + self.tracker_fiducials_raw = np.zeros((6, 6))
  526 +
  527 + self.tracker_connected = False
  528 +
  529 + def SetTracker(self, new_tracker):
  530 + if self.trk_init:
  531 + trck = self.trk_init[0]
  532 + else:
  533 + trck = None
  534 +
  535 + # Conditions check if click was on current selection and if any other tracker
  536 + # has been initialized before
  537 + if new_tracker != const.DISCTRACK and trck:
  538 + self.ResetTrackerFiducials()
  539 + Publisher.sendMessage('Update status text in GUI',
  540 + label=_("Disconnecting tracker..."))
  541 + Publisher.sendMessage('Remove sensors ID')
  542 + self.trk_init = dt.TrackerConnection(self.tracker_id, trck, 'disconnect')
  543 + Publisher.sendMessage('Remove object data')
  544 +
  545 + if not self.trk_init[0] and new_tracker:
  546 + Publisher.sendMessage('Update status text in GUI',
  547 + label=_("Tracker disconnected successfully"))
  548 + self.trk_init = dt.TrackerConnection(new_tracker, None, 'connect')
  549 + if not self.trk_init[0]:
  550 + self.tracked_connected = False
  551 + self.tracker_id = 0
  552 +
  553 + dlg.ShowNavigationTrackerWarning(self.tracker_id, self.trk_init[1])
  554 + print("Tracker not connected!")
  555 + else:
  556 + self.tracked_connected = True
  557 + self.tracker_id = new_tracker
  558 +
  559 + print("Tracker connected!")
  560 +
  561 + # TODO: const.DISCTRACK is not a tracker, so discoupling it from the actual trackers
  562 + # would make this cleaner.
  563 + #
  564 + elif new_tracker == const.DISCTRACK and trck:
  565 + self.ResetTrackerFiducials()
  566 + Publisher.sendMessage('Update status text in GUI',
  567 + label=_("Disconnecting tracker ..."))
  568 + Publisher.sendMessage('Remove sensors ID')
  569 + Publisher.sendMessage('Remove object data')
  570 + self.trk_init = dt.TrackerConnection(self.tracker_id, trck, 'disconnect')
  571 + if not self.trk_init[0]:
  572 + dlg.ShowNavigationTrackerWarning(self.tracker_id, 'disconnect')
  573 +
  574 + self.tracked_connected = False
  575 + self.tracker_id = 0
  576 +
  577 + Publisher.sendMessage('Update status text in GUI',
  578 + label=_("Tracker disconnected"))
  579 +
  580 + print("Tracker disconnected!")
  581 + else:
  582 + Publisher.sendMessage('Update status text in GUI',
  583 + label=_("Tracker still connected"))
  584 + print("Tracker still connected!")
  585 +
  586 + else:
  587 + # If trk_init is None try to connect. If doesn't succeed show dialog.
  588 + if new_tracker:
  589 + self.trk_init = dt.TrackerConnection(new_tracker, None, 'connect')
  590 + if not self.trk_init[0]:
  591 + dlg.ShowNavigationTrackerWarning(self.tracker_id, self.trk_init[1])
  592 +
  593 + self.tracker_id = 0
  594 + self.tracker_connected = False
  595 + else:
  596 + self.tracker_id = new_tracker
  597 + self.tracker_connected = True
  598 +
  599 + Publisher.sendMessage('Update tracker initializer',
  600 + nav_prop=(self.tracker_id, self.trk_init, self.ref_mode_id))
  601 +
  602 + def DisconnectTracker(self):
  603 + if self.tracker_connected:
  604 + self.ResetTrackerFiducials()
  605 + Publisher.sendMessage('Update status text in GUI',
  606 + label=_("Disconnecting tracker ..."))
  607 + Publisher.sendMessage('Remove sensors ID')
  608 + Publisher.sendMessage('Remove object data')
  609 + self.trk_init = dt.TrackerConnection(self.tracker_id, self.tracker_connected, 'disconnect')
  610 + if not self.trk_init[0]:
  611 + self.tracker_connected = False
  612 + self.tracker_id = 0
  613 +
  614 + Publisher.sendMessage('Update status text in GUI',
  615 + label=_("Tracker disconnected"))
  616 + print("Tracker disconnected!")
  617 + else:
  618 + Publisher.sendMessage('Update status text in GUI',
  619 + label=_("Tracker still connected"))
  620 + print("Tracker still connected!")
  621 +
  622 + def IsTrackerInitialized(self):
  623 + return self.trk_init and self.tracker_id and self.tracker_connected
  624 +
  625 + def AreTrackerFiducialsSet(self):
  626 + return not np.isnan(self.tracker_fiducials).any()
  627 +
  628 + def SetTrackerFiducial(self, fiducial_index):
  629 + coord = None
  630 +
  631 + coord_raw = dco.GetCoordinates(self.trk_init, self.tracker_id, self.ref_mode_id)
  632 +
  633 + if self.ref_mode_id:
  634 + coord = dco.dynamic_reference_m(coord_raw[0, :], coord_raw[1, :])
  635 + else:
  636 + coord = coord_raw[0, :]
  637 + coord[2] = -coord[2]
  638 +
  639 + # Update tracker fiducial with tracker coordinates
  640 + self.tracker_fiducials[fiducial_index, :] = coord[0:3]
  641 +
  642 + assert 0 <= fiducial_index <= 2, "Fiducial index out of range (0-2): {}".format(fiducial_index)
  643 +
  644 + self.tracker_fiducials_raw[2 * fiducial_index, :] = coord_raw[0, :]
  645 + self.tracker_fiducials_raw[2 * fiducial_index + 1, :] = coord_raw[1, :]
  646 +
  647 + print("Set tracker fiducial {} to coordinates {}.".format(fiducial_index, coord[0:3]))
  648 +
  649 + def ResetTrackerFiducials(self):
  650 + for m in range(3):
  651 + self.tracker_fiducials[m, :] = [np.nan, np.nan, np.nan]
  652 +
  653 + def GetTrackerFiducials(self):
  654 + return self.tracker_fiducials, self.tracker_fiducials_raw
  655 +
  656 + def GetTrackerInfo(self):
  657 + return self.trk_init, self.tracker_id, self.ref_mode_id
  658 +
  659 + def SetReferenceMode(self, value):
  660 + self.ref_mode_id = value
  661 +
  662 + # When ref mode is changed the tracker coordinates are set to zero
  663 + self.ResetTrackerFiducials()
  664 +
  665 + # Some trackers do not accept restarting within this time window
  666 + # TODO: Improve the restarting of trackers after changing reference mode
  667 + Publisher.sendMessage('Update tracker initializer',
  668 + nav_prop=(self.tracker_id, self.trk_init, self.ref_mode_id))
  669 +
  670 + def GetReferenceMode(self):
  671 + return self.ref_mode_id
  672 +
  673 + def UpdateUI(self, selection_ctrl, numctrls_coord, txtctrl_fre):
  674 + if self.tracker_connected:
  675 + selection_ctrl.SetSelection(self.tracker_id)
  676 + else:
  677 + selection_ctrl.SetSelection(0)
  678 +
  679 + # Update tracker location in the UI.
  680 + for m in range(3):
  681 + coord = self.tracker_fiducials[m, :]
  682 + for n in range(0, 3):
  683 + value = 0.0 if np.isnan(coord[n]) else float(coord[n])
  684 + numctrls_coord[m][n].SetValue(value)
  685 +
  686 + txtctrl_fre.SetValue('')
  687 + txtctrl_fre.SetBackgroundColour('WHITE')
  688 +
  689 + def get_trackers(self):
  690 + return const.TRACKERS
  691 +
  692 +
  693 +class NeuronavigationPanel(wx.Panel):
  694 + def __init__(self, parent):
  695 + wx.Panel.__init__(self, parent)
  696 + try:
  697 + default_colour = wx.SystemSettings.GetColour(wx.SYS_COLOUR_MENUBAR)
  698 + except AttributeError:
  699 + default_colour = wx.SystemSettings_GetColour(wx.SYS_COLOUR_MENUBAR)
  700 + self.SetBackgroundColour(default_colour)
  701 +
  702 + self.SetAutoLayout(1)
  703 +
  704 + self.__bind_events()
  705 +
  706 + # Initialize global variables
  707 + self.pedal_connection = PedalConnection() if HAS_PEDAL_CONNECTION else None
  708 + self.tracker = Tracker()
  709 + self.navigation = Navigation()
  710 +
  711 + self.nav_status = False
  712 +
358 713 # Initialize list of buttons and numctrls for wx objects
359 714 self.btns_coord = [None, None, None, None, None, None]
360 715 self.numctrls_coord = [[], [], [], [], [], []]
361 716  
362 717 # ComboBox for spatial tracker device selection
  718 + tracker_options = [_("Select tracker:")] + self.tracker.get_trackers()
  719 + select_tracker_elem = wx.ComboBox(self, -1, "",
  720 + choices=tracker_options, style=wx.CB_DROPDOWN|wx.CB_READONLY)
  721 +
363 722 tooltip = wx.ToolTip(_("Choose the tracking device"))
364   - choice_trck = wx.ComboBox(self, -1, "",
365   - choices=const.TRACKER, style=wx.CB_DROPDOWN|wx.CB_READONLY)
366   - choice_trck.SetToolTip(tooltip)
367   - choice_trck.SetSelection(const.DEFAULT_TRACKER)
368   - choice_trck.Bind(wx.EVT_COMBOBOX, partial(self.OnChoiceTracker, ctrl=choice_trck))
369   - self.choice_trck = choice_trck
  723 + select_tracker_elem.SetToolTip(tooltip)
  724 +
  725 + select_tracker_elem.SetSelection(const.DEFAULT_TRACKER)
  726 + select_tracker_elem.Bind(wx.EVT_COMBOBOX, partial(self.OnChooseTracker, ctrl=select_tracker_elem))
  727 + self.select_tracker_elem = select_tracker_elem
370 728  
371 729 # ComboBox for tracker reference mode
372 730 tooltip = wx.ToolTip(_("Choose the navigation reference mode"))
... ... @@ -374,7 +732,7 @@ class NeuronavigationPanel(wx.Panel):
374 732 choices=const.REF_MODE, style=wx.CB_DROPDOWN|wx.CB_READONLY)
375 733 choice_ref.SetSelection(const.DEFAULT_REF_MODE)
376 734 choice_ref.SetToolTip(tooltip)
377   - choice_ref.Bind(wx.EVT_COMBOBOX, partial(self.OnChoiceRefMode, ctrl=choice_trck))
  735 + choice_ref.Bind(wx.EVT_COMBOBOX, partial(self.OnChooseReferenceMode, ctrl=select_tracker_elem))
378 736 self.choice_ref = choice_ref
379 737  
380 738 # Toggle buttons for image fiducials
... ... @@ -422,12 +780,12 @@ class NeuronavigationPanel(wx.Panel):
422 780 btn_nav.Bind(wx.EVT_TOGGLEBUTTON, partial(self.OnNavigate, btn_nav=btn_nav))
423 781  
424 782 tooltip = wx.ToolTip(_(u"Refine the coregistration"))
425   - checkicp = wx.CheckBox(self, -1, _(' '))
426   - checkicp.SetValue(False)
427   - checkicp.Enable(False)
428   - checkicp.Bind(wx.EVT_CHECKBOX, partial(self.Oncheckicp, ctrl=checkicp))
429   - checkicp.SetToolTip(tooltip)
430   - self.checkicp = checkicp
  783 + checkbox_icp = wx.CheckBox(self, -1, _(' '))
  784 + checkbox_icp.SetValue(False)
  785 + checkbox_icp.Enable(False)
  786 + checkbox_icp.Bind(wx.EVT_CHECKBOX, partial(self.OnCheckboxICP, ctrl=checkbox_icp))
  787 + checkbox_icp.SetToolTip(tooltip)
  788 + self.checkbox_icp = checkbox_icp
431 789  
432 790 # An indicator for pedal trigger
433 791 if HAS_PEDAL_CONNECTION and self.pedal_connection.in_use:
... ... @@ -454,7 +812,7 @@ class NeuronavigationPanel(wx.Panel):
454 812  
455 813 # Sizer to group all GUI objects
456 814 choice_sizer = wx.FlexGridSizer(rows=1, cols=2, hgap=5, vgap=5)
457   - choice_sizer.AddMany([(choice_trck, wx.LEFT),
  815 + choice_sizer.AddMany([(select_tracker_elem, wx.LEFT),
458 816 (choice_ref, wx.RIGHT)])
459 817  
460 818 coord_sizer = wx.GridBagSizer(hgap=5, vgap=5)
... ... @@ -471,7 +829,7 @@ class NeuronavigationPanel(wx.Panel):
471 829 (txtctrl_fre, 0, wx.ALIGN_CENTER_HORIZONTAL | wx.ALIGN_CENTER_VERTICAL),
472 830 (btn_nav, 0, wx.ALIGN_CENTER_HORIZONTAL | wx.ALIGN_CENTER_VERTICAL),
473 831 (txt_icp, 0, wx.ALIGN_CENTER_HORIZONTAL | wx.ALIGN_CENTER_VERTICAL),
474   - (checkicp, 0, wx.ALIGN_LEFT | wx.ALIGN_CENTER_VERTICAL)])
  832 + (checkbox_icp, 0, wx.ALIGN_CENTER_HORIZONTAL | wx.ALIGN_CENTER_VERTICAL)])
475 833  
476 834 pedal_sizer = wx.FlexGridSizer(rows=1, cols=2, hgap=5, vgap=5)
477 835 if HAS_PEDAL_CONNECTION and self.pedal_connection.in_use:
... ... @@ -544,172 +902,87 @@ class NeuronavigationPanel(wx.Panel):
544 902 def SetImageFiducial(self, fiducial_name, coord):
545 903 fiducial = self.GetFiducialByAttribute(const.IMAGE_FIDUCIALS, 'fiducial_name', fiducial_name)
546 904 fiducial_index = fiducial['fiducial_index']
547   - self.fiducials[fiducial_index, :] = coord
548 905  
549   - print("Set image fiducial {} to coordinates {}".format(fiducial_name, coord))
  906 + self.navigation.SetImageFiducial(fiducial_index, coord)
550 907  
551 908 def SetTrackerFiducial(self, fiducial_name):
552   - fiducial = self.GetFiducialByAttribute(const.TRACKER_FIDUCIALS, 'fiducial_name', fiducial_name)
553   - fiducial_index = fiducial['fiducial_index']
554   -
555   - coord = None
556   -
557   - if not(self.trk_init and self.tracker_id):
  909 + if not self.tracker.IsTrackerInitialized():
558 910 dlg.ShowNavigationTrackerWarning(0, 'choose')
559 911 return
560 912  
561   - # if self.tracker_id == const.DEBUGTRACK:
562   - # if btn_id == 3:
563   - # coord1 = np.array([-120., 0., 0., 0., 0., 0.])
564   - # elif btn_id == 4:
565   - # coord1 = np.array([120., 0., 0., 0., 0., 0.])
566   - # elif btn_id == 5:
567   - # coord1 = np.array([0., 120., 0., 0., 0., 0.])
568   - # coord2 = np.zeros([3, 6])
569   - # coord_raw = np.vstack([coord1, coord2])
570   - # else:
571   - coord_raw = dco.GetCoordinates(self.trk_init, self.tracker_id, self.ref_mode_id)
572   -
573   - if self.ref_mode_id:
574   - coord = dco.dynamic_reference_m(coord_raw[0, :], coord_raw[1, :])
575   - else:
576   - coord = coord_raw[0, :]
577   - coord[2] = -coord[2]
578   -
579   - # Update tracker fiducial with tracker coordinates
580   - self.fiducials[fiducial_index, :] = coord[0:3]
581   -
582   - if fiducial_index == 3:
583   - self.fiducials_raw[0, :] = coord_raw[0, :]
584   - self.fiducials_raw[1, :] = coord_raw[1, :]
585   - elif fiducial_index == 4:
586   - self.fiducials_raw[2, :] = coord_raw[0, :]
587   - self.fiducials_raw[3, :] = coord_raw[1, :]
588   - else:
589   - self.fiducials_raw[4, :] = coord_raw[0, :]
590   - self.fiducials_raw[5, :] = coord_raw[1, :]
591   -
592   - # Update tracker location in the UI.
593   - for n in [0, 1, 2]:
594   - self.numctrls_coord[fiducial_index][n].SetValue(float(coord[n]))
595   -
596   - print("Set tracker fiducial {} to coordinates {}.".format(fiducial_name, coord[0:3]))
597   -
598   - def UpdateNavigationStatus(self, nav_status, vis_status):
599   - self.nav_status = nav_status
600   - if nav_status and (self.m_icp is not None):
601   - self.checkicp.Enable(True)
602   - else:
603   - self.checkicp.Enable(False)
604   - #self.checkicp.SetValue(False)
605   -
606   - def UpdateFRE(self, fre):
607   - # TODO: Exhibit FRE in a warning dialog and only starts navigation after user clicks ok
608   - self.txtctrl_fre.SetValue(str(round(fre, 2)))
609   - if fre <= 3:
610   - self.txtctrl_fre.SetBackgroundColour('GREEN')
611   - else:
612   - self.txtctrl_fre.SetBackgroundColour('RED')
613   -
614   - def UpdateTrekkerObject(self, data):
615   - # self.trk_inp = data
616   - self.trekker = data
617   -
618   - def UpdateNumTracts(self, data):
619   - self.n_tracts = data
620   -
621   - def UpdateSeedOffset(self, data):
622   - self.seed_offset = data
623   -
624   - def UpdateSeedRadius(self, data):
625   - self.seed_radius = data
626   -
627   - def UpdateSleep(self, data):
628   - self.sleep_nav = data
629   -
630   - def UpdateNumberThreads(self, data):
631   - self.n_threads = data
  913 + fiducial = self.GetFiducialByAttribute(const.TRACKER_FIDUCIALS, 'fiducial_name', fiducial_name)
  914 + fiducial_index = fiducial['fiducial_index']
632 915  
633   - def UpdateTractsVisualization(self, data):
634   - self.view_tracts = data
  916 + self.tracker.SetTrackerFiducial(fiducial_index)
635 917  
636   - def UpdateACTData(self, data):
637   - self.act_data = data
  918 + self.tracker.UpdateUI(self.select_tracker_elem, self.numctrls_coord[3:6], self.txtctrl_fre)
638 919  
639 920 def UpdateNavigationStatus(self, nav_status, vis_status):
640 921 self.nav_status = nav_status
641   - if nav_status and (self.m_icp is not None):
642   - self.checkicp.Enable(True)
643   - else:
644   - self.checkicp.Enable(False)
645   - #self.checkicp.SetValue(False)
646   -
647   - def UpdateFRE(self, fre):
648   - # TODO: Exhibit FRE in a warning dialog and only starts navigation after user clicks ok
649   - self.txtctrl_fre.SetValue(str(round(fre, 2)))
650   - if fre <= 3:
651   - self.txtctrl_fre.SetBackgroundColour('GREEN')
  922 + if nav_status and self.navigation.m_icp is not None:
  923 + self.checkbox_icp.Enable(True)
652 924 else:
653   - self.txtctrl_fre.SetBackgroundColour('RED')
  925 + self.checkbox_icp.Enable(False)
654 926  
655 927 def UpdateTrekkerObject(self, data):
656 928 # self.trk_inp = data
657   - self.trekker = data
  929 + self.navigation.trekker = data
658 930  
659 931 def UpdateNumTracts(self, data):
660   - self.n_tracts = data
  932 + self.navigation.n_tracts = data
661 933  
662 934 def UpdateSeedOffset(self, data):
663   - self.seed_offset = data
  935 + self.navigation.seed_offset = data
664 936  
665 937 def UpdateSeedRadius(self, data):
666   - self.seed_radius = data
  938 + self.navigation.seed_radius = data
667 939  
668 940 def UpdateSleep(self, data):
669   - self.sleep_nav = data
  941 + self.navigation.sleep_nav = data
670 942  
671 943 def UpdateNumberThreads(self, data):
672   - self.n_threads = data
  944 + self.navigation.n_threads = data
673 945  
674 946 def UpdateTractsVisualization(self, data):
675   - self.view_tracts = data
  947 + self.navigation.view_tracts = data
676 948  
677 949 def UpdateACTData(self, data):
678   - self.act_data = data
  950 + self.navigation.act_data = data
679 951  
680 952 def UpdateTarget(self, coord):
681   - self.target = coord
  953 + self.navigation.target = coord
682 954  
683 955 def EnableACT(self, data):
684   - self.enable_act = data
  956 + self.navigation.enable_act = data
685 957  
686 958 def UpdateImageCoordinates(self, position):
687 959 # TODO: Change from world coordinates to matrix coordinates. They are better for multi software communication.
688   - self.current_coord = position
  960 + self.navigation.current_coord = position
  961 +
689 962 for m in [0, 1, 2]:
690 963 if not self.btns_coord[m].GetValue():
691 964 for n in [0, 1, 2]:
692   - self.numctrls_coord[m][n].SetValue(float(self.current_coord[n]))
  965 + self.numctrls_coord[m][n].SetValue(float(position[n]))
693 966  
694 967 def UpdateObjectRegistration(self, data=None):
695   - if data:
696   - self.obj_reg = data
697   - self.obj_reg_status = True
698   - else:
699   - self.obj_reg = None
700   - self.obj_reg_status = False
  968 + self.navigation.obj_reg = data
701 969  
702 970 def UpdateTrackObjectState(self, evt=None, flag=None, obj_name=None, polydata=None):
703   - self.track_obj = flag
  971 + self.navigation.track_obj = flag
704 972  
705 973 def UpdateTriggerState(self, trigger_state):
706   - self.trigger_state = trigger_state
  974 + self.navigation.trigger_state = trigger_state
  975 +
  976 + def ResetICP(self):
  977 + self.navigation.ResetICP()
  978 + self.checkbox_icp.Enable(False)
  979 + self.checkbox_icp.SetValue(False)
707 980  
708 981 def OnDisconnectTracker(self):
709   - if self.tracker_id:
710   - dt.TrackerConnection(self.tracker_id, self.trk_init[0], 'disconnect')
  982 + self.tracker.DisconnectTracker()
  983 + self.ResetICP()
711 984  
712   - def OnChoiceTracker(self, evt, ctrl):
  985 + def OnChooseTracker(self, evt, ctrl):
713 986 Publisher.sendMessage('Update status text in GUI',
714 987 label=_("Configuring tracker ..."))
715 988 if hasattr(evt, 'GetSelection'):
... ... @@ -717,81 +990,16 @@ class NeuronavigationPanel(wx.Panel):
717 990 else:
718 991 choice = const.DISCTRACK
719 992  
720   - if self.trk_init:
721   - trck = self.trk_init[0]
722   - else:
723   - trck = None
724   -
725   - # Conditions check if click was on current selection and if any other tracker
726   - # has been initialized before
727   - if trck and choice != const.DISCTRACK:
728   - self.ResetTrackerFiducials()
729   - self.ResetIcp()
730   - Publisher.sendMessage('Update status text in GUI',
731   - label=_("Disconnecting tracker..."))
732   - Publisher.sendMessage('Remove sensors ID')
733   - self.trk_init = dt.TrackerConnection(self.tracker_id, trck, 'disconnect')
734   - Publisher.sendMessage('Remove object data')
735   - self.tracker_id = choice
736   - if not self.trk_init[0] and choice:
737   - Publisher.sendMessage('Update status text in GUI',
738   - label=_("Tracker disconnected successfully"))
739   - self.trk_init = dt.TrackerConnection(self.tracker_id, None, 'connect')
740   - if not self.trk_init[0]:
741   - dlg.ShowNavigationTrackerWarning(self.tracker_id, self.trk_init[1])
742   - ctrl.SetSelection(0)
743   - print("Tracker not connected!")
744   - else:
745   - ctrl.SetSelection(self.tracker_id)
746   - print("Tracker connected!")
747   - elif choice == const.DISCTRACK:
748   - if trck:
749   - self.ResetTrackerFiducials()
750   - self.ResetIcp()
751   - Publisher.sendMessage('Update status text in GUI',
752   - label=_("Disconnecting tracker ..."))
753   - Publisher.sendMessage('Remove sensors ID')
754   - Publisher.sendMessage('Remove object data')
755   - self.trk_init = dt.TrackerConnection(self.tracker_id, trck, 'disconnect')
756   - if not self.trk_init[0]:
757   - if evt is not False:
758   - dlg.ShowNavigationTrackerWarning(self.tracker_id, 'disconnect')
759   - self.tracker_id = 0
760   - ctrl.SetSelection(self.tracker_id)
761   - Publisher.sendMessage('Update status text in GUI',
762   - label=_("Tracker disconnected"))
763   - print("Tracker disconnected!")
764   - else:
765   - Publisher.sendMessage('Update status text in GUI',
766   - label=_("Tracker still connected"))
767   - print("Tracker still connected!")
768   - else:
769   - ctrl.SetSelection(self.tracker_id)
770   -
771   - else:
772   - # If trk_init is None try to connect. If doesn't succeed show dialog.
773   - if choice:
774   - self.tracker_id = choice
775   - self.trk_init = dt.TrackerConnection(self.tracker_id, None, 'connect')
776   - if not self.trk_init[0]:
777   - dlg.ShowNavigationTrackerWarning(self.tracker_id, self.trk_init[1])
778   - self.tracker_id = 0
779   - ctrl.SetSelection(self.tracker_id)
  993 + self.tracker.SetTracker(choice)
  994 + self.ResetICP()
  995 + self.tracker.UpdateUI(ctrl, self.numctrls_coord[3:6], self.txtctrl_fre)
780 996  
781 997 Publisher.sendMessage('Update status text in GUI', label=_("Ready"))
782   - Publisher.sendMessage('Update tracker initializer',
783   - nav_prop=(self.tracker_id, self.trk_init, self.ref_mode_id))
784 998  
785   - def OnChoiceRefMode(self, evt, ctrl):
786   - # When ref mode is changed the tracker coordinates are set to zero
787   - self.ref_mode_id = evt.GetSelection()
788   - self.ResetTrackerFiducials()
789   - self.ResetIcp()
790   - # Some trackers do not accept restarting within this time window
791   - # TODO: Improve the restarting of trackers after changing reference mode
792   - # self.OnChoiceTracker(None, ctrl)
793   - Publisher.sendMessage('Update tracker initializer',
794   - nav_prop=(self.tracker_id, self.trk_init, self.ref_mode_id))
  999 + def OnChooseReferenceMode(self, evt, ctrl):
  1000 + self.tracker.SetReferenceMode(evt.GetSelection())
  1001 + self.ResetICP()
  1002 +
795 1003 print("Reference mode changed!")
796 1004  
797 1005 def OnImageFiducials(self, n, evt):
... ... @@ -818,205 +1026,69 @@ class NeuronavigationPanel(wx.Panel):
818 1026 fiducial_name = const.TRACKER_FIDUCIALS[n]['fiducial_name']
819 1027 Publisher.sendMessage('Set tracker fiducial', fiducial_name=fiducial_name)
820 1028  
821   - def OnICP(self, m_change):
822   - dialog = dlg.ICPCorregistrationDialog(nav_prop=(m_change, self.tracker_id, self.trk_init, self.ref_mode_id))
823   - if dialog.ShowModal() == wx.ID_OK:
824   - self.m_icp, point_coord, transformed_points, prev_error, final_error = dialog.GetValue()
825   - #TODO: checkbox in the dialog to transfer the icp points to 3D viewer
826   - #create markers
827   - # for i in range(len(point_coord)):
828   - # img_coord = point_coord[i][0],-point_coord[i][1],point_coord[i][2], 0, 0, 0
829   - # transf_coord = transformed_points[i][0],-transformed_points[i][1],transformed_points[i][2], 0, 0, 0
830   - # Publisher.sendMessage('Create marker', coord=img_coord, marker_id=None, colour=(1,0,0))
831   - # Publisher.sendMessage('Create marker', coord=transf_coord, marker_id=None, colour=(0,0,1))
832   - if self.m_icp is not None:
833   - dlg.ReportICPerror(prev_error, final_error)
834   - self.checkicp.Enable(True)
835   - self.checkicp.SetValue(True)
836   - self.icp = True
837   - else:
838   - self.checkicp.Enable(False)
839   - self.checkicp.SetValue(False)
840   - self.icp = False
841   -
842   - return self.m_icp
843   -
844   - def Oncheckicp(self, evt, ctrl):
845   - if ctrl.GetValue() and evt and (self.m_icp is not None):
846   - self.icp = True
847   - else:
848   - self.icp = False
849   - self.ctrl_icp()
850   -
851   - def ctrl_icp(self):
852   - if self.icp:
853   - self.UpdateFRE(self.icp_fre)
854   - else:
855   - self.UpdateFRE(self.fre)
856   - self.icp_queue.put_nowait([self.icp, self.m_icp])
857   - #print(self.icp, self.m_icp)
858   -
859 1029 def onStopNavigation(self):
860   - choice_trck = self.choice_trck
  1030 + select_tracker_elem = self.select_tracker_elem
861 1031 choice_ref = self.choice_ref
862 1032  
863   - self.event.set()
864   -
865   - # print("coord unfinished: {}, queue {}", self.coord_queue.unfinished_tasks, self.coord_queue.qsize())
866   - # print("coord_tracts unfinished: {}, queue {}", self.coord_tracts_queue.unfinished_tasks, self.coord_tracts_queue.qsize())
867   - # print("tracts unfinished: {}, queue {}", self.tracts_queue.unfinished_tasks, self.tracts_queue.qsize())
868   - self.coord_queue.clear()
869   - # self.visualization_queue.clear()
870   - if self.trigger_state:
871   - self.trigger_queue.clear()
872   - if self.view_tracts:
873   - self.coord_tracts_queue.clear()
874   - self.tracts_queue.clear()
875   -
876   - # print("coord after unfinished: {}, queue {}", self.coord_queue.unfinished_tasks, self.coord_queue.qsize())
877   - # print("coord_tracts after unfinished: {}, queue {}", self.coord_tracts_queue.unfinished_tasks, self.coord_tracts_queue.qsize())
878   - # print("tracts after unfinished: {}, queue {}", self.tracts_queue.unfinished_tasks, self.tracts_queue.qsize())
879   - self.coord_queue.join()
880   - # self.visualization_queue.join()
881   - if self.trigger_state:
882   - self.trigger_queue.join()
883   - if self.view_tracts:
884   - self.coord_tracts_queue.join()
885   - self.tracts_queue.join()
886   -
887   - # print("coord join unfinished: {}, queue {}", self.coord_queue.unfinished_tasks, self.coord_queue.qsize())
888   - # print("vis join unfinished: {}, queue {}", self.visualization_queue.unfinished_tasks, self.visualization_queue.qsize())
  1033 + self.navigation.StopNavigation()
889 1034  
890 1035 # Enable all navigation buttons
891 1036 choice_ref.Enable(True)
892   - choice_trck.Enable(True)
  1037 + select_tracker_elem.Enable(True)
  1038 +
893 1039 for btn_c in self.btns_coord:
894 1040 btn_c.Enable(True)
895 1041  
896   - # if self.trigger_state:
897   - # self.trigger.stop()
  1042 + def UpdateFiducialRegistrationError(self):
  1043 + self.navigation.UpdateFiducialRegistrationError(self.tracker)
  1044 + fre, fre_ok = self.navigation.GetFiducialRegistrationError()
898 1045  
899   - vis_components = [self.trigger_state, self.view_tracts]
900   - Publisher.sendMessage("Navigation status", nav_status=False, vis_status=vis_components)
  1046 + self.txtctrl_fre.SetValue(str(round(fre, 2)))
  1047 + if fre_ok:
  1048 + self.txtctrl_fre.SetBackgroundColour('GREEN')
  1049 + else:
  1050 + self.txtctrl_fre.SetBackgroundColour('RED')
  1051 +
  1052 + return fre_ok
901 1053  
902 1054 def onStartNavigation(self):
903   - choice_trck = self.choice_trck
  1055 + select_tracker_elem = self.select_tracker_elem
904 1056 choice_ref = self.choice_ref
905 1057  
906   - errors = False
907   -
908   - # initialize jobs list
909   - jobs_list = []
910   - vis_components = [self.trigger_state, self.view_tracts]
911   - vis_queues = [self.coord_queue, self.trigger_queue, self.tracts_queue, self.icp_queue]
912   -
913   - if np.isnan(self.fiducials).any():
  1058 + if not self.tracker.AreTrackerFiducialsSet() or not self.navigation.AreImageFiducialsSet():
914 1059 wx.MessageBox(_("Invalid fiducials, select all coordinates."), _("InVesalius 3"))
915 1060  
916   - elif not self.trk_init[0] or not self.tracker_id:
  1061 + elif not self.tracker.IsTrackerInitialized():
917 1062 dlg.ShowNavigationTrackerWarning(0, 'choose')
918 1063 errors = True
919 1064  
920 1065 else:
921   - if self.event.is_set():
922   - self.event.clear()
  1066 + if not self.UpdateFiducialRegistrationError():
  1067 + # TODO: Exhibit FRE in a warning dialog and only starts navigation after user clicks ok
  1068 + print("WARNING: Fiducial registration error too large.")
923 1069  
924   - # prepare GUI for navigation
925   - Publisher.sendMessage("Navigation status", nav_status=True, vis_status=vis_components)
  1070 + # Prepare GUI for navigation.
926 1071 Publisher.sendMessage("Toggle Cross", id=const.SLICE_STATE_CROSS)
927 1072 Publisher.sendMessage("Hide current mask")
928 1073  
929   - # disable all navigation buttons
  1074 + # Disable all navigation buttons.
930 1075 choice_ref.Enable(False)
931   - choice_trck.Enable(False)
  1076 + select_tracker_elem.Enable(False)
932 1077 for btn_c in self.btns_coord:
933 1078 btn_c.Enable(False)
934 1079  
935   - # fiducials matrix
936   - m_change = tr.affine_matrix_from_points(self.fiducials[3:, :].T, self.fiducials[:3, :].T,
937   - shear=False, scale=False)
938   - # initialize spatial tracker parameters
939   - tracker_mode = self.trk_init, self.tracker_id, self.ref_mode_id
940   -
941   - # compute fiducial registration error (FRE)
942   - if not self.icp_fre:
943   - self.fre = db.calculate_fre(self.fiducials_raw, self.fiducials, self.ref_mode_id, m_change)
944   - self.UpdateFRE(self.fre)
945   -
946   - if self.track_obj:
947   - # if object tracking is selected
948   - if not self.obj_reg_status:
949   - # check if object registration was performed
950   - wx.MessageBox(_("Perform coil registration before navigation."), _("InVesalius 3"))
951   - errors = True
952   - else:
953   - # if object registration was correctly performed continue with navigation
954   - # obj_reg[0] is object 3x3 fiducial matrix and obj_reg[1] is 3x3 orientation matrix
955   - obj_fiducials, obj_orients, obj_ref_mode, obj_name = self.obj_reg
956   -
957   - coreg_data = [m_change, obj_ref_mode]
958   -
959   - if self.ref_mode_id:
960   - coord_raw = dco.GetCoordinates(self.trk_init, self.tracker_id, self.ref_mode_id)
961   - else:
962   - coord_raw = np.array([None])
963   -
964   - obj_data = db.object_registration(obj_fiducials, obj_orients, coord_raw, m_change)
965   - coreg_data.extend(obj_data)
966   -
967   - queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue]
968   - jobs_list.append(dcr.CoordinateCorregistrate(self.ref_mode_id, tracker_mode, coreg_data,
969   - self.view_tracts, queues,
970   - self.event, self.sleep_nav, self.tracker_id,
971   - self.target))
972   - else:
973   - coreg_data = (m_change, 0)
974   - queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue]
975   - jobs_list.append(dcr.CoordinateCorregistrateNoObject(self.ref_mode_id, tracker_mode, coreg_data,
976   - self.view_tracts, queues,
977   - self.event, self.sleep_nav))
978   -
979   - if not errors:
980   - #TODO: Test the trigger thread
981   - if self.trigger_state:
982   - # self.trigger = trig.Trigger(nav_id)
983   - jobs_list.append(trig.TriggerNew(self.trigger_queue, self.event, self.sleep_nav))
984   -
985   - if self.view_tracts:
986   - # initialize Trekker parameters
987   - slic = sl.Slice()
988   - prj_data = prj.Project()
989   - matrix_shape = tuple(prj_data.matrix_shape)
990   - affine = slic.affine.copy()
991   - affine[1, -1] -= matrix_shape[1]
992   - affine_vtk = vtk_utils.numpy_to_vtkMatrix4x4(affine)
993   - Publisher.sendMessage("Update marker offset state", create=True)
994   - self.trk_inp = self.trekker, affine, self.seed_offset, self.n_tracts, self.seed_radius,\
995   - self.n_threads, self.act_data, affine_vtk, matrix_shape[1]
996   - # print("Appending the tract computation thread!")
997   - queues = [self.coord_tracts_queue, self.tracts_queue]
998   - if self.enable_act:
999   - jobs_list.append(dti.ComputeTractsACTThread(self.trk_inp, queues, self.event, self.sleep_nav))
1000   - else:
1001   - jobs_list.append(dti.ComputeTractsThread(self.trk_inp, queues, self.event, self.sleep_nav))
1002   -
1003   - jobs_list.append(UpdateNavigationScene(vis_queues, vis_components,
1004   - self.event, self.sleep_nav))
  1080 + self.navigation.StartNavigation(self.tracker)
1005 1081  
1006   - for jobs in jobs_list:
1007   - # jobs.daemon = True
1008   - jobs.start()
1009   - # del jobs
  1082 + # Update FRE once more after starting the navigation, due to the optional use of ICP,
  1083 + # which improves FRE.
  1084 + self.UpdateFiducialRegistrationError()
1010 1085  
1011   - if not self.checkicp.GetValue():
1012   - if dlg.ICPcorregistration(self.fre):
1013   - m_icp = self.OnICP(m_change)
1014   - self.icp_fre = db.calculate_fre(self.fiducials_raw, self.fiducials, self.ref_mode_id,
1015   - m_change, m_icp)
1016   - self.ctrl_icp()
  1086 + if self.navigation.use_icp:
  1087 + self.checkbox_icp.Enable(True)
  1088 + self.checkbox_icp.SetValue(True)
1017 1089  
1018 1090 def OnNavigate(self, evt, btn_nav):
1019   - choice_trck = self.choice_trck
  1091 + select_tracker_elem = self.select_tracker_elem
1020 1092 choice_ref = self.choice_ref
1021 1093  
1022 1094 nav_id = btn_nav.GetValue()
... ... @@ -1041,28 +1113,15 @@ class NeuronavigationPanel(wx.Panel):
1041 1113 for n in range(0, 3):
1042 1114 self.numctrls_coord[m][n].SetValue(0.0)
1043 1115  
1044   - def ResetTrackerFiducials(self):
1045   - for m in range(3, 6):
1046   - self.fiducials[m, :] = [np.nan, np.nan, np.nan]
1047   - for n in range(0, 3):
1048   - self.numctrls_coord[m][n].SetValue(0.0)
1049   -
1050   - self.txtctrl_fre.SetValue('')
1051   - self.txtctrl_fre.SetBackgroundColour('WHITE')
1052   -
1053   - def ResetIcp(self):
1054   - self.m_icp = None
1055   - self.fre = None
1056   - self.icp_fre = None
1057   - self.icp = False
1058   - self.checkicp.Enable(False)
1059   - self.checkicp.SetValue(False)
  1116 + def OnCheckboxICP(self, evt, ctrl):
  1117 + self.navigation.SetICP(ctrl.GetValue())
  1118 + self.UpdateFiducialRegistrationError()
1060 1119  
1061 1120 def OnCloseProject(self):
1062 1121 self.ResetTrackerFiducials()
1063 1122 self.ResetImageFiducials()
1064   - self.ResetIcp()
1065   - self.OnChoiceTracker(False, self.choice_trck)
  1123 + self.navigation.ResetICP()
  1124 + self.OnChooseTracker(False, self.select_tracker_elem)
1066 1125 Publisher.sendMessage('Update object registration')
1067 1126 Publisher.sendMessage('Update track object state', flag=False, obj_name=False)
1068 1127 Publisher.sendMessage('Delete all markers')
... ...