Refactor 'Track coil' button behavior: this determines (on the fly) w…

…hether to follow the coil or probe with marker. Join the two Corregistrate threads into one. Edit CoilVisualizer and GUI to work with this change.

Please excuse some stray comments and var-definitions (eg. n_coils)... I will clean these up in future multicoil PR.

invesalius/data/coregistration.py

@@ -48,7 +48,6 @@ def object_marker_to_center(coord_raw, obj_ref_mode, t_obj_raw, s0_raw, r_s0_raw
     :return: 4 x 4 numpy double array
     :rtype: numpy.ndarray
     """
-
     as1, bs1, gs1 = np.radians(coord_raw[obj_ref_mode, 3:])
     r_probe = tr.euler_matrix(as1, bs1, gs1, "rzyx")
     t_probe_raw = tr.translation_matrix(coord_raw[obj_ref_mode, :3])
@@ -211,17 +210,17 @@ def corregistrate_probe(m_change, r_stylus, coord_raw, ref_mode_id, icp=[None, N
     return coord, m_img
 
 
-def corregistrate_object_dynamic(inp, coord_raw, ref_mode_id, icp):
+def corregistrate_object_dynamic(inp, coord_raw, i_obj, icp):
+    """
+    Corregistrate the object at coord_raw[i_obj] in dynamic ref_mode
+    """
     m_change, obj_ref_mode, t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img = inp
 
     # transform raw marker coordinate to object center
-    m_probe = object_marker_to_center(coord_raw, obj_ref_mode, t_obj_raw, s0_raw, r_s0_raw)
+    m_probe = object_marker_to_center(coord_raw, i_obj, t_obj_raw, s0_raw, r_s0_raw)
 
-    # transform object center to reference marker if specified as dynamic reference
-    if ref_mode_id:
-        m_probe_ref = object_to_reference(coord_raw, m_probe)
-    else:
-        m_probe_ref = m_probe
+    # transform object center to reference marker
+    m_probe_ref = object_to_reference(coord_raw, m_probe)
 
     # invert y coordinate
     m_probe_ref[2, -1] = -m_probe_ref[2, -1]
@@ -246,6 +245,38 @@ def corregistrate_object_dynamic(inp, coord_raw, ref_mode_id, icp):
     return coord, m_img
 
 
+def corregistrate_object_static(inp, coord_raw, i_obj, icp):
+    """
+    Corregistrate the object at coord_raw[i_obj] in static ref_mode
+    """
+    m_change, obj_ref_mode, t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img = inp
+
+    # transform raw marker coordinate to object center
+    m_probe = object_marker_to_center(coord_raw, i_obj, t_obj_raw, s0_raw, r_s0_raw)
+
+    # invert y coordinate
+    m_probe[2, -1] = -m_probe[2, -1]
+
+    # corregistrate from tracker to image space
+    m_img = tracker_to_image(m_change, m_probe, r_obj_img, m_obj_raw, s0_dyn)
+    m_img = apply_icp(m_img, icp)
+
+    # compute rotation angles
+    angles = np.degrees(tr.euler_from_matrix(m_img, axes="sxyz"))
+
+    # create output coordinate list
+    coord = (
+        m_img[0, -1],
+        m_img[1, -1],
+        m_img[2, -1],
+        angles[0],
+        angles[1],
+        angles[2],
+    )
+
+    return coord, m_img
+
+
 def compute_marker_transformation(coord_raw, obj_ref_mode):
     m_probe = dco.coordinates_to_transformation_matrix(
         position=coord_raw[obj_ref_mode, :3],
@@ -299,6 +330,7 @@ def __init__(
         self,
         ref_mode_id,
         tracker,
+        n_coils,
         coreg_data,
         view_tracts,
         queues,
@@ -312,6 +344,7 @@ def __init__(
         threading.Thread.__init__(self, name="CoordCoregObject")
         self.ref_mode_id = ref_mode_id
         self.tracker = tracker
+        self.n_coils = n_coils
         self.coreg_data = coreg_data
         self.coord_queue = queues[0]
         self.view_tracts = view_tracts
@@ -340,9 +373,20 @@ def __init__(
 
     def run(self):
         coreg_data = self.coreg_data
-        view_obj = 1
+        corregistrate_object = (
+            corregistrate_object_dynamic if self.ref_mode_id else corregistrate_object_static
+        )
+
+        # compute n_coils_effective, the no. of coils to actually process:
+        # check how many coords we get from tracker (-2 for probe & head)
+        n_coils_trk = self.tracker.TrackerCoordinates.GetCoordinates()[0].shape[0] - 2
+
+        obj_ref_mode = coreg_data[2]
+        # if obj_ref_mode=0: only coregister one coil
+        # else: process the other (n_coils - 1) coils too
+        # min(obj_ref_mode, 1) = 0 if obj_ref_mode==0 else 1
+        n_coils_effective = min(n_coils_trk, 1 + min(obj_ref_mode, 1) * (self.n_coils - 1))
 
-        # print('CoordCoreg: event {}'.format(self.event.is_set()))
         while not self.event.is_set():
             try:
                 if not self.icp_queue.empty():
@@ -351,20 +395,27 @@ def run(self):
                 if not self.object_at_target_queue.empty():
                     self.target_flag = self.object_at_target_queue.get_nowait()
 
-                # print(f"Set the coordinate")
                 coord_raw, marker_visibilities = self.tracker.TrackerCoordinates.GetCoordinates()
 
-                # m_change = coreg_data[1], r_stylus = coreg_data[0] (r_stylus used for probe only)
+                # m_change = coreg_data[1], r_stylus = coreg_data[0]
                 coord_probe, m_img_probe = corregistrate_probe(
                     coreg_data[1], coreg_data[0], coord_raw, self.ref_mode_id
                 )
-                coord_coil, m_img_coil = corregistrate_object_dynamic(
-                    coreg_data[1:], coord_raw, self.ref_mode_id, [self.use_icp, self.m_icp]
+                coord_coil, m_img_coil = corregistrate_object(
+                    coreg_data[1:], coord_raw, obj_ref_mode, [self.use_icp, self.m_icp]
                 )
 
                 coords = [coord_probe, coord_coil]
                 m_imgs = [m_img_probe, m_img_coil]
 
+                # the possible other coils are i_obj=3 onwards at coord_raw[i_obj]
+                for i_obj in range(3, n_coils_effective + 2):
+                    coord_coil, m_img_coil = corregistrate_object(
+                        coreg_data[1:], coord_raw, i_obj, [self.use_icp, self.m_icp]
+                    )
+                    coords.append(coord_coil)
+                    m_imgs.append(m_img_coil)
+
                 # XXX: This is not the best place to do the logic related to approaching the target when the
                 #      debug tracker is in use. However, the trackers (including the debug trackers) operate in
                 #      the tracker space where it is hard to make the tracker approach the target in the image space.
@@ -385,10 +436,12 @@ def run(self):
                     translate = coord[0:3]
                     m_imgs[1] = tr.compose_matrix(angles=angles, translate=translate)
 
-                self.coord_queue.put_nowait([coords, marker_visibilities, m_imgs, view_obj])
+                self.coord_queue.put_nowait([coords, marker_visibilities, m_imgs])
 
                 coord = coords[1]  # main coil
                 m_img = m_imgs[1]
+                # LUKATODO: should coord = coords[track_coil]
+                # should the stuff below ever be done for stylus, but not coil?
 
                 m_img_flip = m_img.copy()
                 m_img_flip[1, -1] = -m_img_flip[1, -1]
@@ -403,65 +456,3 @@ def run(self):
                 pass
             # The sleep has to be in both threads
             sleep(self.sle)
-
-
-class CoordinateCorregistrateNoObject(threading.Thread):
-    def __init__(
-        self, ref_mode_id, tracker, coreg_data, view_tracts, queues, event, sle, icp, e_field_loaded
-    ):
-        threading.Thread.__init__(self, name="CoordCoregNoObject")
-        self.ref_mode_id = ref_mode_id
-        self.tracker = tracker
-        self.coreg_data = coreg_data
-        self.coord_queue = queues[0]
-        self.view_tracts = view_tracts
-        self.coord_tracts_queue = queues[1]
-        self.event = event
-        self.sle = sle
-        self.icp_queue = queues[2]
-        self.use_icp = icp.use_icp
-        self.m_icp = icp.m_icp
-        self.efield_queue = queues[3]
-        self.e_field_loaded = e_field_loaded
-
-    def run(self):
-        coreg_data = self.coreg_data
-        view_obj = 0
-
-        # print('CoordCoreg: event {}'.format(self.event.is_set()))
-        while not self.event.is_set():
-            try:
-                if self.icp_queue.empty():
-                    None
-                else:
-                    self.use_icp, self.m_icp = self.icp_queue.get_nowait()
-                # print(f"Set the coordinate")
-                # print(self.icp, self.m_icp)
-                coord_raw, marker_visibilities = self.tracker.TrackerCoordinates.GetCoordinates()
-
-                # NOTE: THIS THREAD WILL BE REFACTORED/DELETED SOON (with multicoil PR)
-
-                # m_change = coreg_data[1], r_stylus = coreg_data[0]
-                coord, m_img = corregistrate_probe(
-                    coreg_data[1], coreg_data[0], coord_raw, self.ref_mode_id
-                )
-
-                # temporary hack to follow old code structure, will be refactored soon
-                coords = [coord, coord]
-                m_imgs = [m_img, m_img]
-                # print("Coord: ", coord)
-                m_img_flip = m_img.copy()
-                m_img_flip[1, -1] = -m_img_flip[1, -1]
-
-                self.coord_queue.put_nowait([coords, marker_visibilities, m_imgs, view_obj])
-
-                if self.view_tracts:
-                    self.coord_tracts_queue.put_nowait(m_img_flip)
-                if self.e_field_loaded:
-                    self.efield_queue.put_nowait([m_img, coord])
-                if not self.icp_queue.empty():
-                    self.icp_queue.task_done()
-            except queue.Full:
-                pass
-            # The sleep has to be in both threads
-            sleep(self.sle)

invesalius/data/visualization/coil_visualizer.py

@@ -36,14 +36,20 @@ def __init__(self, renderer, interactor, actor_factory, vector_field_visualizer)
         # The vector field visualizer is used to show a vector field relative to the coil.
         self.vector_field_visualizer = vector_field_visualizer
 
+        # Number of coils is by default 1
+        self.n_coils = 1
+
         # The actor for showing the actual coil in the volume viewer.
         self.coil_actor = None
+        self.coil_actors = []
 
         # The actor for showing the center of the actual coil in the volume viewer.
         self.coil_center_actor = None
+        self.coil_center_actors = []
 
         # The actor for showing the target coil in the volume viewer.
         self.target_coil_actor = None
+        self.target_coil_actors = []
 
         # The assembly for showing the vector field relative to the coil in the volume viewer.
         self.vector_field_assembly = self.vector_field_visualizer.CreateVectorFieldAssembly()
@@ -73,13 +79,17 @@ def __init__(self, renderer, interactor, actor_factory, vector_field_visualizer)
 
         self.LoadConfig()
 
+        self.AddCoilActor(self.coil_path)
+        self.ShowCoil(False)
+
         self.__bind_events()
 
     def __bind_events(self):
         Publisher.subscribe(self.SetCoilAtTarget, "Coil at target")
         Publisher.subscribe(self.OnNavigationStatus, "Navigation status")
         Publisher.subscribe(self.TrackObject, "Track object")
         Publisher.subscribe(self.ShowCoil, "Show coil in viewer volume")
+        Publisher.subscribe(self.SetCoilCount, "Set coil count")
         Publisher.subscribe(self.ConfigureCoil, "Configure coil")
         Publisher.subscribe(self.UpdateCoilPose, "Update coil pose")
         Publisher.subscribe(self.UpdateVectorField, "Update vector field")
@@ -163,6 +173,9 @@ def SetCoilAtTarget(self, state):
         self.target_coil_actor.GetProperty().SetDiffuseColor(target_coil_color)
         self.coil_center_actor.GetProperty().SetDiffuseColor(target_coil_color)
 
+    def SetCoilCount(self, n_coils):
+        self.n_coils = n_coils
+
     def RemoveCoilActor(self):
         self.renderer.RemoveActor(self.coil_actor)
         self.renderer.RemoveActor(self.coil_center_actor)
@@ -187,16 +200,12 @@ def OnNavigationStatus(self, nav_status, vis_status):
     def TrackObject(self, enabled):
         self.track_object_pressed = enabled
 
-        if self.coil_path is None:
-            return
-
-        # Remove the previous coil actor if it exists.
-        if self.coil_actor is not None:
-            self.RemoveCoilActor()
-
-        # If enabled, add a new coil actor.
+        # Hide coil vector_field if it exists. LUKATODO: what is this vector field?
         if enabled:
-            self.AddCoilActor(self.coil_path)
+            self.vector_field_assembly.SetVisibility(1)
+        else:
+            self.vector_field_assembly.SetVisibility(0)
+
 
     # Called when 'show coil' button is pressed in the user interface or in code.
     def ShowCoil(self, state):

invesalius/gui/preferences.py

@@ -947,10 +947,7 @@ def OnChooseTracker(self, evt, ctrl):
             self.ShowParent()
 
     def OnChooseReferenceMode(self, evt, ctrl):
-        # Probably need to refactor object registration as a whole to use the
-        # OnChooseReferenceMode function which was used earlier. It can be found in
-        # the deprecated code in ObjectRegistrationPanel in task_navigator.py.
-        pass
+        Navigation().SetReferenceMode(evt.GetSelection())
 
     def HideParent(self):  # hide preferences dialog box
         self.GetGrandParent().Hide()

invesalius/gui/task_navigator.py

@@ -1344,7 +1344,7 @@ def __init__(self, parent, nav_hub):
         show_coil_button.SetBitmap(BMP_SHOW_COIL)
         show_coil_button.SetToolTip(tooltip)
         show_coil_button.SetValue(False)
-        show_coil_button.Enable(False)
+        show_coil_button.Enable(True)
         show_coil_button.Bind(wx.EVT_TOGGLEBUTTON, self.OnShowCoil)
         self.show_coil_button = show_coil_button
 
@@ -1671,12 +1671,11 @@ def OnCheckStatus(self, nav_status, vis_status):
             self.EnableToggleButton(self.checkbox_serial_port, 1)
             self.UpdateToggleButton(self.checkbox_serial_port)
 
-        # Enable/Disable track-object checkbox if navigation is off/on and object registration is valid.
+        # Enable/Disable track-object checkbox if object registration is valid.
         obj_registration = self.navigation.GetObjectRegistration()
-        enable_track_object = (
-            obj_registration is not None and obj_registration[0] is not None and not nav_status
-        )
+        enable_track_object = obj_registration is not None and obj_registration[0] is not None
         self.EnableTrackObjectButton(enable_track_object)
+        self.EnableShowCoilButton(enable_track_object)
 
     # Robot
     def OnRobotStatus(self, data):
@@ -1773,10 +1772,7 @@ def OnTrackObjectButton(self, evt=None, ctrl=None):
         if not pressed:
             Publisher.sendMessage("Press target mode button", pressed=pressed)
 
-        # Disable or enable 'Show coil' button, based on if 'Track object' button is pressed.
-        Publisher.sendMessage("Enable show-coil button", enabled=pressed)
-
-        # Also, automatically press or unpress 'Show coil' button.
+        # Automatically press or unpress 'Show coil' button.
         Publisher.sendMessage("Press show-coil button", pressed=pressed)
 
         self.SaveConfig()