Navigation ICP coregistration improvements (#299)

* MOD: Get raw coordinates to create points from probe * ENH: Improve processing data with a new thread -ADD: Enable/Disable Done and Apply buttons * MOD: show the amount of poses recorded * FIX: removes ICP matrix after disconnect tracker

Navigation ICP coregistration improvements (#299)
* MOD: Get raw coordinates to create points from probe * ENH: Improve processing data with a new thread -ADD: Enable/Disable Done and Apply buttons * MOD: show the amount of poses recorded * FIX: removes ICP matrix after disconnect tracker
Renan · GitHub
1 parent 7fae7387
Showing 3 changed files with 79 additions and 28 deletions Show diff stats
invesalius/data/vtk_utils.py
invesalius/gui/dialogs.py
invesalius/gui/task_navigator.py
@@ -148,6 +148,9 @@ class Text(object):
             except(UnicodeEncodeError):
                 self.mapper.SetInput(value.encode("utf-8", errors='replace'))
+    def GetValue(self):
+        return self.mapper.GetInput()
+
     def SetCoilDistanceValue(self, value):
         #TODO: Not being used anymore. Can be deleted.
         if isinstance(value, int) or isinstance(value, float):
@@ -59,7 +59,8 @@ import invesalius.data.coordinates as dco
 import invesalius.gui.widgets.gradient as grad
 import invesalius.session as ses
 import invesalius.utils as utils
-import invesalius.data.bases as bases
+import invesalius.data.vtk_utils as vtku
+import invesalius.data.coregistration as dcr
 from invesalius.gui.widgets.inv_spinctrl import InvSpinCtrl, InvFloatSpinCtrl
 from invesalius.gui.widgets import clut_imagedata
 from invesalius.gui.widgets.clut_imagedata import CLUTImageDataWidget, EVT_CLUT_NODE_CHANGED
@@ -3536,10 +3537,9 @@ class ICPCorregistrationDialog(wx.Dialog):
     def __init__(self, nav_prop):
         import invesalius.project as prj
-        self.__bind_events()
-
-        self.tracker_id = nav_prop[0]
-        self.trk_init = nav_prop[1]
+        self.m_change = nav_prop[0]
+        self.tracker_id = nav_prop[1]
+        self.trk_init = nav_prop[2]
         self.obj_ref_id = 2
         self.obj_name = None
         self.obj_actor = None
@@ -3563,12 +3563,6 @@ class ICPCorregistrationDialog(wx.Dialog):
         self._init_gui()
-    def __bind_events(self):
-        Publisher.subscribe(self.UpdateCurrentCoord, 'Set cross focal point')
-
-    def UpdateCurrentCoord(self, position):
-        self.current_coord = position[:]
-
     def _init_gui(self):
         self.interactor = wxVTKRenderWindowInteractor(self, -1, size=self.GetSize())
         self.interactor.Enable(1)
@@ -3617,12 +3611,15 @@ class ICPCorregistrationDialog(wx.Dialog):
         btn_reset.Bind(wx.EVT_BUTTON, self.OnReset)
         btn_apply_icp = wx.Button(self, -1, label=_('Apply registration'))
-        btn_apply_icp.Bind(wx.EVT_BUTTON, self.OnICP)
+        btn_apply_icp.Bind(wx.EVT_BUTTON, self.thread_ICP_start, btn_apply_icp)
+        btn_apply_icp.Enable(False)
+        self.btn_apply_icp = btn_apply_icp
-        # Buttons to finish or cancel object registration
         tooltip = wx.ToolTip(_(u"Refine done"))
         btn_ok = wx.Button(self, wx.ID_OK, _(u"Done"))
         btn_ok.SetToolTip(tooltip)
+        btn_ok.Enable(False)
+        self.btn_ok = btn_ok
         btn_cancel = wx.Button(self, wx.ID_CANCEL)
         btn_cancel.SetHelpText("")
@@ -3667,20 +3664,41 @@ class ICPCorregistrationDialog(wx.Dialog):
         obj_actor.SetMapper(mapper)
         self.obj_actor = obj_actor
+        poses_recorded = vtku.Text()
+        poses_recorded.SetSize(const.TEXT_SIZE_LARGE)
+        poses_recorded.SetPosition((const.X, const.Y))
+        poses_recorded.ShadowOff()
+        poses_recorded.SetValue("Poses recorded: ")
+
+        collect_points = vtku.Text()
+        collect_points.SetSize(const.TEXT_SIZE_LARGE)
+        collect_points.SetPosition((const.X+0.35, const.Y))
+        collect_points.ShadowOff()
+        collect_points.SetValue("0")
+        self.collect_points = collect_points
+
         self.ren.AddActor(obj_actor)
+        self.ren.AddActor(poses_recorded.actor)
+        self.ren.AddActor(collect_points.actor)
         self.ren.ResetCamera()
         self.interactor.Render()
     def RemoveActor(self):
-        #self.ren.RemoveActor(self.obj_actor)
         self.ren.RemoveAllViewProps()
         self.point_coord = []
         self.transformed_points = []
         self.m_icp = None
         self.SetProgress(0)
+        self.btn_apply_icp.Enable(False)
+        self.btn_ok.Enable(False)
         self.ren.ResetCamera()
         self.interactor.Render()
+    def GetCurrentCoord(self):
+        coord_raw = dco.GetCoordinates(self.trk_init, self.tracker_id, const.DYNAMIC_REF)
+        coord, _ = dcr.corregistrate_dynamic((self.m_change, 0), coord_raw, const.DEFAULT_REF_MODE, [None, None])
+        return coord[:3]
+
     def AddMarker(self, size, colour, coord):
         """
         Points are rendered into the scene. These points give visual information about the registration.
@@ -3713,11 +3731,19 @@ class ICPCorregistrationDialog(wx.Dialog):
         self.ren.AddActor(sphere_actor)
         self.point_coord.append([x, y, z])
-        self.Refresh()
+        self.collect_points.SetValue(str(int(self.collect_points.GetValue()) + 1))
+
+        self.interactor.Render()
+
+        if len(self.point_coord) >= 5 and self.btn_apply_icp.IsEnabled() is False:
+            self.btn_apply_icp.Enable(True)
+
+        if self.progress.GetValue() != 0:
+            self.SetProgress(0)
     def SetProgress(self, progress):
         self.progress.SetValue(progress * 100)
-        self.Refresh()
+        self.interactor.Render()
     def vtkmatrix_to_numpy(self, matrix):
         """
@@ -3759,7 +3785,8 @@ class ICPCorregistrationDialog(wx.Dialog):
         cam.SetFocalPoint(cam_focus)
         cam.SetPosition(cam_pos)
-        self.Refresh()
+
+        self.interactor.Render()
     def ErrorEstimation(self, surface, points):
         """
@@ -3804,22 +3831,30 @@ class ICPCorregistrationDialog(wx.Dialog):
             self.timer.Stop()
     def OnUpdate(self, evt):
-        self.AddMarker(3, (1, 0, 0), self.current_coord[:3])
-        self.SetCameraVolume(self.current_coord[:3])
+        current_coord = self.GetCurrentCoord()
+        self.AddMarker(3, (1, 0, 0), current_coord)
+        self.SetCameraVolume(current_coord)
     def OnCreatePoint(self, evt):
-        self.AddMarker(3,(1,0,0),self.current_coord[:3])
-        self.SetCameraVolume(self.current_coord[:3])
+        current_coord = self.GetCurrentCoord()
+        self.AddMarker(3, (1, 0, 0), current_coord)
+        self.SetCameraVolume(current_coord)
     def OnReset(self, evt):
+        if self.cont_point:
+            self.cont_point.SetValue(False)
+            self.OnContinuousAcquisition(evt=None, btn=self.cont_point)
+
         self.RemoveActor()
         self.LoadActor()
-    def OnICP(self, evt):
-        self.SetProgress(0.3)
+    def OnICP(self):
         if self.cont_point:
             self.cont_point.SetValue(False)
             self.OnContinuousAcquisition(evt=None, btn=self.cont_point)
+
+        self.SetProgress(0.3)
+
         sourcePoints = np.array(self.point_coord)
         sourcePoints_vtk = vtk.vtkPoints()
@@ -3833,6 +3868,8 @@ class ICPCorregistrationDialog(wx.Dialog):
         icp.SetSource(source)
         icp.SetTarget(self.surface)
+        self.SetProgress(0.5)
+
         if self.icp_mode == 0:
             print("Affine mode")
             icp.GetLandmarkTransform().SetModeToAffine()
@@ -3860,7 +3897,6 @@ class ICPCorregistrationDialog(wx.Dialog):
         transformedSource = icpTransformFilter.GetOutput()
-        self.SetProgress(1)
         for i in range(transformedSource.GetNumberOfPoints()):
             p = [0, 0, 0]
@@ -3884,7 +3920,16 @@ class ICPCorregistrationDialog(wx.Dialog):
         self.prev_error = self.ErrorEstimation(self.surface, sourcePoints)
         self.final_error = self.ErrorEstimation(self.surface, self.transformed_points)
-        self.Refresh()
+        self.interactor.Render()
+
+        self.SetProgress(1)
+
+        self.btn_ok.Enable(True)
+
+    def thread_ICP_start(self, evt):
+        import threading
+        th = threading.Thread(target=self.OnICP, args=[])
+        th.start()
     def GetValue(self):
         return self.m_icp, self.point_coord, self.transformed_points, self.prev_error, self.final_error
@@ -691,6 +691,7 @@ class NeuronavigationPanel(wx.Panel):
         # has been initialized before
         if trck and choice != const.DISCTRACK:
             self.ResetTrackerFiducials()
+            self.ResetIcp()
             Publisher.sendMessage('Update status text in GUI',
                                   label=_("Disconnecting tracker..."))
             Publisher.sendMessage('Remove sensors ID')
@@ -711,6 +712,7 @@ class NeuronavigationPanel(wx.Panel):
         elif choice == const.DISCTRACK:
             if trck:
                 self.ResetTrackerFiducials()
+                self.ResetIcp()
                 Publisher.sendMessage('Update status text in GUI',
                                       label=_("Disconnecting tracker ..."))
                 Publisher.sendMessage('Remove sensors ID')
@@ -749,6 +751,7 @@ class NeuronavigationPanel(wx.Panel):
         # When ref mode is changed the tracker coordinates are set to zero
         self.ref_mode_id = evt.GetSelection()
         self.ResetTrackerFiducials()
+        self.ResetIcp()
         # Some trackers do not accept restarting within this time window
         # TODO: Improve the restarting of trackers after changing reference mode
         # self.OnChoiceTracker(None, ctrl)
@@ -780,8 +783,8 @@ class NeuronavigationPanel(wx.Panel):
         fiducial_name = const.TRACKER_FIDUCIALS[n]['fiducial_name']
         Publisher.sendMessage('Set tracker fiducial', fiducial_name=fiducial_name)
-    def OnICP(self):
-        dialog = dlg.ICPCorregistrationDialog(nav_prop=(self.tracker_id, self.trk_init, self.ref_mode_id))
+    def OnICP(self, m_change):
+        dialog = dlg.ICPCorregistrationDialog(nav_prop=(m_change, self.tracker_id, self.trk_init, self.ref_mode_id))
         if dialog.ShowModal() == wx.ID_OK:
             self.m_icp, point_coord, transformed_points, prev_error, final_error = dialog.GetValue()
             #TODO: checkbox in the dialog to transfer the icp points to 3D viewer
@@ -972,7 +975,7 @@ class NeuronavigationPanel(wx.Panel):
                 if not self.checkicp.GetValue():
                     if dlg.ICPcorregistration(self.fre):
-                        m_icp = self.OnICP()
+                        m_icp = self.OnICP(m_change)
                         self.icp_fre = db.calculate_fre(self.fiducials_raw, self.fiducials, self.ref_mode_id,
                                                         m_change, m_icp)
                         self.ctrl_icp()