From 65d8bbdf1797384b32f5609d6995dfbb863aa0e1 Mon Sep 17 00:00:00 2001 From: Tolonen Luka Date: Sat, 20 Jul 2024 19:41:35 +0300 Subject: [PATCH] Refactor 'Track coil' button behavior: this determines (on the fly) whether 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 | 145 ++++++++-------- .../data/visualization/coil_visualizer.py | 27 ++- invesalius/gui/preferences.py | 5 +- invesalius/gui/task_navigator.py | 14 +- invesalius/navigation/navigation.py | 158 +++++++++--------- 5 files changed, 169 insertions(+), 180 deletions(-) diff --git a/invesalius/data/coregistration.py b/invesalius/data/coregistration.py index 927209231..3d7fe6e50 100644 --- a/invesalius/data/coregistration.py +++ b/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) diff --git a/invesalius/data/visualization/coil_visualizer.py b/invesalius/data/visualization/coil_visualizer.py index 3a9504ea0..cec0d2df4 100644 --- a/invesalius/data/visualization/coil_visualizer.py +++ b/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,6 +79,9 @@ 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): @@ -80,6 +89,7 @@ def __bind_events(self): 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): diff --git a/invesalius/gui/preferences.py b/invesalius/gui/preferences.py index 18fc9c057..f422951f6 100644 --- a/invesalius/gui/preferences.py +++ b/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() diff --git a/invesalius/gui/task_navigator.py b/invesalius/gui/task_navigator.py index ee268f66a..85f7e86ac 100644 --- a/invesalius/gui/task_navigator.py +++ b/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() diff --git a/invesalius/navigation/navigation.py b/invesalius/navigation/navigation.py index 3d5cf0efa..3ca1a8709 100644 --- a/invesalius/navigation/navigation.py +++ b/invesalius/navigation/navigation.py @@ -132,18 +132,28 @@ def __init__(self, vis_queues, vis_components, event, sle, neuronavigation_api): self.sle = sle self.event = event self.neuronavigation_api = neuronavigation_api + self.navigation = Navigation() def run(self): while not self.event.is_set(): got_coords = False - object_visible_flag = False try: - coords, marker_visibilities, m_imgs, view_obj = self.coord_queue.get_nowait() - coord = coords[1] # main coil (or stylus if view_obj=False) - m_img = m_imgs[1] - + coords, marker_visibilities, m_imgs = self.coord_queue.get_nowait() got_coords = True - object_visible_flag = marker_visibilities[2] + + probe_visible = marker_visibilities[0] + coil_visible = marker_visibilities[2] + + # automatically track either coil or stylus if only one of them is visible, otherise use navigation.track_obj + track_coil = ( + (coil_visible or not probe_visible) + if (coil_visible ^ probe_visible) + else self.navigation.track_obj + ) + + # choose which object to track in slices and viewer_volume pointer + coord = coords[track_coil] # main-coil if track_coil else stylus + m_img = m_imgs[track_coil] # use of CallAfter is mandatory otherwise crashes the wx interface if self.view_tracts: @@ -180,15 +190,32 @@ def run(self): # new marker is created, it is created in the current position of the object. wx.CallAfter(Publisher.sendMessage, "Set cross focal point", position=coord) - if self.e_field_loaded and object_visible_flag: + wx.CallAfter( + Publisher.sendMessage, + "Update volume viewer pointer", + position=[coord[0], -coord[1], coord[2]], + ) + + if coil_visible: + wx.CallAfter( + Publisher.sendMessage, "Update coil pose", m_img=m_imgs[1], coord=coords[1] + ) wx.CallAfter( Publisher.sendMessage, - "Update point location for e-field calculation", - m_img=m_img, - coord=coord, - queue_IDs=self.e_field_IDs_queue, + "Update object arrow matrix", + m_img=m_imgs[1], + coord=coords[1], + flag=self.peel_loaded, ) - if not self.e_field_norms_queue.empty(): + + if self.e_field_loaded: + wx.CallAfter( + Publisher.sendMessage, + "Update point location for e-field calculation", + m_img=m_imgs[1], + coord=coords[1], + queue_IDs=self.e_field_IDs_queue, + ) try: enorm_data = self.e_field_norms_queue.get_nowait() wx.CallAfter( @@ -197,31 +224,16 @@ def run(self): enorm_data=enorm_data, plot_vector=self.plot_efield_vectors, ) - finally: + except queue.Empty: + pass + else: self.e_field_norms_queue.task_done() - if view_obj: - wx.CallAfter( - Publisher.sendMessage, "Update coil pose", m_img=m_img, coord=coord - ) - wx.CallAfter( - Publisher.sendMessage, - "Update object arrow matrix", - m_img=m_img, - coord=coord, - flag=self.peel_loaded, - ) - else: + if probe_visible: wx.CallAfter( - Publisher.sendMessage, - "Update volume viewer pointer", - position=[coord[0], -coord[1], coord[2]], + Publisher.sendMessage, "Update probe pose", m_img=m_imgs[0], coord=coords[0] ) - wx.CallAfter( - Publisher.sendMessage, "Update probe pose", m_img=m_imgs[0], coord=coords[0] - ) - # Render the volume viewer and the slice viewers. wx.CallAfter(Publisher.sendMessage, "Render volume viewer") wx.CallAfter(Publisher.sendMessage, "Update slice viewer") @@ -242,6 +254,8 @@ def __init__(self, pedal_connector, neuronavigation_api): self.correg = None self.target = None + self.n_coils = 1 + self.obj_registrations = [] self.object_registration = None self.track_obj = False self.m_change = None @@ -460,71 +474,53 @@ def StartNavigation(self, tracker, icp): ] Publisher.sendMessage("Navigation status", nav_status=True, vis_status=vis_components) - errors = False - - if self.track_obj: - # if object tracking is selected - if self.object_registration is None: - # check if object registration was performed - wx.MessageBox(_("Perform coil registration before navigation."), _("InVesalius 3")) - errors = True - else: - # if object registration was correctly performed continue with navigation - # object_registration[0] is object 3x3 fiducial matrix and object_registration[1] is 3x3 orientation matrix - obj_fiducials, obj_orients, obj_ref_mode, obj_name = self.object_registration - coreg_data = [self.r_stylus, self.m_change, obj_ref_mode] + # LUKATODO: object_registration --> coil_registrations + if self.object_registration is None: + # check if object registration was performed + wx.MessageBox(_("Perform coil registration before navigation."), _("InVesalius 3")) + else: + # if object registration was correctly performed continue with navigation + # object_registration[0] is object 3x3 fiducial matrix and object_registration[1] is 3x3 orientation matrix + obj_fiducials, obj_orients, obj_ref_mode, obj_name = self.object_registration - if self.ref_mode_id: - coord_raw, marker_visibilities = tracker.TrackerCoordinates.GetCoordinates() - else: - coord_raw = np.array([None]) + coreg_data = [self.r_stylus, self.m_change, obj_ref_mode] - self.obj_data = db.object_registration( - obj_fiducials, obj_orients, coord_raw, self.m_change - ) - coreg_data.extend(self.obj_data) - - queues = [ - self.coord_queue, - self.coord_tracts_queue, - self.icp_queue, - self.object_at_target_queue, - self.efield_queue, - ] - jobs_list.append( - dcr.CoordinateCorregistrate( - self.ref_mode_id, - tracker, - coreg_data, - self.view_tracts, - queues, - self.event, - self.sleep_nav, - tracker.tracker_id, - self.target, - icp, - self.e_field_loaded, - ) - ) - else: - coreg_data = (self.r_stylus, self.m_change, 0) - queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue, self.efield_queue] + if self.ref_mode_id: + coord_raw, marker_visibilities = tracker.TrackerCoordinates.GetCoordinates() + else: + coord_raw = np.array([None]) + + # LUKATODO: NOTE: coil does not need to be visible here, we only need the head coord for obj_data: + self.obj_data = db.object_registration( + obj_fiducials, obj_orients, coord_raw, self.m_change + ) + coreg_data.extend(self.obj_data) + + queues = [ + self.coord_queue, + self.coord_tracts_queue, + self.icp_queue, + self.object_at_target_queue, + self.efield_queue, + ] jobs_list.append( - dcr.CoordinateCorregistrateNoObject( + dcr.CoordinateCorregistrate( self.ref_mode_id, tracker, + self.n_coils, coreg_data, self.view_tracts, queues, self.event, self.sleep_nav, + tracker.tracker_id, + self.target, icp, self.e_field_loaded, ) ) - if not errors: # TODO: Test the serial port thread if self.serial_port_in_use: self.serial_port_connection = spc.SerialPortConnection(