Set target (invesalius#125)

* working

* Highlight target and added target icon

* The markers angles are saved now. Target ID created.

-Load markers works for data with angles and/or without angles data
-Load markers recognizes target ID.

* bug fix

* Task panel and dialog to configure coil tracking
- ADD: task panel for coil registration
- ADD: dialog with vtk window and coil obj

* Fixed markers angles bug
-Sometimes the sendmessage "Navigation Status" was arriving before the while thread is over.

* testing...

* Improve coil tracking and tracker communication
- ADD: dialog for coil registration
- ADD: panel for coil registration
- ENH: coordinate recording with Polhemus

* Improve Polhemus communication and trackers management
- ENH: Polhemus wrapper communication, connect and disconnect
- ENH: Choice of trackers
- FIX: Polhemus Fastrak navigation

* show/hide coil tracker
-tracker angles is updating coil tracker

* Just one target can be set

* Uncheck camera volume box when "set target" is selected

* Create target and track distance and orientation

-coil turn to green when orientation is less than the threshold (accept)

* Camera position and focus is based on target

- set transparency on target
- reposition 'distance txt'

* Improve coordinates handling and coil tracking interface
- ENH: coordinates managed with one array, each row is one tracker sensor
- FIX: several minor bugs and wx import masking
- ENH: better dialog for coil tracking
- ADD: coil tracking test thread simulating navigation
- ENH: dynamic reference must be applied outside coordinates module

* Disabled the set target menu when navigation is off

- Avoided bug if user set ID as TARGET

* Temporary coil tracking pipeline
- ENH: improved dialog UI for coil registration
- ENH: multiple coregistration methods depending on coil registration
- ADD: coil tracking to volume viewer

* Minor adjustments in UI and coil tracking

* Zoom camera with target distance

-set arrow upper limit

* Fixed distance text

* Fixed camera position

* Add translate of coil object in vtk matrix

* Set colors

* Text improvements

* Changes in coil registration
- ENH: improve variables and general bugs
- FIX: math for coil registration almost working

* Created coil tracker constants

* Coil coord threshold constants

* Fixed object orientation in dynamic and static navigation
- FIX: coil registration in static mode (sensor 1,2 working)
- ENH: object update in volume viewer improved
- ENH: coregistration adpated to object tracking
- TODO: allow coil registration with sensor 3

* Fixed object tracking and registration bugs
- ENH: UI for object tracking
- ADD: load and save object registration files
- Obs: orientation tracking works for static coil mode (1 ref)
- Obs: orientation tracking works for static and dynamic navigation

* Wrappers are set as metric

* Bug fix
-Avoided bug when no data is selected and the user tries to remove marker

* Removed unused icon

* Code refactoring for object registration
- ADD: button for object registration save in navigation panel
- ADD: dynamic object registration (3 sensors)
- ENH: object registration information saved
- Code cleaned and removed axes for visualization

* Group coil tracking interface with navigation refactoring
- Merge of vhosouza and rmatsuda branchs to track coil
- Navigation still not fully working
- Coil registration not working properly

* Enhance dynamic reference transformation equation

* Created ctrls angles and dist threshold

* Created option to record coordinates and angles along time

* Improve object orientation tracking in dynamic reference

* improvements

* Trials to fix track object orientation

* Fixed coils position

* Created message box for objects registration

* Coregistration changed to matrix multiplication
- Using transformations module
- Only Dynamic Reference without object is working

* Attempts to coil tracking improved

* Changes in object tracking algorithm
- Still not working

* Coregistration improvements

* New changes to track object orientation

* Object tracking with commented code working, axis rotation alligned

* Dynamic object registration working for rotation
- ENH: object registration base creation
- ENH: object registration algorithm for rotation
- TODO: translation and offset not working properly

* Dynamic object registration trying to fix object offset

* Dynamic object registration working
- FIX: offset of sensor to object center

* Code refactoring for navigation with object registration
- FIX: Close project not restarting navigation panel
- ENH: Navigation coregistration algorithms
- ENH: Object data handling in viewer volume
- ENH: Panels and controls in navigation task

* More closing project fix for navigation mode

* Enable object new, load and save buttons
- FIX: minor fix in navigation interface

* Improvement in object tracking panel
- FIX: restart variables while closing project
- ENH: object show and track checkboxes

* Update to correct Neurosoft Fig 8 TMS coil model

* Coil tracking improvements

-TODO: remove dummy coil when set target mode is off
-TODO: Enable target mode button only when target and object are set

* Enable target mode button only when target and object are set

-TODO: remove dummy coil when set target mode is off

* Disable target mode when track obj is off

* Fixed arrow directions and created text for obj registration

* Crucial fix to the object registration algorithm
- object was registered to the head m_head, now registered to the object basis in image space
- minor changes in variables related to navigation

* Camera orientation and distance text update

-Dist text is showing with dummy coil actor
-Camera is always orienting parallel to dummy coil actor
-TODO: fix - when target mode is off the dist text and the dummy coil keeps green
-TODO: fix - if the obj are not loaded the load markers (target) doesnt work properly

* Fixed initial current_coord

* Fixed fiducials creation markers and ENH: just showing coil when nav is on

* Target is reset when target mode is off

* Remove messages when project is closed

* Disable ref mode for obj registration

* Improvements on camera position

* Set vol. camera default true

- TODO: when nav is off, vol is reset to front position

* Camera is not reset when nav is off, but when target_mode is off

* Nav off do not set target mode off anymore

invesalius/constants.py

@@ -56,6 +56,7 @@
 TEXT_SIZE_SMALL = 11
 TEXT_SIZE = 12
 TEXT_SIZE_LARGE = 16
+TEXT_SIZE_EXTRA_LARGE = 20
 TEXT_COLOUR = (1,1,1)
 
 (X,Y) = (0.03, 0.97)
@@ -678,6 +679,10 @@
 DEFAULT_REF_MODE = DYNAMIC_REF
 REF_MODE = [_("Static ref."), _("Dynamic ref.")]
 
+DEFAULT_COIL = SELECT
+COIL = [_("Select coil:"), _("Neurosoft Figure-8"),
+           _("Magstim 70 mm"), _("Nexstim")]
+
 IR1 = wx.NewId()
 IR2 = wx.NewId()
 IR3 = wx.NewId()
@@ -708,5 +713,34 @@
             _("Select nasion with spatial tracker"),
             _("Show set coordinates in image")]
 
+OBJL = wx.NewId()
+OBJR = wx.NewId()
+OBJA = wx.NewId()
+OBJC = wx.NewId()
+OBJF = wx.NewId()
+
+BTNS_OBJ = {OBJL: {0: _('Left')},
+            OBJR: {1: _('Right')},
+            OBJA: {2: _('Anterior')},
+            OBJC: {3: _('Center')},
+            OBJF: {4: _('Fixed')}}
+
+TIPS_OBJ = [_("Select left object fiducial"),
+            _("Select right object fiducial"),
+            _("Select anterior object fiducial"),
+            _("Select object center"),
+            _("Attach sensor to object")]
+
 CAL_DIR = os.path.abspath(os.path.join(FILE_PATH, '..', 'navigation', 'mtc_files', 'CalibrationFiles'))
 MAR_DIR = os.path.abspath(os.path.join(FILE_PATH, '..', 'navigation', 'mtc_files', 'Markers'))
+
+#OBJECT TRACKING
+OBJ_DIR = os.path.abspath(os.path.join(FILE_PATH, '..', 'navigation', 'objects'))
+ARROW_SCALE = 3
+ARROW_UPPER_LIMIT = 30
+#COIL_ANGLES_THRESHOLD = 3 * ARROW_SCALE
+COIL_ANGLES_THRESHOLD = 3
+COIL_COORD_THRESHOLD = 3
+TIMESTAMP = 2.0
+
+CAM_MODE = True

invesalius/data/bases.py

@@ -1,5 +1,7 @@
-from math import sqrt
+from math import sqrt, pi
 import numpy as np
+import invesalius.data.coordinates as dco
+import invesalius.data.transformations as tr
 
 
 def angle_calculation(ap_axis, coil_axis):
@@ -19,7 +21,7 @@ def angle_calculation(ap_axis, coil_axis):
     return float(angle)
 
 
-def base_creation(fiducials):
+def base_creation_old(fiducials):
     """
     Calculate the origin and matrix for coordinate system
     transformation.
@@ -55,31 +57,70 @@ def base_creation(fiducials):
                    [g2[0], g2[1], g2[2]],
                    [g3[0], g3[1], g3[2]]])
 
-    q.shape = (3, 1)
-    q = np.matrix(q.copy())
     m_inv = m.I
 
-    # print"M: ", m
-    # print"q: ", q
+    return m, q, m_inv
+
+
+def base_creation(fiducials):
+    """
+    Calculate the origin and matrix for coordinate system
+    transformation.
+    q: origin of coordinate system
+    g1, g2, g3: orthogonal vectors of coordinate system
+
+    :param fiducials: array of 3 rows (p1, p2, p3) and 3 columns (x, y, z) with fiducials coordinates
+    :return: matrix and origin for base transformation
+    """
+
+    p1 = fiducials[0, :]
+    p2 = fiducials[1, :]
+    p3 = fiducials[2, :]
+
+    sub1 = p2 - p1
+    sub2 = p3 - p1
+    lamb = (sub1[0]*sub2[0]+sub1[1]*sub2[1]+sub1[2]*sub2[2])/np.dot(sub1, sub1)
+
+    q = p1 + lamb*sub1
+    g1 = p3 - q
+    g2 = p1 - q
+
+    if not g1.any():
+        g1 = p2 - q
+
+    g3 = np.cross(g1, g2)
+
+    g1 = g1/sqrt(np.dot(g1, g1))
+    g2 = g2/sqrt(np.dot(g2, g2))
+    g3 = g3/sqrt(np.dot(g3, g3))
+
+    m = np.matrix([[g1[0], g2[0], g3[0]],
+                   [g1[1], g2[1], g3[1]],
+                   [g1[2], g2[2], g3[2]]])
+
+    m_inv = m.I
 
     return m, q, m_inv
 
 
-def calculate_fre(fiducials, minv, n, q1, q2):
+def calculate_fre(fiducials, minv, n, q, o):
     """
     Calculate the Fiducial Registration Error for neuronavigation.
 
     :param fiducials: array of 6 rows (image and tracker fiducials) and 3 columns (x, y, z) with coordinates
     :param minv: inverse matrix given by base creation
     :param n: base change matrix given by base creation
-    :param q1: origin of first base
-    :param q2: origin of second base
+    :param q: origin of first base
+    :param o: origin of second base
     :return: float number of fiducial registration error
     """
 
     img = np.zeros([3, 3])
     dist = np.zeros([3, 1])
 
+    q1 = np.mat(q).reshape(3, 1)
+    q2 = np.mat(o).reshape(3, 1)
+
     p1 = np.mat(fiducials[3, :]).reshape(3, 1)
     p2 = np.mat(fiducials[4, :]).reshape(3, 1)
     p3 = np.mat(fiducials[5, :]).reshape(3, 1)
@@ -133,3 +174,98 @@ def flip_x(point):
     x, y, z = point_rot.tolist()[0][:3]
 
     return x, y, z
+
+
+def flip_x_m(point):
+    """
+    Rotate coordinates of a vector by pi around X axis in static reference frame.
+
+    InVesalius also require to multiply the z coordinate by (-1). Possibly
+    because the origin of coordinate system of imagedata is
+    located in superior left corner and the origin of VTK scene coordinate
+    system (polygonal surface) is in the interior left corner. Second
+    possibility is the order of slice stacking
+
+    :param point: list of coordinates x, y and z
+    :return: rotated coordinates
+    """
+
+    point_4 = np.hstack((point, 1.)).reshape([4, 1])
+    point_4[2, 0] = -point_4[2, 0]
+
+    m_rot = np.asmatrix(tr.euler_matrix(pi, 0, 0))
+
+    point_rot = m_rot*point_4
+
+    return point_rot[0, 0], point_rot[1, 0], point_rot[2, 0]
+
+
+def object_registration(fiducials, orients, coord_raw, m_change):
+    """
+
+    :param fiducials: 3x3 array of fiducials translations
+    :param orients: 3x3 array of fiducials orientations in degrees
+    :param coord_raw: nx6 array of coordinates from tracking device where n = 1 is the reference attached to the head
+    :param m_change: 3x3 array representing change of basis from head in tracking system to vtk head system
+    :return:
+    """
+
+    coords_aux = np.hstack((fiducials, orients))
+    mask = np.ones(len(coords_aux), dtype=bool)
+    mask[[3]] = False
+    coords = coords_aux[mask]
+
+    fids_dyn = np.zeros([4, 6])
+    fids_img = np.zeros([4, 6])
+    fids_raw = np.zeros([3, 3])
+
+    # compute fiducials of object with reference to the fixed probe in source frame
+    for ic in range(0, 3):
+        fids_raw[ic, :] = dco.dynamic_reference_m2(coords[ic, :], coords[3, :])[:3]
+
+    # compute initial alignment of probe fixed in the object in source frame
+    t_s0_raw = np.asmatrix(tr.translation_matrix(coords[3, :3]))
+    r_s0_raw = np.asmatrix(tr.euler_matrix(np.radians(coords[3, 3]), np.radians(coords[3, 4]),
+                             np.radians(coords[3, 5]), 'rzyx'))
+    s0_raw = np.asmatrix(tr.concatenate_matrices(t_s0_raw, r_s0_raw))
+
+    # compute change of basis for object fiducials in source frame
+    base_obj_raw, q_obj_raw, base_inv_obj_raw = base_creation(fids_raw[:3, :3])
+    r_obj_raw = np.asmatrix(np.identity(4))
+    r_obj_raw[:3, :3] = base_obj_raw[:3, :3]
+    t_obj_raw = np.asmatrix(tr.translation_matrix(q_obj_raw))
+    m_obj_raw = np.asmatrix(tr.concatenate_matrices(t_obj_raw, r_obj_raw))
+
+    for ic in range(0, 4):
+        if coord_raw.any():
+            # compute object fiducials in reference frame
+            fids_dyn[ic, :] = dco.dynamic_reference_m2(coords[ic, :], coord_raw[1, :])
+            fids_dyn[ic, 2] = -fids_dyn[ic, 2]
+        else:
+            # compute object fiducials in source frame
+            fids_dyn[ic, :] = coords[ic, :]
+
+        # compute object fiducials in vtk head frame
+        a, b, g = np.radians(fids_dyn[ic, 3:])
+        T_p = tr.translation_matrix(fids_dyn[ic, :3])
+        R_p = tr.euler_matrix(a, b, g, 'rzyx')
+        M_p = np.asmatrix(tr.concatenate_matrices(T_p, R_p))
+        M_img = np.asmatrix(m_change) * M_p
+
+        angles_img = np.degrees(np.asarray(tr.euler_from_matrix(M_img, 'rzyx')))
+        coord_img = np.asarray(flip_x_m(tr.translation_from_matrix(M_img)))
+
+        fids_img[ic, :] = np.hstack((coord_img, angles_img))
+
+    # compute object base change in vtk head frame
+    base_obj_img, q_obj_img, base_inv_obj_img = base_creation(fids_img[:3, :3])
+    r_obj_img = np.asmatrix(np.identity(4))
+    r_obj_img[:3, :3] = base_obj_img[:3, :3]
+
+    # compute initial alignment of probe fixed in the object in reference (or static) frame
+    s0_trans_dyn = np.asmatrix(tr.translation_matrix(fids_dyn[3, :3]))
+    s0_rot_dyn = np.asmatrix(tr.euler_matrix(np.radians(fids_dyn[3, 3]), np.radians(fids_dyn[3, 4]),
+                                             np.radians(fids_dyn[3, 5]), 'rzyx'))
+    s0_dyn = np.asmatrix(tr.concatenate_matrices(s0_trans_dyn, s0_rot_dyn))
+
+    return t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img