diff --git a/invesalius/constants.py b/invesalius/constants.py index 6d6db53..51418a8 100644 --- a/invesalius/constants.py +++ b/invesalius/constants.py @@ -666,8 +666,9 @@ ISOTRAKII = 3 PATRIOT = 4 CAMERA = 5 POLARIS = 6 -DEBUGTRACK = 7 -DISCTRACK = 8 +OPTITRACK = 7 +DEBUGTRACK = 8 +DISCTRACK = 9 DEFAULT_TRACKER = SELECT NDICOMPORT = b'COM1' @@ -675,7 +676,7 @@ NDICOMPORT = b'COM1' TRACKER = [_("Select tracker:"), _("Claron MicronTracker"), _("Polhemus FASTRAK"), _("Polhemus ISOTRAK II"), _("Polhemus PATRIOT"), _("Camera tracker"), - _("NDI Polaris"), _("Debug tracker"), _("Disconnect tracker")] + _("NDI Polaris"), _("Optitrack"), _("Debug tracker"), _("Disconnect tracker")] STATIC_REF = 0 DYNAMIC_REF = 1 @@ -753,7 +754,7 @@ PEEL_DEPTH = 5 MAX_PEEL_DEPTH = 30 SEED_OFFSET = 15 SEED_RADIUS = 1.5 -SLEEP_NAVIGATION = 0.3 +SLEEP_NAVIGATION = 0.1 BRAIN_OPACITY = 0.5 N_CPU = psutil.cpu_count() TREKKER_CONFIG = {'seed_max': 1, 'step_size': 0.1, 'min_fod': 0.1, 'probe_quality': 3, diff --git a/invesalius/data/coordinates.py b/invesalius/data/coordinates.py index c7041e8..abcbe74 100644 --- a/invesalius/data/coordinates.py +++ b/invesalius/data/coordinates.py @@ -48,6 +48,7 @@ def GetCoordinates(trck_init, trck_id, ref_mode): const.PATRIOT: PolhemusCoord, const.CAMERA: CameraCoord, const.POLARIS: PolarisCoord, + const.OPTITRACK: OptitrackCoord, const.DEBUGTRACK: DebugCoord} coord = getcoord[trck_id](trck_init, trck_id, ref_mode) else: @@ -55,6 +56,46 @@ def GetCoordinates(trck_init, trck_id, ref_mode): return coord +def OptitrackCoord(trck_init, trck_id, ref_mode): + """ + + Obtains coordinates and angles of tracking rigid bodies (Measurement Probe, Coil, Head). Converts orientations from quaternion + rotations to Euler angles. This function uses Optitrack wrapper from Motive API 2.2. + + Parameters + ---------- + :trck_init: tracker initialization instance from OptitrackTracker function at trackers.py + :trck_id: not used + :ref_mode: not used + + Returns + ------- + coord: position of tracking rigid bodies + """ + trck=trck_init[0] + trck.Run() + + scale = 1000*np.array([1.0, 1.0, 1.0]) # coordinates are in millimeters in Motive API + + angles_probe = np.degrees(tr.euler_from_quaternion([float(trck.qwToolTip), float(trck.qzToolTip), float(trck.qxToolTip), float(trck.qyToolTip)], axes='rzyx')) + coord1 = np.array([float(trck.PositionToolTipZ1) * scale[0], float(trck.PositionToolTipX1) * scale[1], + float(trck.PositionToolTipY1) * scale[2]]) + coord1 = np.hstack((coord1, angles_probe)) + + angles_head = np.degrees(tr.euler_from_quaternion([float(trck.qwHead), float(trck.qzHead), float(trck.qxHead), float(trck.qyHead)], axes='rzyx')) + coord2 = np.array([float(trck.PositionHeadZ1) * scale[0], float(trck.PositionHeadX1) * scale[1], + float(trck.PositionHeadY1) * scale[2]]) + coord2 = np.hstack((coord2, angles_head)) + + angles_coil = np.degrees(tr.euler_from_quaternion([float(trck.qwCoil), float(trck.qzCoil), float(trck.qxCoil), float(trck.qyCoil)], axes='rzyx')) + 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 + + def PolarisCoord(trck_init, trck_id, ref_mode): trck = trck_init[0] trck.Run() diff --git a/invesalius/data/trackers.py b/invesalius/data/trackers.py index d2d1f02..ffc71b4 100644 --- a/invesalius/data/trackers.py +++ b/invesalius/data/trackers.py @@ -40,6 +40,7 @@ def TrackerConnection(tracker_id, trck_init, action): const.PATRIOT: PolhemusTracker, # PATRIOT const.CAMERA: CameraTracker, # CAMERA const.POLARIS: PolarisTracker, # POLARIS + const.OPTITRACK: OptitrackTracker, #Optitrack const.DEBUGTRACK: DebugTracker} trck_init = trck_fcn[tracker_id](tracker_id) @@ -62,6 +63,30 @@ def DefaultTracker(tracker_id): # return tracker initialization variable and type of connection return trck_init, 'wrapper' +def OptitrackTracker(tracker_id): + """ + Imports optitrack wrapper from Motive 2.2. Initialize cameras, attach listener, loads Calibration, loads User Profile + (Rigid bodies information). + + Parameters + ---------- + tracker_id : Optitrack ID + + Returns + ------- + trck_init : local name for Optitrack module + """ + trck_init = None + try: + import optitrack + trck_init = optitrack.optr() + if trck_init.Initialize()==0: + trck_init.Run() #Runs once Run function, to update cameras. + else: + trck_init = None + except ImportError: + print('Error') + return trck_init, 'wrapper' def PolarisTracker(tracker_id): from wx import ID_OK diff --git a/invesalius/gui/dialogs.py b/invesalius/gui/dialogs.py index d4c59e4..1e2932a 100644 --- a/invesalius/gui/dialogs.py +++ b/invesalius/gui/dialogs.py @@ -869,6 +869,7 @@ def ShowNavigationTrackerWarning(trck_id, lib_mode): const.PATRIOT: 'Polhemus PATRIOT', const.CAMERA: 'CAMERA', const.POLARIS: 'NDI Polaris', + const.OPTITRACK: 'Optitrack', const.DEBUGTRACK: 'Debug tracker device'} if lib_mode == 'choose': -- libgit2 0.21.2