diff --git a/invesalius/constants.py b/invesalius/constants.py index 7fb8344..a4d2f2b 100644 --- a/invesalius/constants.py +++ b/invesalius/constants.py @@ -818,6 +818,7 @@ SEED_RADIUS = 1.5 # Increased the default sleep parameter from 0.1 to 0.15 to decrease CPU load during navigation. SLEEP_NAVIGATION = 0.15 +SLEEP_COORDINATES = 0.05 BRAIN_OPACITY = 0.5 N_CPU = psutil.cpu_count() diff --git a/invesalius/data/coordinates.py b/invesalius/data/coordinates.py index 45993eb..74729f7 100644 --- a/invesalius/data/coordinates.py +++ b/invesalius/data/coordinates.py @@ -19,6 +19,9 @@ from math import sin, cos import numpy as np +import queue +import threading +import wx import invesalius.data.bases as db import invesalius.data.transformations as tr @@ -28,9 +31,21 @@ from time import sleep from random import uniform from invesalius.pubsub import pub as Publisher +class TrackerCoordinates(): + def __init__(self): + self.coord = None + self.markers_flag = [False, False, False] -def GetCoordinates(trck_init, trck_id, ref_mode): + def SetCoordinates(self, coord, markers_flag): + self.coord = coord + self.markers_flag = markers_flag + def GetCoordinates(self): + wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag) + return self.coord, self.markers_flag + + +def GetCoordinatesForThread(trck_init, trck_id, ref_mode): """ Read coordinates from spatial tracking devices using @@ -52,11 +67,55 @@ def GetCoordinates(trck_init, trck_id, ref_mode): const.OPTITRACK: OptitrackCoord, const.DEBUGTRACKRANDOM: DebugCoordRandom, const.DEBUGTRACKAPPROACH: DebugCoordRandom} - coord = getcoord[trck_id](trck_init, trck_id, ref_mode) + + coord, markers_flag = getcoord[trck_id](trck_init, trck_id, ref_mode) else: print("Select Tracker") - return coord + return coord, markers_flag + +def PolarisP4Coord(trck_init, trck_id, ref_mode): + trck = trck_init[0] + trck.Run() + + probe = trck.probe.decode(const.FS_ENCODE) + ref = trck.ref.decode(const.FS_ENCODE) + obj = trck.obj.decode(const.FS_ENCODE) + + probe = probe[2:] + ref = ref[2:] + obj = obj[2:] + + if probe[:7] == "MISSING": + coord1 = np.hstack(([0, 0, 0], [0, 0, 0])) + else: + q = [int(probe[i:i + 6]) * 0.0001 for i in range(0, 24, 6)] + t = [int(probe[i:i + 7]) * 0.01 for i in range(24, 45, 7)] + angles_probe = np.degrees(tr.euler_from_quaternion(q, axes='rzyx')) + trans_probe = np.array(t).astype(float) + coord1 = np.hstack((trans_probe, angles_probe)) + + if ref[:7] == "MISSING": + coord2 = np.hstack(([0, 0, 0], [0, 0, 0])) + else: + q = [int(ref[i:i + 6]) * 0.0001 for i in range(0, 24, 6)] + t = [int(ref[i:i + 7]) * 0.01 for i in range(24, 45, 7)] + angles_ref = np.degrees(tr.euler_from_quaternion(q, axes='rzyx')) + trans_ref = np.array(t).astype(float) + coord2 = np.hstack((trans_ref, angles_ref)) + + if obj[:7] == "MISSING": + coord3 = np.hstack(([0, 0, 0], [0, 0, 0])) + else: + q = [int(obj[i:i + 6]) * 0.0001 for i in range(0, 24, 6)] + t = [int(obj[i:i + 7]) * 0.01 for i in range(24, 45, 7)] + angles_obj = np.degrees(tr.euler_from_quaternion(q, axes='rzyx')) + trans_obj = np.array(t).astype(float) + coord3 = np.hstack((trans_obj, angles_obj)) + + coord = np.vstack([coord1, coord2, coord3]) + + return coord, [trck.probeID, trck.refID, trck.objID] def OptitrackCoord(trck_init, trck_id, ref_mode): """ @@ -93,8 +152,10 @@ def OptitrackCoord(trck_init, trck_id, ref_mode): coord3 = np.array([float(trck.PositionCoilZ1) * scale[0], float(trck.PositionCoilX1) * scale[1], float(trck.PositionCoilY1) * scale[2]]) coord3 = np.hstack((coord3, angles_coil)) + coord = np.vstack([coord1, coord2, coord3]) - return coord + + return coord, [trck.probeID, trck.HeadID, trck.coilID] def PolarisCoord(trck_init, trck_id, ref_mode): @@ -117,62 +178,25 @@ def PolarisCoord(trck_init, trck_id, ref_mode): coord3 = np.hstack((trans_obj, angles_obj)) coord = np.vstack([coord1, coord2, coord3]) - # Publisher.sendMessage('Sensors ID', probe_id=trck.probeID, ref_id=trck.refID, obj_id=trck.objID) - return coord + return coord, [trck.probeID, trck.refID, trck.coilID] -def PolarisP4Coord(trck_init, trck_id, ref_mode): - trck = trck_init[0] - trck.Run() - - probe = trck.probe.decode(const.FS_ENCODE) - ref = trck.ref.decode(const.FS_ENCODE) - obj = trck.obj.decode(const.FS_ENCODE) - - probe = probe[2:] - ref = ref[2:] - obj = obj[2:] - - if probe[:7] == "MISSING": - if not "coord1" in locals(): - coord1 = np.hstack(([0, 0, 0], [0, 0, 0])) +def ElfinCoord(trck_init): + if len(trck_init) > 2: + robotcoordinates = trck_init[-1] + coord = robotcoordinates.GetRobotCoordinates() + if coord is None: + coord = np.array([0, 0, 0, 0, 0, 0]) else: - q = [int(probe[i:i + 6]) * 0.0001 for i in range(0, 24, 6)] - t = [int(probe[i:i + 7]) * 0.01 for i in range(24, 45, 7)] - angles_probe = np.degrees(tr.euler_from_quaternion(q, axes='rzyx')) - trans_probe = np.array(t).astype(float) - coord1 = np.hstack((trans_probe, angles_probe)) + coord = np.array([0, 0, 0, 0, 0, 0]) - if ref[:7] == "MISSING": - if not "coord2" in locals(): - coord2 = np.hstack(([0, 0, 0], [0, 0, 0])) - else: - q = [int(ref[i:i + 6]) * 0.0001 for i in range(0, 24, 6)] - t = [int(ref[i:i + 7]) * 0.01 for i in range(24, 45, 7)] - angles_ref = np.degrees(tr.euler_from_quaternion(q, axes='rzyx')) - trans_ref = np.array(t).astype(float) - coord2 = np.hstack((trans_ref, angles_ref)) - - if obj[:7] == "MISSING": - if not "coord3" in locals(): - coord3 = np.hstack(([0, 0, 0], [0, 0, 0])) - else: - q = [int(obj[i:i + 6]) * 0.0001 for i in range(0, 24, 6)] - t = [int(obj[i:i + 7]) * 0.01 for i in range(24, 45, 7)] - angles_obj = np.degrees(tr.euler_from_quaternion(q, axes='rzyx')) - trans_obj = np.array(t).astype(float) - coord3 = np.hstack((trans_obj, angles_obj)) - - Publisher.sendMessage('Sensors ID', probe_id=trck.probeID, ref_id=trck.refID, obj_id=trck.objID) - coord = np.vstack([coord1, coord2, coord3]) - - return coord + return coord, None def CameraCoord(trck_init, trck_id, ref_mode): trck = trck_init[0] - coord, probeID, refID = trck.Run() - Publisher.sendMessage('Sensors ID', probe_id=probeID, ref_id=refID) - return coord + coord, probeID, refID, coilID = trck.Run() + + return coord, [probeID, refID, coilID] def ClaronCoord(trck_init, trck_id, ref_mode): trck = trck_init[0] @@ -193,9 +217,7 @@ def ClaronCoord(trck_init, trck_id, ref_mode): coord = np.vstack([coord1, coord2, coord3]) - Publisher.sendMessage('Sensors ID', probe_id=trck.probeID, ref_id=trck.refID) - - return coord + return coord, [trck.probeID, trck.refID, trck.coilID] def PolhemusCoord(trck, trck_id, ref_mode): @@ -210,7 +232,7 @@ def PolhemusCoord(trck, trck_id, ref_mode): elif trck[1] == 'wrapper': coord = PolhemusWrapperCoord(trck[0], trck_id, ref_mode) - return coord + return coord, None def PolhemusWrapperCoord(trck, trck_id, ref_mode): @@ -362,16 +384,15 @@ def DebugCoordRandom(trk_init, trck_id, ref_mode): # coord4 = np.array([uniform(1, 200), uniform(1, 200), uniform(1, 200), # uniform(-180.0, 180.0), uniform(-180.0, 180.0), uniform(-180.0, 180.0)]) - Publisher.sendMessage('Sensors ID', probe_id=int(uniform(0, 5)), ref_id=int(uniform(0, 5)), obj_id=int(uniform(0, 5))) + #Publisher.sendMessage('Sensors ID', probe_id=int(uniform(0, 5)), ref_id=int(uniform(0, 5)), obj_id=int(uniform(0, 5))) - return np.vstack([coord1, coord2, coord3, coord4]) + return np.vstack([coord1, coord2, coord3, coord4]), [int(uniform(0, 5)), int(uniform(0, 5)), int(uniform(0, 5))] def coordinates_to_transformation_matrix(position, orientation, axes='sxyz'): """ Transform vectors consisting of position and orientation (in Euler angles) in 3d-space into a 4x4 transformation matrix that combines the rotation and translation. - :param position: A vector of three coordinates. :param orientation: A vector of three Euler angles in degrees. :param axes: The order in which the rotations are done for the axes. See transformations.py for details. Defaults to 'sxyz'. @@ -391,9 +412,7 @@ def transformation_matrix_to_coordinates(matrix, axes='sxyz'): """ Given a matrix that combines the rotation and translation, return the position and the orientation determined by the matrix. The orientation is given as three Euler angles. - The inverse of coordinates_of_transformation_matrix when the parameter 'axes' matches. - :param matrix: A 4x4 transformation matrix. :param axes: The order in which the rotations are done for the axes. See transformations.py for details. Defaults to 'sxyz'. :return: The position (a vector of length 3) and Euler angles for the orientation in degrees (a vector of length 3). @@ -452,11 +471,11 @@ def dynamic_reference_m(probe, reference): :param reference: sensor two defined as reference :return: rotated and translated coordinates """ - affine = coordinates_to_transformation_matrix( - position=reference[:3], - orientation=reference[3:], - axes='rzyx', - ) + a, b, g = np.radians(reference[3:6]) + + trans = tr.translation_matrix(reference[:3]) + rot = tr.euler_matrix(a, b, g, 'rzyx') + affine = tr.concatenate_matrices(trans, rot) probe_4 = np.vstack((probe[:3].reshape([3, 1]), 1.)) coord_rot = np.linalg.inv(affine) @ probe_4 # minus sign to the z coordinate @@ -479,16 +498,17 @@ def dynamic_reference_m2(probe, reference): :param reference: sensor two defined as reference :return: rotated and translated coordinates """ - M = coordinates_to_transformation_matrix( - position=reference[:3], - orientation=reference[3:], - axes='rzyx', - ) - M_p = coordinates_to_transformation_matrix( - position=probe[:3], - orientation=probe[3:], - axes='rzyx', - ) + + a, b, g = np.radians(reference[3:6]) + a_p, b_p, g_p = np.radians(probe[3:6]) + + T = tr.translation_matrix(reference[:3]) + T_p = tr.translation_matrix(probe[:3]) + R = tr.euler_matrix(a, b, g, 'rzyx') + R_p = tr.euler_matrix(a_p, b_p, g_p, 'rzyx') + M = tr.concatenate_matrices(T, R) + M_p = tr.concatenate_matrices(T_p, R_p) + M_dyn = np.linalg.inv(M) @ M_p al, be, ga = tr.euler_from_matrix(M_dyn, 'rzyx') @@ -529,3 +549,17 @@ def offset_coordinate(p_old, norm_vec, offset): """ p_offset = p_old - offset * norm_vec return p_offset + +class ReceiveCoordinates(threading.Thread): + def __init__(self, trck_init, trck_id, TrackerCoordinates, event): + threading.Thread.__init__(self, name='ReceiveCoordinates') + self.trck_init = trck_init + self.trck_id = trck_id + self.event = event + self.TrackerCoordinates = TrackerCoordinates + + def run(self): + while not self.event.is_set(): + coord_raw, markers_flag = GetCoordinatesForThread(self.trck_init, self.trck_id, const.DEFAULT_REF_MODE) + self.TrackerCoordinates.SetCoordinates(coord_raw, markers_flag) + sleep(const.SLEEP_COORDINATES) \ No newline at end of file diff --git a/invesalius/data/coregistration.py b/invesalius/data/coregistration.py index d6618da..4a644d3 100644 --- a/invesalius/data/coregistration.py +++ b/invesalius/data/coregistration.py @@ -219,7 +219,7 @@ class CoordinateCorregistrate(threading.Thread): else: self.icp, self.m_icp = self.icp_queue.get_nowait() # print(f"Set the coordinate") - coord_raw = dco.GetCoordinates(trck_init, trck_id, self.ref_mode_id) + coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates() coord, m_img = corregistrate_object_dynamic(coreg_data, coord_raw, self.ref_mode_id, [self.icp, self.m_icp]) # XXX: This is not the best place to do the logic related to approaching the target when the @@ -294,7 +294,7 @@ class CoordinateCorregistrateNoObject(threading.Thread): self.icp, self.m_icp = self.icp_queue.get_nowait() # print(f"Set the coordinate") #print(self.icp, self.m_icp) - coord_raw = dco.GetCoordinates(trck_init, trck_id, self.ref_mode_id) + coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates() coord, m_img = corregistrate_dynamic(coreg_data, coord_raw, self.ref_mode_id, [self.icp, self.m_icp]) # print("Coord: ", coord) m_img_flip = m_img.copy() diff --git a/invesalius/data/viewer_volume.py b/invesalius/data/viewer_volume.py index 0fb1906..0f36f8f 100644 --- a/invesalius/data/viewer_volume.py +++ b/invesalius/data/viewer_volume.py @@ -368,7 +368,9 @@ class Viewer(wx.Panel): self.interactor.Render() - def OnSensors(self, probe_id, ref_id, obj_id=0): + def OnSensors(self, markers_flag): + probe_id, ref_id, obj_id = markers_flag + if not self.probe: self.CreateSensorID() diff --git a/invesalius/gui/dialogs.py b/invesalius/gui/dialogs.py index 8df4abe..9dfddb2 100644 --- a/invesalius/gui/dialogs.py +++ b/invesalius/gui/dialogs.py @@ -3804,7 +3804,7 @@ class ICPCorregistrationDialog(wx.Dialog): self.interactor.Render() def GetCurrentCoord(self): - coord_raw = dco.GetCoordinates(self.trk_init, self.tracker_id, const.DYNAMIC_REF) + coord_raw = dco.TrackerCoordinates.GetCoordinates() coord, _ = dcr.corregistrate_dynamic((self.m_change, 0), coord_raw, const.DEFAULT_REF_MODE, [None, None]) return coord[:3] diff --git a/invesalius/navigation/navigation.py b/invesalius/navigation/navigation.py index 884ce19..7e3bdb7 100644 --- a/invesalius/navigation/navigation.py +++ b/invesalius/navigation/navigation.py @@ -255,7 +255,7 @@ class Navigation(): coreg_data = [m_change, obj_ref_mode] if self.ref_mode_id: - coord_raw = dco.GetCoordinates(tracker.trk_init, tracker.tracker_id, self.ref_mode_id) + coord_raw, markers_flag = tracker.TrackerCoordinates.GetCoordinates() else: coord_raw = np.array([None]) diff --git a/invesalius/navigation/tracker.py b/invesalius/navigation/tracker.py index 89b08fd..be8c517 100644 --- a/invesalius/navigation/tracker.py +++ b/invesalius/navigation/tracker.py @@ -18,6 +18,7 @@ #-------------------------------------------------------------------------- import numpy as np +import threading import invesalius.constants as const import invesalius.data.coordinates as dco @@ -36,6 +37,10 @@ class Tracker(): self.tracker_connected = False + self.event_coord = threading.Event() + + self.TrackerCoordinates = dco.TrackerCoordinates() + def SetTracker(self, new_tracker): if new_tracker: self.DisconnectTracker() @@ -49,6 +54,8 @@ class Tracker(): else: self.tracker_id = new_tracker self.tracker_connected = True + dco.ReceiveCoordinates(self.trk_init, self.tracker_id, self.TrackerCoordinates, + self.event_coord).start() def DisconnectTracker(self): if self.tracker_connected: @@ -81,7 +88,7 @@ class Tracker(): coord_samples = {} for i in range(n_samples): - coord_raw = dco.GetCoordinates(self.trk_init, self.tracker_id, ref_mode_id) + coord_raw, markers_flag = self.TrackerCoordinates.GetCoordinates() if ref_mode_id == const.DYNAMIC_REF: coord = dco.dynamic_reference_m(coord_raw[0, :], coord_raw[1, :]) -- libgit2 0.21.2