Set target (#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

Set target (#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
Renan · Thiago Franco de Moraes
1 parent 8a5a3505
Showing 18 changed files with 2461 additions and 257 deletions Show diff stats
icons/target.png
invesalius/constants.py
invesalius/data/bases.py
invesalius/data/coordinates.py
invesalius/data/coregistration.py
invesalius/data/record_coords.py
invesalius/data/styles.py
invesalius/data/trackers.py
invesalius/data/viewer_slice.py
invesalius/data/viewer_volume.py
invesalius/data/vtk_utils.py
invesalius/gui/default_viewers.py
invesalius/gui/dialogs.py
invesalius/gui/frame.py
invesalius/gui/task_navigator.py
navigation/objects/aim.stl
navigation/objects/magstim_fig8_coil.stl
navigation/objects/magstim_fig8_coil_no_handle.stl
@@ -56,6 +56,7 @@ STEREO_ANAGLYPH = _(&quot;Anaglyph&quot;)
 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 @@ DYNAMIC_REF = 1
 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 @@ TIPS_TRK = [_(&quot;Select left ear with spatial tracker&quot;),
             _("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
-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
@@ -20,10 +20,13 @@
 from math import sin, cos
 import numpy as np
+import invesalius.data.transformations as tr
+
 from time import sleep
 from random import uniform
 from wx.lib.pubsub import pub as Publisher
+
 def GetCoordinates(trck_init, trck_id, ref_mode):
     """
@@ -54,7 +57,7 @@ def ClaronCoord(trck_init, trck_id, ref_mode):
     scale = np.array([1.0, 1.0, -1.0])
     coord = None
     k = 0
-    # TODO: try to replace while and use some Claron internal computation
+    # TODO: try to replace 'while' and use some Claron internal computation
     if ref_mode:
         while k < 20:
@@ -77,6 +80,7 @@ def ClaronCoord(trck_init, trck_id, ref_mode):
                 trck.Run()
                 coord = np.array([trck.PositionTooltipX1 * scale[0], trck.PositionTooltipY1 * scale[1],
                                   trck.PositionTooltipZ1 * scale[2], trck.AngleX1, trck.AngleY1, trck.AngleZ1])
+
                 k = 30
             except AttributeError:
                 k += 1
@@ -104,27 +108,23 @@ def PolhemusCoord(trck, trck_id, ref_mode):
 def PolhemusWrapperCoord(trck, trck_id, ref_mode):
-    scale = 25.4 * np.array([1., 1.0, -1.0])
-    coord = None
+    trck.Run()
+    scale = 10.0 * np.array([1., 1., 1.])
-    if ref_mode:
-        trck.Run()
-        probe = np.array([float(trck.PositionTooltipX1), float(trck.PositionTooltipY1),
-                          float(trck.PositionTooltipZ1), float(trck.AngleX1), float(trck.AngleY1),
-                          float(trck.AngleZ1)])
-        reference = np.array([float(trck.PositionTooltipX2), float(trck.PositionTooltipY2),
-                          float(trck.PositionTooltipZ2), float(trck.AngleX2), float(trck.AngleY2),
-                          float(trck.AngleZ2)])
+    coord1 = np.array([float(trck.PositionTooltipX1)*scale[0], float(trck.PositionTooltipY1)*scale[1],
+                      float(trck.PositionTooltipZ1)*scale[2],
+                      float(trck.AngleX1), float(trck.AngleY1), float(trck.AngleZ1)])
-        if probe.all() and reference.all():
-            coord = dynamic_reference(probe, reference)
-            coord = (coord[0] * scale[0], coord[1] * scale[1], coord[2] * scale[2], coord[3], coord[4], coord[5])
+    coord2 = np.array([float(trck.PositionTooltipX2)*scale[0], float(trck.PositionTooltipY2)*scale[1],
+                       float(trck.PositionTooltipZ2)*scale[2],
+                       float(trck.AngleX2), float(trck.AngleY2), float(trck.AngleZ2)])
+    coord = np.vstack([coord1, coord2])
-    else:
-        trck.Run()
-        coord = np.array([float(trck.PositionTooltipX1) * scale[0], float(trck.PositionTooltipY1) * scale[1],
-                          float(trck.PositionTooltipZ1) * scale[2], float(trck.AngleX1), float(trck.AngleY1),
-                          float(trck.AngleZ1)])
+    if trck_id == 2:
+        coord3 = np.array([float(trck.PositionTooltipX3) * scale[0], float(trck.PositionTooltipY3) * scale[1],
+                           float(trck.PositionTooltipZ3) * scale[2],
+                           float(trck.AngleX3), float(trck.AngleY3), float(trck.AngleZ3)])
+        coord = np.vstack([coord, coord3])
     if trck.StylusButton:
         Publisher.sendMessage('PLH Stylus Button On')
@@ -213,22 +213,21 @@ def DebugCoord(trk_init, trck_id, ref_mode):
     :param trck_id: id of tracking device
     :return: six coordinates x, y, z, alfa, beta and gama
     """
-    sleep(0.2)
-    if ref_mode:
-        probe = np.array([uniform(1, 200), uniform(1, 200), uniform(1, 200),
-                          uniform(1, 200), uniform(1, 200), uniform(1, 200)])
-        reference = np.array([uniform(1, 200), uniform(1, 200), uniform(1, 200),
-                              uniform(1, 200), uniform(1, 200), uniform(1, 200)])
-        coord = dynamic_reference(probe, reference)
+    sleep(0.05)
-    else:
-        coord = np.array([uniform(1, 200), uniform(1, 200), uniform(1, 200),
-                          uniform(1, 200), uniform(1, 200), uniform(1, 200)])
+    coord1 = 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)])
+
+    coord2 = 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)])
+
+    coord3 = 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', [int(uniform(0, 5)), int(uniform(0, 5))])
-    return coord
+    return np.vstack([coord1, coord2, coord3])
 def dynamic_reference(probe, reference):
@@ -245,28 +244,143 @@ def dynamic_reference(probe, reference):
     """
     a, b, g = np.radians(reference[3:6])
-    vet = probe[0:3] - reference[0:3]
-    vet = np.mat(vet.reshape(3, 1))
+    vet = np.asmatrix(probe[0:3] - reference[0:3])
+    # vet = np.mat(vet.reshape(3, 1))
+
+    # Attitude matrix given by Patriot manual
+    # a: rotation of plane (X, Y) around Z axis (azimuth)
+    # b: rotation of plane (X', Z) around Y' axis (elevation)
+    # a: rotation of plane (Y', Z') around X'' axis (roll)
+    m_rot = np.mat([[cos(a) * cos(b), sin(b) * sin(g) * cos(a) - cos(g) * sin(a),
+                    cos(a) * sin(b) * cos(g) + sin(a) * sin(g)],
+                   [cos(b) * sin(a), sin(b) * sin(g) * sin(a) + cos(g) * cos(a),
+                    cos(g) * sin(b) * sin(a) - sin(g) * cos(a)],
+                   [-sin(b), sin(g) * cos(b), cos(b) * cos(g)]])
+
+    # coord_rot = m_rot.T * vet
+    coord_rot = vet*m_rot
+    coord_rot = np.squeeze(np.asarray(coord_rot))
+
+    return coord_rot[0], coord_rot[1], -coord_rot[2], probe[3], probe[4], probe[5]
-    # Attitude Matrix given by Patriot Manual
-    Mrot = np.mat([[cos(a) * cos(b), sin(b) * sin(g) * cos(a) - cos(g) * sin(a),
-                       cos(a) * sin(b) * cos(g) + sin(a) * sin(g)],
-                      [cos(b) * sin(a), sin(b) * sin(g) * sin(a) + cos(g) * cos(a),
-                       cos(g) * sin(b) * sin(a) - sin(g) * cos(a)],
-                      [-sin(b), sin(g) * cos(b), cos(b) * cos(g)]])
-    coord_rot = Mrot.T * vet
+def dynamic_reference_m(probe, reference):
+    """
+    Apply dynamic reference correction to probe coordinates. Uses the alpha, beta and gama
+    rotation angles of reference to rotate the probe coordinate and returns the x, y, z
+    difference between probe and reference. Angles sequences and equation was extracted from
+    Polhemus manual and Attitude matrix in Wikipedia.
+    General equation is:
+    coord = Mrot * (probe - reference)
+    :param probe: sensor one defined as probe
+    :param reference: sensor two defined as reference
+    :return: rotated and translated coordinates
+    """
+
+    a, b, g = np.radians(reference[3:6])
+
+    T = tr.translation_matrix(reference[:3])
+    R = tr.euler_matrix(a, b, g, 'rzyx')
+    M = np.asmatrix(tr.concatenate_matrices(T, R))
+    # M = tr.compose_matrix(angles=np.radians(reference[3:6]), translate=reference[:3])
+    # print M
+    probe_4 = np.vstack((np.asmatrix(probe[:3]).reshape([3, 1]), 1.))
+    coord_rot = M.I * probe_4
     coord_rot = np.squeeze(np.asarray(coord_rot))
-    return coord_rot[0], coord_rot[1], coord_rot[2], probe[3], probe[4], probe[5]
+    return coord_rot[0], coord_rot[1], -coord_rot[2], probe[3], probe[4], probe[5]
+
+def dynamic_reference_m2(probe, reference):
+    """
+    Apply dynamic reference correction to probe coordinates. Uses the alpha, beta and gama
+    rotation angles of reference to rotate the probe coordinate and returns the x, y, z
+    difference between probe and reference. Angles sequences and equation was extracted from
+    Polhemus manual and Attitude matrix in Wikipedia.
+    General equation is:
+    coord = Mrot * (probe - reference)
+    :param probe: sensor one defined as probe
+    :param reference: sensor two defined as reference
+    :return: rotated and translated coordinates
+    """
+
+    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 = np.asmatrix(tr.concatenate_matrices(T, R))
+    M_p = np.asmatrix(tr.concatenate_matrices(T_p, R_p))
+    # M = tr.compose_matrix(angles=np.radians(reference[3:6]), translate=reference[:3])
+    # print M
+
+    M_dyn = M.I * M_p
+
+    al, be, ga = tr.euler_from_matrix(M_dyn, 'rzyx')
+    coord_rot = tr.translation_from_matrix(M_dyn)
+
+    coord_rot = np.squeeze(coord_rot)
+
+    # probe_4 = np.vstack((np.asmatrix(probe[:3]).reshape([3, 1]), 1.))
+    # coord_rot_test = M.I * probe_4
+    # coord_rot_test = np.squeeze(np.asarray(coord_rot_test))
+    #
+    # print "coord_rot: ", coord_rot
+    # print "coord_rot_test: ", coord_rot_test
+    # print "test: ", np.allclose(coord_rot, coord_rot_test[:3])
+
+    return coord_rot[0], coord_rot[1], coord_rot[2], np.degrees(al), np.degrees(be), np.degrees(ga)
+
+# def dynamic_reference_m3(probe, reference):
+#     """
+#     Apply dynamic reference correction to probe coordinates. Uses the alpha, beta and gama
+#     rotation angles of reference to rotate the probe coordinate and returns the x, y, z
+#     difference between probe and reference. Angles sequences and equation was extracted from
+#     Polhemus manual and Attitude matrix in Wikipedia.
+#     General equation is:
+#     coord = Mrot * (probe - reference)
+#     :param probe: sensor one defined as probe
+#     :param reference: sensor two defined as reference
+#     :return: rotated and translated coordinates
+#     """
+#
+#     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 = np.asmatrix(tr.concatenate_matrices(T, R))
+#     M_p = np.asmatrix(tr.concatenate_matrices(T_p, R_p))
+#     # M = tr.compose_matrix(angles=np.radians(reference[3:6]), translate=reference[:3])
+#     # print M
+#
+#     M_dyn = M.I * M_p
+#
+#     # al, be, ga = tr.euler_from_matrix(M_dyn, 'rzyx')
+#     # coord_rot = tr.translation_from_matrix(M_dyn)
+#     #
+#     # coord_rot = np.squeeze(coord_rot)
+#
+#     # probe_4 = np.vstack((np.asmatrix(probe[:3]).reshape([3, 1]), 1.))
+#     # coord_rot_test = M.I * probe_4
+#     # coord_rot_test = np.squeeze(np.asarray(coord_rot_test))
+#     #
+#     # print "coord_rot: ", coord_rot
+#     # print "coord_rot_test: ", coord_rot_test
+#     # print "test: ", np.allclose(coord_rot, coord_rot_test[:3])
+#
+#     return M_dyn
 def str2float(data):
     """
-    Converts string detected wth Polhemus device to float array of coordinates. THis method applies
+    Converts string detected wth Polhemus device to float array of coordinates. This method applies
     a correction for the minus sign in string that raises error while splitting the string into coordinates.
     :param data: string of coordinates read with Polhemus
-    :return: six float coordinates x, y, z, alfa, beta and gama
+    :return: six float coordinates x, y, z, alpha, beta and gamma
     """
     count = 0
@@ -20,16 +20,123 @@
 import threading
 from time import sleep
-from numpy import mat
+from numpy import asmatrix, mat, degrees, radians, identity
 import wx
 from wx.lib.pubsub import pub as Publisher
 import invesalius.data.coordinates as dco
+import invesalius.data.transformations as tr
 # TODO: Optimize navigation thread. Remove the infinite loop and optimize sleep.
-class Coregistration(threading.Thread):
+class CoregistrationStatic(threading.Thread):
+    """
+    Thread to update the coordinates with the fiducial points
+    co-registration method while the Navigation Button is pressed.
+    Sleep function in run method is used to avoid blocking GUI and
+    for better real-time navigation
+    """
+
+    def __init__(self, coreg_data, nav_id, trck_info):
+        threading.Thread.__init__(self)
+        self.coreg_data = coreg_data
+        self.nav_id = nav_id
+        self.trck_info = trck_info
+        self._pause_ = False
+        self.start()
+
+    def stop(self):
+        self._pause_ = True
+
+    def run(self):
+        # m_change = self.coreg_data[0]
+        # obj_ref_mode = self.coreg_data[2]
+        #
+        # trck_init = self.trck_info[0]
+        # trck_id = self.trck_info[1]
+        # trck_mode = self.trck_info[2]
+
+        m_change, obj_ref_mode = self.coreg_data
+        trck_init, trck_id, trck_mode = self.trck_info
+
+        while self.nav_id:
+            coord_raw = dco.GetCoordinates(trck_init, trck_id, trck_mode)
+
+            psi, theta, phi = coord_raw[obj_ref_mode, 3:]
+            t_probe_raw = asmatrix(tr.translation_matrix(coord_raw[obj_ref_mode, :3]))
+
+            t_probe_raw[2, -1] = -t_probe_raw[2, -1]
+
+            m_img = m_change * t_probe_raw
+
+            coord = m_img[0, -1], m_img[1, -1], m_img[2, -1], psi, theta, phi
+
+            wx.CallAfter(Publisher.sendMessage, 'Co-registered points', (m_img, coord))
+
+            # TODO: Optimize the value of sleep for each tracking device.
+            sleep(0.175)
+
+            if self._pause_:
+                return
+
+
+class CoregistrationDynamic(threading.Thread):
+    """
+    Thread to update the coordinates with the fiducial points
+    co-registration method while the Navigation Button is pressed.
+    Sleep function in run method is used to avoid blocking GUI and
+    for better real-time navigation
+    """
+
+    def __init__(self, coreg_data, nav_id, trck_info):
+        threading.Thread.__init__(self)
+        self.coreg_data = coreg_data
+        self.nav_id = nav_id
+        self.trck_info = trck_info
+        self._pause_ = False
+        self.start()
+
+    def stop(self):
+        self._pause_ = True
+
+    def run(self):
+        m_change, obj_ref_mode = self.coreg_data
+        trck_init, trck_id, trck_mode = self.trck_info
+
+        while self.nav_id:
+            coord_raw = dco.GetCoordinates(trck_init, trck_id, trck_mode)
+
+            psi, theta, phi = radians(coord_raw[obj_ref_mode, 3:])
+            r_probe = tr.euler_matrix(psi, theta, phi, 'rzyx')
+            t_probe = tr.translation_matrix(coord_raw[obj_ref_mode, :3])
+            m_probe = asmatrix(tr.concatenate_matrices(t_probe, r_probe))
+
+            psi_ref, theta_ref, phi_ref = radians(coord_raw[1, 3:])
+            r_ref = tr.euler_matrix(psi_ref, theta_ref, phi_ref, 'rzyx')
+            t_ref = tr.translation_matrix(coord_raw[1, :3])
+            m_ref = asmatrix(tr.concatenate_matrices(t_ref, r_ref))
+
+            m_dyn = m_ref.I * m_probe
+            m_dyn[2, -1] = -m_dyn[2, -1]
+
+            m_img = m_change * m_dyn
+
+            scale, shear, angles, trans, persp = tr.decompose_matrix(m_img)
+
+            coord = m_img[0, -1], m_img[1, -1], m_img[2, -1], \
+                    degrees(angles[0]), degrees(angles[1]), degrees(angles[2])
+
+            wx.CallAfter(Publisher.sendMessage, 'Co-registered points', (m_img, coord))
+
+            # TODO: Optimize the value of sleep for each tracking device.
+            sleep(0.175)
+
+            if self._pause_:
+                return
+
+
+class CoregistrationDynamic_old(threading.Thread):
     """
     Thread to update the coordinates with the fiducial points
     co-registration method while the Navigation Button is pressed.
@@ -44,10 +151,10 @@ class Coregistration(threading.Thread):
         self.trck_info = trck_info
         self._pause_ = False
         self.start()
-        
+
     def stop(self):
         self._pause_ = True
-       
+
     def run(self):
         m_inv = self.bases[0]
         n = self.bases[1]
@@ -58,24 +165,199 @@ class Coregistration(threading.Thread):
         trck_mode = self.trck_info[2]
         while self.nav_id:
-            trck_coord = dco.GetCoordinates(trck_init, trck_id, trck_mode)
-            trck_xyz = mat([[trck_coord[0]], [trck_coord[1]], [trck_coord[2]]])
+            # trck_coord, probe, reference = dco.GetCoordinates(trck_init, trck_id, trck_mode)
+            coord_raw = dco.GetCoordinates(trck_init, trck_id, trck_mode)
-            img = q1 + (m_inv*n)*(trck_xyz - q2)
+            trck_coord = dco.dynamic_reference(coord_raw[0, :], coord_raw[1, :])
+
+            trck_xyz = mat([[trck_coord[0]], [trck_coord[1]], [trck_coord[2]]])
+            img = q1 + (m_inv * n) * (trck_xyz - q2)
             coord = (float(img[0]), float(img[1]), float(img[2]), trck_coord[3],
                      trck_coord[4], trck_coord[5])
+            angles = coord_raw[0, 3:6]
             # Tried several combinations and different locations to send the messages,
             # however only this one does not block the GUI during navigation.
-            wx.CallAfter(Publisher.sendMessage, 'Co-registered points', coord[0:3])
-            wx.CallAfter(Publisher.sendMessage, 'Set camera in volume', coord[0:3])
+            wx.CallAfter(Publisher.sendMessage, 'Co-registered points', coord)
+            wx.CallAfter(Publisher.sendMessage, 'Set camera in volume', coord)
+            wx.CallAfter(Publisher.sendMessage, 'Update tracker angles', angles)
             # TODO: Optimize the value of sleep for each tracking device.
             # Debug tracker is not working with 0.175 so changed to 0.2
             # However, 0.2 is too low update frequency ~5 Hz. Need optimization URGENTLY.
-            #sleep(.3)
+            # sleep(.3)
+            sleep(0.175)
+
+            if self._pause_:
+                return
+
+
+class CoregistrationObjectStatic(threading.Thread):
+    """
+    Thread to update the coordinates with the fiducial points
+    co-registration method while the Navigation Button is pressed.
+    Sleep function in run method is used to avoid blocking GUI and
+    for better real-time navigation
+    """
+
+    def __init__(self, coreg_data, nav_id, trck_info):
+        threading.Thread.__init__(self)
+        self.coreg_data = coreg_data
+        self.nav_id = nav_id
+        self.trck_info = trck_info
+        self._pause_ = False
+        self.start()
+
+    def stop(self):
+        self._pause_ = True
+
+    def run(self):
+        # m_change = self.coreg_data[0]
+        # t_obj_raw = self.coreg_data[1]
+        # s0_raw = self.coreg_data[2]
+        # r_s0_raw = self.coreg_data[3]
+        # s0_dyn = self.coreg_data[4]
+        # m_obj_raw = self.coreg_data[5]
+        # r_obj_img = self.coreg_data[6]
+        # obj_ref_mode = self.coreg_data[7]
+        #
+        # trck_init = self.trck_info[0]
+        # trck_id = self.trck_info[1]
+        # trck_mode = self.trck_info[2]
+
+        m_change, obj_ref_mode, t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img = self.coreg_data
+        trck_init, trck_id, trck_mode = self.trck_info
+
+        while self.nav_id:
+            coord_raw = dco.GetCoordinates(trck_init, trck_id, trck_mode)
+
+            as1, bs1, gs1 = radians(coord_raw[obj_ref_mode, 3:])
+            r_probe = asmatrix(tr.euler_matrix(as1, bs1, gs1, 'rzyx'))
+            t_probe_raw = asmatrix(tr.translation_matrix(coord_raw[obj_ref_mode, :3]))
+            t_offset_aux = r_s0_raw.I * r_probe * t_obj_raw
+            t_offset = asmatrix(identity(4))
+            t_offset[:, -1] = t_offset_aux[:, -1]
+            t_probe = s0_raw * t_offset * s0_raw.I * t_probe_raw
+            m_probe = asmatrix(tr.concatenate_matrices(t_probe, r_probe))
+
+            m_probe[2, -1] = -m_probe[2, -1]
+
+            m_img = m_change * m_probe
+            r_obj = r_obj_img * m_obj_raw.I * s0_dyn.I * m_probe * m_obj_raw
+
+            m_img[:3, :3] = r_obj[:3, :3]
+
+            scale, shear, angles, trans, persp = tr.decompose_matrix(m_img)
+
+            coord = m_img[0, -1], m_img[1, -1], m_img[2, -1], \
+                    degrees(angles[0]), degrees(angles[1]), degrees(angles[2])
+
+            wx.CallAfter(Publisher.sendMessage, 'Co-registered points', (m_img, coord))
+            wx.CallAfter(Publisher.sendMessage, 'Update object matrix', (m_img, coord))
+
+            # TODO: Optimize the value of sleep for each tracking device.
             sleep(0.175)
+            # Debug tracker is not working with 0.175 so changed to 0.2
+            # However, 0.2 is too low update frequency ~5 Hz. Need optimization URGENTLY.
+            # sleep(.3)
+
+            # partially working for translate and offset,
+            # but offset is kept always in same axis, have to fix for rotation
+            # M_dyn = M_reference.I * T_stylus
+            # M_dyn[2, -1] = -M_dyn[2, -1]
+            # M_dyn_ch = M_change * M_dyn
+            # ddd = M_dyn_ch[0, -1], M_dyn_ch[1, -1], M_dyn_ch[2, -1]
+            # M_dyn_ch[:3, -1] = asmatrix(db.flip_x_m(ddd)).reshape([3, 1])
+            # M_final = S0 * M_obj_trans_0 * S0.I * M_dyn_ch
+
+            # this works for static reference object rotation
+            # R_dyn = M_vtk * M_obj_rot_raw.I * S0_rot_raw.I * R_stylus * M_obj_rot_raw
+            # this works for dynamic reference in rotation but not in translation
+            # R_dyn = M_vtk * M_obj_rot_raw.I * S0_rot_dyn.I * R_reference.I * R_stylus * M_obj_rot_raw
+
             if self._pause_:
                 return
+
+
+class CoregistrationObjectDynamic(threading.Thread):
+    """
+    Thread to update the coordinates with the fiducial points
+    co-registration method while the Navigation Button is pressed.
+    Sleep function in run method is used to avoid blocking GUI and
+    for better real-time navigation
+    """
+
+    def __init__(self, coreg_data, nav_id, trck_info):
+        threading.Thread.__init__(self)
+        self.coreg_data = coreg_data
+        self.nav_id = nav_id
+        self.trck_info = trck_info
+        self._pause_ = False
+        self.start()
+
+    def stop(self):
+        self._pause_ = True
+
+    def run(self):
+
+        m_change, obj_ref_mode, t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img = self.coreg_data
+        trck_init, trck_id, trck_mode = self.trck_info
+
+        while self.nav_id:
+            coord_raw = dco.GetCoordinates(trck_init, trck_id, trck_mode)
+
+            as1, bs1, gs1 = radians(coord_raw[obj_ref_mode, 3:])
+            r_probe = asmatrix(tr.euler_matrix(as1, bs1, gs1, 'rzyx'))
+            t_probe_raw = asmatrix(tr.translation_matrix(coord_raw[obj_ref_mode, :3]))
+            t_offset_aux = r_s0_raw.I * r_probe * t_obj_raw
+            t_offset = asmatrix(identity(4))
+            t_offset[:, -1] = t_offset_aux[:, -1]
+            t_probe = s0_raw * t_offset * s0_raw.I * t_probe_raw
+            m_probe = asmatrix(tr.concatenate_matrices(t_probe, r_probe))
+
+            a, b, g = radians(coord_raw[1, 3:])
+            r_ref = tr.euler_matrix(a, b, g, 'rzyx')
+            t_ref = tr.translation_matrix(coord_raw[1, :3])
+            m_ref = asmatrix(tr.concatenate_matrices(t_ref, r_ref))
+
+            m_dyn = m_ref.I * m_probe
+            m_dyn[2, -1] = -m_dyn[2, -1]
+
+            m_img = m_change * m_dyn
+            r_obj = r_obj_img * m_obj_raw.I * s0_dyn.I * m_dyn * m_obj_raw
+
+            m_img[:3, :3] = r_obj[:3, :3]
+
+            scale, shear, angles, trans, persp = tr.decompose_matrix(m_img)
+
+            coord = m_img[0, -1], m_img[1, -1], m_img[2, -1],\
+                    degrees(angles[0]), degrees(angles[1]), degrees(angles[2])
+
+            wx.CallAfter(Publisher.sendMessage, 'Co-registered points', (m_img, coord))
+            wx.CallAfter(Publisher.sendMessage, 'Update object matrix', (m_img, coord))
+
+            # TODO: Optimize the value of sleep for each tracking device.
+            sleep(0.175)
+
+            # Debug tracker is not working with 0.175 so changed to 0.2
+            # However, 0.2 is too low update frequency ~5 Hz. Need optimization URGENTLY.
+            # sleep(.3)
+
+            # partially working for translate and offset,
+            # but offset is kept always in same axis, have to fix for rotation
+            # M_dyn = M_reference.I * T_stylus
+            # M_dyn[2, -1] = -M_dyn[2, -1]
+            # M_dyn_ch = M_change * M_dyn
+            # ddd = M_dyn_ch[0, -1], M_dyn_ch[1, -1], M_dyn_ch[2, -1]
+            # M_dyn_ch[:3, -1] = asmatrix(db.flip_x_m(ddd)).reshape([3, 1])
+            # M_final = S0 * M_obj_trans_0 * S0.I * M_dyn_ch
+
+            # this works for static reference object rotation
+            # R_dyn = M_vtk * M_obj_rot_raw.I * S0_rot_raw.I * R_stylus * M_obj_rot_raw
+            # this works for dynamic reference in rotation but not in translation
+            # R_dyn = M_vtk * M_obj_rot_raw.I * S0_rot_dyn.I * R_reference.I * R_stylus * M_obj_rot_raw
+
+            if self._pause_:
+                return
 \ No newline at end of file
@@ -0,0 +1,67 @@
+#--------------------------------------------------------------------------
+# Software:     InVesalius - Software de Reconstrucao 3D de Imagens Medicas
+# Copyright:    (C) 2001  Centro de Pesquisas Renato Archer
+# Homepage:     http://www.softwarepublico.gov.br
+# Contact:      invesalius@cti.gov.br
+# License:      GNU - GPL 2 (LICENSE.txt/LICENCA.txt)
+#--------------------------------------------------------------------------
+#    Este programa e software livre; voce pode redistribui-lo e/ou
+#    modifica-lo sob os termos da Licenca Publica Geral GNU, conforme
+#    publicada pela Free Software Foundation; de acordo com a versao 2
+#    da Licenca.
+#
+#    Este programa eh distribuido na expectativa de ser util, mas SEM
+#    QUALQUER GARANTIA; sem mesmo a garantia implicita de
+#    COMERCIALIZACAO ou de ADEQUACAO A QUALQUER PROPOSITO EM
+#    PARTICULAR. Consulte a Licenca Publica Geral GNU para obter mais
+#    detalhes.
+#--------------------------------------------------------------------------
+
+import threading
+import time
+
+import wx
+from numpy import array, savetxt, hstack,vstack, asarray
+import invesalius.gui.dialogs as dlg
+from wx.lib.pubsub import pub as Publisher
+
+
+class Record(threading.Thread):
+    """
+    Thread created to save obj coords with software during neuronavigation
+    """
+
+    def __init__(self, nav_id, timestamp):
+        threading.Thread.__init__(self)
+        self.nav_id = nav_id
+        self.coord = None
+        self.timestamp = timestamp
+        self.coord_list = array([])
+        self.__bind_events()
+        self._pause_ = False
+        self.start()
+
+    def __bind_events(self):
+        Publisher.subscribe(self.UpdateCurrentCoords, 'Co-registered points')
+
+    def UpdateCurrentCoords(self, pubsub_evt):
+        self.coord = asarray(pubsub_evt.data[1])
+
+    def stop(self):
+        self._pause_ = True
+        #save coords dialog
+        filename = dlg.ShowSaveCoordsDialog("coords.csv")
+        if filename:
+            savetxt(filename, self.coord_list, delimiter=',', fmt='%.4f', header="time, x, y, z, a, b, g", comments="")
+
+    def run(self):
+        initial_time = time.time()
+        while self.nav_id:
+            relative_time = asarray(time.time() - initial_time)
+            time.sleep(self.timestamp)
+            if self.coord_list.size == 0:
+                self.coord_list = hstack((relative_time, self.coord))
+            else:
+                self.coord_list = vstack((self.coord_list, hstack((relative_time, self.coord))))
+            if self._pause_:
+                return
 \ No newline at end of file
@@ -238,7 +238,7 @@ class CrossInteractorStyle(DefaultInteractorStyle):
         Publisher.sendMessage('Update cross position', (wx, wy, wz))
         self.ScrollSlice(coord)
         Publisher.sendMessage('Set ball reference position', (wx, wy, wz))
-        Publisher.sendMessage('Set camera in volume', (wx, wy, wz))
+        Publisher.sendMessage('Co-registered points',  (None, (wx, wy, wz, 0., 0., 0.)))
         iren.Render()
@@ -21,18 +21,19 @@
 # TODO: Test if there are too many prints when connection fails
-def TrackerConnection(tracker_id, action):
+def TrackerConnection(tracker_id, trck_init, action):
     """
-    Initialize spatial trackers for coordinate detection during navigation.
+    Initialize or disconnect spatial trackers for coordinate detection during navigation.
     :param tracker_id: ID of tracking device.
+    :param trck_init: tracker initialization instance.
     :param action: string with to decide whether connect or disconnect the selected device.
     :return spatial tracker initialization instance or None if could not open device.
     """
     if action == 'connect':
         trck_fcn = {1: ClaronTracker,
-                    2: PolhemusTrackerFT,    # FASTRAK
+                    2: PolhemusTracker,    # FASTRAK
                     3: PolhemusTracker,    # ISOTRAK
                     4: PolhemusTracker,    # PATRIOT
                     5: DebugTracker}
@@ -40,7 +41,7 @@ def TrackerConnection(tracker_id, action):
         trck_init = trck_fcn[tracker_id](tracker_id)
     elif action == 'disconnect':
-        trck_init = DisconnectTracker(tracker_id)
+        trck_init = DisconnectTracker(tracker_id, trck_init)
     return trck_init
@@ -87,29 +88,10 @@ def ClaronTracker(tracker_id):
     return trck_init, lib_mode
-def PolhemusTrackerFT(tracker_id):
-    trck_init = None
-    lib_mode = 'wrapper'
-    try:
-        import polhemusFT
-
-        trck_init = polhemusFT.polhemusFT()
-        trck_check = trck_init.Initialize()
-
-        if trck_check:
-            # First run is necessary to discard the first coord collection
-            trck_init.Run()
-        else:
-            trck_init = trck_check
-    except:
-        print 'Could not connect to Polhemus via wrapper.'
-
-    return trck_init, lib_mode
 def PolhemusTracker(tracker_id):
-    trck_init = None
     try:
-        trck_init = PlhWrapperConnection()
+        trck_init = PlhWrapperConnection(tracker_id)
         lib_mode = 'wrapper'
         if not trck_init:
             print 'Could not connect with Polhemus wrapper, trying USB connection...'
@@ -120,6 +102,7 @@ def PolhemusTracker(tracker_id):
                 trck_init = PlhSerialConnection(tracker_id)
                 lib_mode = 'serial'
     except:
+        trck_init = None
         lib_mode = 'error'
         print 'Could not connect to Polhemus.'
@@ -132,21 +115,29 @@ def DebugTracker(tracker_id):
     return trck_init, 'debug'
-def PlhWrapperConnection():
-    trck_init = None
+def PlhWrapperConnection(tracker_id):
     try:
-        import polhemus
+        from time import sleep
+        if tracker_id == 2:
+            import polhemusFT
+            trck_init = polhemusFT.polhemusFT()
+        else:
+            import polhemus
+            trck_init = polhemus.polhemus()
-        trck_init = polhemus.polhemus()
         trck_check = trck_init.Initialize()
         if trck_check:
-            # First run is necessary to discard the first coord collection
-            trck_init.Run()
+            # Sequence of runs necessary to throw away unnecessary data
+            for n in range(0, 5):
+                trck_init.Run()
+                sleep(0.175)
         else:
-            trck_init = trck_check
+            trck_init = None
+            print 'Could not connect to Polhemus via wrapper without error.'
     except:
-        print 'Could not connect to Polhemus via wrapper.'
+        trck_init = None
+        print 'Could not connect to Polhemus via wrapper with error.'
     return trck_init
@@ -172,10 +163,11 @@ def PlhSerialConnection(tracker_id):
         if not data:
             trck_init = None
+            print 'Could not connect to Polhemus serial without error.'
     except:
         trck_init = None
-        print 'Could not connect to Polhemus serial.'
+        print 'Could not connect to Polhemus serial with error.'
     return trck_init
@@ -184,7 +176,10 @@ def PlhUSBConnection(tracker_id):
     trck_init = None
     try:
         import usb.core as uc
-        trck_init = uc.find(idVendor=0x0F44, idProduct=0x0003)
+        # Check the idProduct using the usbdeview software, the idProduct is unique for each
+        # device and connection fails when is incorrect
+        # trck_init = uc.find(idVendor=0x0F44, idProduct=0x0003) [used in a different device]
+        trck_init = uc.find(idVendor=0x0F44, idProduct=0xEF12)
         cfg = trck_init.get_active_configuration()
         for i in cfg:
             for x in i:
@@ -203,58 +198,35 @@ def PlhUSBConnection(tracker_id):
                               endpoint.wMaxPacketSize)
         if not data:
             trck_init = None
+            print 'Could not connect to Polhemus USB without error.'
     except:
-        print 'Could not connect to Polhemus USB.'
+        print 'Could not connect to Polhemus USB with error.'
     return trck_init
-def DisconnectTracker(tracker_id):
+def DisconnectTracker(tracker_id, trck_init):
     """
     Disconnect current spatial tracker
     :param tracker_id: ID of tracking device.
+    :param trck_init: Initialization variable of tracking device.
     """
-    from wx.lib.pubsub import pub as Publisher
-    Publisher.sendMessage('Update status text in GUI', _("Disconnecting tracker ..."))
-    Publisher.sendMessage('Remove sensors ID')
-    trck_init = None
-    # TODO: create individual functions to disconnect each other device, e.g. Polhemus.
-    if tracker_id == 1:
-        try:
-            import pyclaron
-            pyclaron.pyclaron().Close()
-            lib_mode = 'wrapper'
-            print 'Claron tracker disconnected.'
-        except ImportError:
-            lib_mode = 'error'
-            print 'The ClaronTracker library is not installed.'
-
-    elif tracker_id == 2:
-        try:
-            import polhemusFT
-            polhemusFT.polhemusFT().Close()
-            lib_mode = 'wrapper'
-            print 'Polhemus tracker disconnected.'
-        except ImportError:
-            lib_mode = 'error'
-            print 'The polhemus library is not installed.'
-    elif tracker_id == 4:
+    if tracker_id == 5:
+        trck_init = False
+        lib_mode = 'debug'
+        print 'Debug tracker disconnected.'
+    else:
         try:
-            import polhemus
-            polhemus.polhemus().Close()
+            trck_init.Close()
+            trck_init = False
             lib_mode = 'wrapper'
-            print 'Polhemus tracker disconnected.'
-        except ImportError:
+            print 'Tracker disconnected.'
+        except:
+            trck_init = True
             lib_mode = 'error'
-            print 'The polhemus library is not installed.'
-
-    elif tracker_id == 5:
-        print 'Debug tracker disconnected.'
-        lib_mode = 'debug'
-
-    Publisher.sendMessage('Update status text in GUI', _("Ready"))
+            print 'The tracker could not be disconnected.'
     return trck_init, lib_mode
 \ No newline at end of file
@@ -934,13 +934,13 @@ class Viewer(wx.Panel):
     def UpdateSlicesNavigation(self, pubsub_evt):
         # Get point from base change
-        wx, wy, wz = pubsub_evt.data
-        px, py = self.get_slice_pixel_coord_by_world_pos(wx, wy, wz)
+        ux, uy, uz = pubsub_evt.data[1][:3]
+        px, py = self.get_slice_pixel_coord_by_world_pos(ux, uy, uz)
         coord = self.calcultate_scroll_position(px, py)
-        self.cross.SetFocalPoint((wx, wy, wz))
+        self.cross.SetFocalPoint((ux, uy, uz))
         self.ScrollSlice(coord)
-        Publisher.sendMessage('Set ball reference position', (wx, wy, wz))
+        Publisher.sendMessage('Set ball reference position', (ux, uy, uz))
     def ScrollSlice(self, coord):
         if self.orientation == "AXIAL":
@@ -20,6 +20,7 @@
 #    detalhes.
 #--------------------------------------------------------------------------
+from math import cos, sin
 import os
 import sys
@@ -29,14 +30,16 @@ import wx
 import vtk
 from vtk.wx.wxVTKRenderWindowInteractor import wxVTKRenderWindowInteractor
 from wx.lib.pubsub import pub as Publisher
+import random
+from scipy.spatial import distance
 import invesalius.constants as const
 import invesalius.data.bases as bases
+import invesalius.data.transformations as tr
 import invesalius.data.vtk_utils as vtku
 import invesalius.project as prj
 import invesalius.style as st
 import invesalius.utils as utils
-import invesalius.data.measures as measures
 if sys.platform == 'win32':
     try:
@@ -95,6 +98,14 @@ class Viewer(wx.Panel):
         self.text.SetValue("")
         self.ren.AddActor(self.text.actor)
+        # axes = vtk.vtkAxesActor()
+        # axes.SetXAxisLabelText('x')
+        # axes.SetYAxisLabelText('y')
+        # axes.SetZAxisLabelText('z')
+        # axes.SetTotalLength(50, 50, 50)
+        #
+        # self.ren.AddActor(axes)
+
         self.slice_plane = None
@@ -123,9 +134,20 @@ class Viewer(wx.Panel):
         self.repositioned_coronal_plan = 0
         self.added_actor = 0
-        self.camera_state = True
+        self.camera_state = const.CAM_MODE
+
+        self.nav_status = False
         self.ball_actor = None
+        self.obj_actor = None
+        self.obj_axes = None
+        self.obj_name = False
+        self.obj_state = None
+        self.obj_actor_list = None
+        self.arrow_actor_list = None
+
+        # self.obj_axes = None
+
         self._mode_cross = False
         self._to_show_ball = 0
         self._ball_ref_visibility = False
@@ -136,6 +158,13 @@ class Viewer(wx.Panel):
         self.timer = False
         self.index = False
+        self.target_coord = None
+        self.aim_actor = None
+        self.dummy_coil_actor = None
+        self.target_mode = False
+        self.anglethreshold = const.COIL_ANGLES_THRESHOLD
+        self.distthreshold = const.COIL_COORD_THRESHOLD
+
     def __bind_events(self):
         Publisher.subscribe(self.LoadActor,
                                  'Load surface actor into viewer')
@@ -159,7 +188,7 @@ class Viewer(wx.Panel):
                                 'Update raycasting preset')
         ###
         Publisher.subscribe(self.AppendActor,'AppendActor')
-        Publisher.subscribe(self.SetWidgetInteractor, 
+        Publisher.subscribe(self.SetWidgetInteractor,
                                 'Set Widget Interactor')
         Publisher.subscribe(self.OnSetViewAngle,
                                 'Set volume view angle')
@@ -173,7 +202,8 @@ class Viewer(wx.Panel):
         Publisher.subscribe(self.LoadSlicePlane, 'Load slice plane')
         Publisher.subscribe(self.ResetCamClippingRange, 'Reset cam clipping range')
-        Publisher.subscribe(self.SetVolumeCamera, 'Set camera in volume')
+        Publisher.subscribe(self.SetVolumeCamera, 'Co-registered points')
+        # Publisher.subscribe(self.SetVolumeCamera, 'Set camera in volume')
         Publisher.subscribe(self.SetVolumeCameraState, 'Update volume camera state')
         Publisher.subscribe(self.OnEnableStyle, 'Enable style')
@@ -190,16 +220,16 @@ class Viewer(wx.Panel):
         Publisher.subscribe(self.OnCloseProject, 'Close project data')
         Publisher.subscribe(self.RemoveAllActor, 'Remove all volume actors')
-        
+
         Publisher.subscribe(self.OnExportPicture,'Export picture to file')
         Publisher.subscribe(self.OnStartSeed,'Create surface by seeding - start')
         Publisher.subscribe(self.OnEndSeed,'Create surface by seeding - end')
         Publisher.subscribe(self.SetStereoMode, 'Set stereo mode')
-    
+
         Publisher.subscribe(self.Reposition3DPlane, 'Reposition 3D Plane')
-        
+
         Publisher.subscribe(self.RemoveVolume, 'Remove Volume')
         Publisher.subscribe(self.SetBallReferencePosition,
@@ -219,10 +249,25 @@ class Viewer(wx.Panel):
         Publisher.subscribe(self.BlinkMarker, 'Blink Marker')
         Publisher.subscribe(self.StopBlinkMarker, 'Stop Blink Marker')
+        # Related to object tracking during neuronavigation
+        Publisher.subscribe(self.OnNavigationStatus, 'Navigation status')
+        Publisher.subscribe(self.UpdateObjectOrientation, 'Update object matrix')
+        Publisher.subscribe(self.UpdateTrackObjectState, 'Update track object state')
+        Publisher.subscribe(self.UpdateShowObjectState, 'Update show object state')
+
+        Publisher.subscribe(self.ActivateTargetMode, 'Target navigation mode')
+        Publisher.subscribe(self.OnUpdateObjectTargetGuide, 'Update object matrix')
+        Publisher.subscribe(self.OnUpdateTargetCoordinates, 'Update target')
+        Publisher.subscribe(self.OnRemoveTarget, 'Disable or enable coil tracker')
+        # Publisher.subscribe(self.UpdateObjectTargetView, 'Co-registered points')
+        Publisher.subscribe(self.OnTargetMarkerTransparency, 'Set target transparency')
+        Publisher.subscribe(self.OnUpdateAngleThreshold, 'Update angle threshold')
+        Publisher.subscribe(self.OnUpdateDistThreshold, 'Update dist threshold')
+
     def SetStereoMode(self, pubsub_evt):
         mode = pubsub_evt.data
         ren_win = self.interactor.GetRenderWindow()
-        
+
         if mode == const.STEREO_OFF:
             ren_win.StereoRenderOff()
         else:
@@ -245,7 +290,7 @@ class Viewer(wx.Panel):
                 ren_win.SetStereoTypeToAnaglyph()
             ren_win.StereoRenderOn()
-        
+
         self.interactor.Render()
     def _check_ball_reference(self, pubsub_evt):
@@ -318,10 +363,10 @@ class Viewer(wx.Panel):
     def OnStartSeed(self, pubsub_evt):
         index = pubsub_evt.data
         self.seed_points = []
-    
+
     def OnEndSeed(self, pubsub_evt):
         Publisher.sendMessage("Create surface from seeds",
-                                    self.seed_points) 
+                                    self.seed_points)
     def OnExportPicture(self, pubsub_evt):
         id, filename, filetype = pubsub_evt.data
@@ -458,8 +503,7 @@ class Viewer(wx.Panel):
     def AddMarker(self, pubsub_evt):
         """
-        Markers create by navigation tools and
-        rendered in volume viewer.
+        Markers created by navigation tools and rendered in volume viewer.
         """
         self.ball_id = pubsub_evt.data[0]
         ballsize = pubsub_evt.data[1]
@@ -487,7 +531,7 @@ class Viewer(wx.Panel):
         self.ball_id = self.ball_id + 1
         #self.UpdateRender()
         self.Refresh()
-        
+
     def HideAllMarkers(self, pubsub_evt):
         ballid = pubsub_evt.data
         for i in range(0, ballid):
@@ -521,11 +565,11 @@ class Viewer(wx.Panel):
             self.staticballs[self.index].SetVisibility(1)
         self.index = pubsub_evt.data
         self.timer = wx.Timer(self)
-        self.Bind(wx.EVT_TIMER, self.blink, self.timer)
+        self.Bind(wx.EVT_TIMER, self.OnBlinkMarker, self.timer)
         self.timer.Start(500)
         self.timer_count = 0
-    def blink(self, evt):
+    def OnBlinkMarker(self, evt):
         self.staticballs[self.index].SetVisibility(int(self.timer_count % 2))
         self.Refresh()
         self.timer_count += 1
@@ -533,11 +577,499 @@ class Viewer(wx.Panel):
     def StopBlinkMarker(self, pubsub_evt):
         if self.timer:
             self.timer.Stop()
-            if pubsub_evt.data == None:
+            if pubsub_evt.data is None:
                 self.staticballs[self.index].SetVisibility(1)
                 self.Refresh()
             self.index = False
+    def OnTargetMarkerTransparency(self, pubsub_evt):
+        status = pubsub_evt.data[0]
+        index = pubsub_evt.data[1]
+        if status:
+            self.staticballs[index].GetProperty().SetOpacity(1)
+            # self.staticballs[index].GetProperty().SetOpacity(0.4)
+        else:
+            self.staticballs[index].GetProperty().SetOpacity(1)
+
+    def OnUpdateAngleThreshold(self, pubsub_evt):
+        self.anglethreshold = pubsub_evt.data
+
+    def OnUpdateDistThreshold(self, pubsub_evt):
+        self.distthreshold = pubsub_evt.data
+
+    def ActivateTargetMode(self, pubsub_evt):
+        self.target_mode = pubsub_evt.data
+        if self.target_coord and self.target_mode:
+            self.CreateTargetAim()
+
+            # Create a line
+            self.ren.SetViewport(0, 0, 0.75, 1)
+            self.ren2 = vtk.vtkRenderer()
+
+            self.interactor.GetRenderWindow().AddRenderer(self.ren2)
+            self.ren2.SetViewport(0.75, 0, 1, 1)
+            self.CreateTextDistance()
+
+            obj_polydata = self.CreateObjectPolyData(self.obj_name)
+
+            normals = vtk.vtkPolyDataNormals()
+            normals.SetInputData(obj_polydata)
+            normals.SetFeatureAngle(80)
+            normals.AutoOrientNormalsOn()
+            normals.Update()
+
+            mapper = vtk.vtkPolyDataMapper()
+            mapper.SetInputData(normals.GetOutput())
+            mapper.ScalarVisibilityOff()
+            mapper.ImmediateModeRenderingOn()  # improve performance
+
+            obj_roll = vtk.vtkActor()
+            obj_roll.SetMapper(mapper)
+            obj_roll.SetPosition(0, 25, -30)
+            obj_roll.RotateX(-60)
+            obj_roll.RotateZ(180)
+
+            obj_yaw = vtk.vtkActor()
+            obj_yaw.SetMapper(mapper)
+            obj_yaw.SetPosition(0, -115, 5)
+            obj_yaw.RotateZ(180)
+
+            obj_pitch = vtk.vtkActor()
+            obj_pitch.SetMapper(mapper)
+            obj_pitch.SetPosition(5, -265, 5)
+            obj_pitch.RotateY(90)
+            obj_pitch.RotateZ(180)
+
+            arrow_roll_z1 = self.CreateArrowActor([-50, -35, 12], [-50, -35, 50])
+            arrow_roll_z1.GetProperty().SetColor(1, 1, 0)
+            arrow_roll_z1.RotateX(-60)
+            arrow_roll_z1.RotateZ(180)
+            arrow_roll_z2 = self.CreateArrowActor([50, -35, 0], [50, -35, -50])
+            arrow_roll_z2.GetProperty().SetColor(1, 1, 0)
+            arrow_roll_z2.RotateX(-60)
+            arrow_roll_z2.RotateZ(180)
+
+            arrow_yaw_y1 = self.CreateArrowActor([-50, -35, 0], [-50, 5, 0])
+            arrow_yaw_y1.GetProperty().SetColor(0, 1, 0)
+            arrow_yaw_y1.SetPosition(0, -150, 0)
+            arrow_yaw_y1.RotateZ(180)
+            arrow_yaw_y2 = self.CreateArrowActor([50, -35, 0], [50, -75, 0])
+            arrow_yaw_y2.GetProperty().SetColor(0, 1, 0)
+            arrow_yaw_y2.SetPosition(0, -150, 0)
+            arrow_yaw_y2.RotateZ(180)
+
+            arrow_pitch_x1 = self.CreateArrowActor([0, 65, 38], [0, 65, 68])
+            arrow_pitch_x1.GetProperty().SetColor(1, 0, 0)
+            arrow_pitch_x1.SetPosition(0, -300, 0)
+            arrow_pitch_x1.RotateY(90)
+            arrow_pitch_x1.RotateZ(180)
+            arrow_pitch_x2 = self.CreateArrowActor([0, -55, 5], [0, -55, -30])
+            arrow_pitch_x2.GetProperty().SetColor(1, 0, 0)
+            arrow_pitch_x2.SetPosition(0, -300, 0)
+            arrow_pitch_x2.RotateY(90)
+            arrow_pitch_x2.RotateZ(180)
+
+            self.obj_actor_list = obj_roll, obj_yaw, obj_pitch
+            self.arrow_actor_list = arrow_roll_z1, arrow_roll_z2, arrow_yaw_y1, arrow_yaw_y2,\
+                                     arrow_pitch_x1, arrow_pitch_x2
+
+            for ind in self.obj_actor_list:
+                self.ren2.AddActor(ind)
+
+            for ind in self.arrow_actor_list:
+                self.ren2.AddActor(ind)
+
+            self.ren.ResetCamera()
+            self.SetCameraTarget()
+            #self.ren.GetActiveCamera().Zoom(4)
+
+            self.ren2.ResetCamera()
+            self.ren2.GetActiveCamera().Zoom(2)
+            self.ren2.InteractiveOff()
+            self.interactor.Render()
+
+        else:
+            self.DisableCoilTracker()
+
+    def OnUpdateObjectTargetGuide(self, pubsub_evt):
+        coord = pubsub_evt.data[1]
+        if self.target_coord and self.target_mode:
+
+            target_dist = distance.euclidean(coord[0:3],
+                                             (self.target_coord[0], -self.target_coord[1], self.target_coord[2]))
+
+            # self.txt.SetCoilDistanceValue(target_dist)
+            self.textSource.SetText('Dist: ' + str("{:06.2f}".format(target_dist)) + ' mm')
+            self.ren.ResetCamera()
+            self.SetCameraTarget()
+            if target_dist > 100:
+                target_dist = 100
+            # ((-0.0404*dst) + 5.0404) is the linear equation to normalize the zoom between 1 and 5 times with
+            # the distance between 1 and 100 mm
+            self.ren.GetActiveCamera().Zoom((-0.0404 * target_dist) + 5.0404)
+
+            if target_dist <= self.distthreshold:
+                thrdist = True
+                self.aim_actor.GetProperty().SetColor(0, 1, 0)
+            else:
+                thrdist = False
+                self.aim_actor.GetProperty().SetColor(1, 1, 1)
+
+            coordx = self.target_coord[3] - coord[3]
+            if coordx > const.ARROW_UPPER_LIMIT:
+                coordx = const.ARROW_UPPER_LIMIT
+            elif coordx < -const.ARROW_UPPER_LIMIT:
+                coordx = -const.ARROW_UPPER_LIMIT
+            coordx = const.ARROW_SCALE * coordx
+
+            coordy = self.target_coord[4] - coord[4]
+            if coordy > const.ARROW_UPPER_LIMIT:
+                coordy = const.ARROW_UPPER_LIMIT
+            elif coordy < -const.ARROW_UPPER_LIMIT:
+                coordy = -const.ARROW_UPPER_LIMIT
+            coordy = const.ARROW_SCALE * coordy
+
+            coordz = self.target_coord[5] - coord[5]
+            if coordz > const.ARROW_UPPER_LIMIT:
+                coordz = const.ARROW_UPPER_LIMIT
+            elif coordz < -const.ARROW_UPPER_LIMIT:
+                coordz = -const.ARROW_UPPER_LIMIT
+            coordz = const.ARROW_SCALE * coordz
+
+            for ind in self.arrow_actor_list:
+                self.ren2.RemoveActor(ind)
+
+            if self.anglethreshold * const.ARROW_SCALE > coordx > -self.anglethreshold * const.ARROW_SCALE:
+                thrcoordx = True
+                self.obj_actor_list[0].GetProperty().SetColor(0, 1, 0)
+            else:
+                thrcoordx = False
+                self.obj_actor_list[0].GetProperty().SetColor(1, 1, 1)
+
+            offset = 5
+
+            arrow_roll_x1 = self.CreateArrowActor([-55, -35, offset], [-55, -35, offset - coordx])
+            arrow_roll_x1.RotateX(-60)
+            arrow_roll_x1.RotateZ(180)
+            arrow_roll_x1.GetProperty().SetColor(1, 1, 0)
+
+            arrow_roll_x2 = self.CreateArrowActor([55, -35, offset], [55, -35, offset + coordx])
+            arrow_roll_x2.RotateX(-60)
+            arrow_roll_x2.RotateZ(180)
+            arrow_roll_x2.GetProperty().SetColor(1, 1, 0)
+
+            if self.anglethreshold * const.ARROW_SCALE > coordz > -self.anglethreshold * const.ARROW_SCALE:
+                thrcoordz = True
+                self.obj_actor_list[1].GetProperty().SetColor(0, 1, 0)
+            else:
+                thrcoordz = False
+                self.obj_actor_list[1].GetProperty().SetColor(1, 1, 1)
+
+            offset = -35
+
+            arrow_yaw_z1 = self.CreateArrowActor([-55, offset, 0], [-55, offset - coordz, 0])
+            arrow_yaw_z1.SetPosition(0, -150, 0)
+            arrow_yaw_z1.RotateZ(180)
+            arrow_yaw_z1.GetProperty().SetColor(0, 1, 0)
+
+            arrow_yaw_z2 = self.CreateArrowActor([55, offset, 0], [55, offset + coordz, 0])
+            arrow_yaw_z2.SetPosition(0, -150, 0)
+            arrow_yaw_z2.RotateZ(180)
+            arrow_yaw_z2.GetProperty().SetColor(0, 1, 0)
+
+            if self.anglethreshold * const.ARROW_SCALE > coordy > -self.anglethreshold * const.ARROW_SCALE:
+                thrcoordy = True
+                self.obj_actor_list[2].GetProperty().SetColor(0, 1, 0)
+            else:
+                thrcoordy = False
+                self.obj_actor_list[2].GetProperty().SetColor(1, 1, 1)
+
+            offset = 38
+            arrow_pitch_y1 = self.CreateArrowActor([0, 65, offset], [0, 65, offset + coordy])
+            arrow_pitch_y1.SetPosition(0, -300, 0)
+            arrow_pitch_y1.RotateY(90)
+            arrow_pitch_y1.RotateZ(180)
+            arrow_pitch_y1.GetProperty().SetColor(1, 0, 0)
+
+            offset = 5
+            arrow_pitch_y2 = self.CreateArrowActor([0, -55, offset], [0, -55, offset - coordy])
+            arrow_pitch_y2.SetPosition(0, -300, 0)
+            arrow_pitch_y2.RotateY(90)
+            arrow_pitch_y2.RotateZ(180)
+            arrow_pitch_y2.GetProperty().SetColor(1, 0, 0)
+
+            if thrdist and thrcoordx and thrcoordy and thrcoordz:
+                self.dummy_coil_actor.GetProperty().SetColor(0, 1, 0)
+            else:
+                self.dummy_coil_actor.GetProperty().SetColor(1, 1, 1)
+
+            self.arrow_actor_list = arrow_roll_x1, arrow_roll_x2, arrow_yaw_z1, arrow_yaw_z2, \
+                                    arrow_pitch_y1, arrow_pitch_y2
+
+            for ind in self.arrow_actor_list:
+                self.ren2.AddActor(ind)
+
+            self.Refresh()
+
+    def OnUpdateTargetCoordinates(self, pubsub_evt):
+        self.target_coord = pubsub_evt.data[0:6]
+        self.target_coord[1] = -self.target_coord[1]
+        self.CreateTargetAim()
+
+    def OnRemoveTarget(self, pubsub_evt):
+        status = pubsub_evt.data
+        if not status:
+            self.target_mode = None
+            self.target_coord = None
+            self.RemoveTargetAim()
+            self.DisableCoilTracker()
+
+    def CreateTargetAim(self):
+        if self.aim_actor:
+            self.RemoveTargetAim()
+            self.aim_actor = None
+
+        self.textSource = vtk.vtkVectorText()
+        mapper = vtk.vtkPolyDataMapper()
+        mapper.SetInputConnection(self.textSource.GetOutputPort())
+        tactor = vtk.vtkFollower()
+        tactor.SetMapper(mapper)
+        tactor.GetProperty().SetColor(1.0, 0.25, 0.0)
+        tactor.SetScale(5)
+        tactor.SetPosition(self.target_coord[0]+10, self.target_coord[1]+30, self.target_coord[2]+20)
+        self.ren.AddActor(tactor)
+        self.tactor = tactor
+        tactor.SetCamera(self.ren.GetActiveCamera())
+
+
+        # v3, M_plane_inv = self.Plane(self.target_coord[0:3], self.pTarget)
+        # mat4x4 = vtk.vtkMatrix4x4()
+        # for i in range(4):
+        #     mat4x4.SetElement(i, 0, M_plane_inv[i][0])
+        #     mat4x4.SetElement(i, 1, M_plane_inv[i][1])
+        #     mat4x4.SetElement(i, 2, M_plane_inv[i][2])
+        #     mat4x4.SetElement(i, 3, M_plane_inv[i][3])
+
+        a, b, g = np.radians(self.target_coord[3:])
+        r_ref = tr.euler_matrix(a, b, g, 'sxyz')
+        t_ref = tr.translation_matrix(self.target_coord[:3])
+        m_img = np.asmatrix(tr.concatenate_matrices(t_ref, r_ref))
+
+        m_img_vtk = vtk.vtkMatrix4x4()
+
+        for row in range(0, 4):
+            for col in range(0, 4):
+                m_img_vtk.SetElement(row, col, m_img[row, col])
+
+        self.m_img_vtk = m_img_vtk
+
+        filename = os.path.join(const.OBJ_DIR, "aim.stl")
+
+        reader = vtk.vtkSTLReader()
+        reader.SetFileName(filename)
+        mapper = vtk.vtkPolyDataMapper()
+        mapper.SetInputConnection(reader.GetOutputPort())
+
+        # Transform the polydata
+        transform = vtk.vtkTransform()
+        transform.SetMatrix(m_img_vtk)
+        #transform.SetMatrix(mat4x4)
+        transformPD = vtk.vtkTransformPolyDataFilter()
+        transformPD.SetTransform(transform)
+        transformPD.SetInputConnection(reader.GetOutputPort())
+        transformPD.Update()
+        # mapper transform
+        mapper.SetInputConnection(transformPD.GetOutputPort())
+
+        aim_actor = vtk.vtkActor()
+        aim_actor.SetMapper(mapper)
+        aim_actor.GetProperty().SetColor(1, 1, 1)
+        aim_actor.GetProperty().SetOpacity(0.6)
+        self.aim_actor = aim_actor
+        self.ren.AddActor(aim_actor)
+
+        obj_polydata = self.CreateObjectPolyData(os.path.join(const.OBJ_DIR, "magstim_fig8_coil_no_handle.stl"))
+
+        transform = vtk.vtkTransform()
+        transform.RotateZ(90)
+
+        transform_filt = vtk.vtkTransformPolyDataFilter()
+        transform_filt.SetTransform(transform)
+        transform_filt.SetInputData(obj_polydata)
+        transform_filt.Update()
+
+        normals = vtk.vtkPolyDataNormals()
+        normals.SetInputData(transform_filt.GetOutput())
+        normals.SetFeatureAngle(80)
+        normals.AutoOrientNormalsOn()
+        normals.Update()
+
+        obj_mapper = vtk.vtkPolyDataMapper()
+        obj_mapper.SetInputData(normals.GetOutput())
+        obj_mapper.ScalarVisibilityOff()
+        obj_mapper.ImmediateModeRenderingOn()  # improve performance
+
+        self.dummy_coil_actor = vtk.vtkActor()
+        self.dummy_coil_actor.SetMapper(obj_mapper)
+        self.dummy_coil_actor.GetProperty().SetOpacity(0.4)
+        self.dummy_coil_actor.SetVisibility(1)
+        self.dummy_coil_actor.SetUserMatrix(m_img_vtk)
+
+        self.ren.AddActor(self.dummy_coil_actor)
+
+        self.Refresh()
+
+    def RemoveTargetAim(self):
+        self.ren.RemoveActor(self.aim_actor)
+        self.ren.RemoveActor(self.dummy_coil_actor)
+        self.ren.RemoveActor(self.tactor)
+        self.Refresh()
+
+    def CreateTextDistance(self):
+        txt = vtku.Text()
+        txt.SetSize(const.TEXT_SIZE_EXTRA_LARGE)
+        txt.SetPosition((0.76, 0.05))
+        txt.ShadowOff()
+        txt.BoldOn()
+        self.txt = txt
+        self.ren2.AddActor(txt.actor)
+
+    def DisableCoilTracker(self):
+        try:
+            self.ren.SetViewport(0, 0, 1, 1)
+            self.interactor.GetRenderWindow().RemoveRenderer(self.ren2)
+            self.SetViewAngle(const.VOL_FRONT)
+            self.ren.RemoveActor(self.txt.actor)
+            self.CreateTargetAim()
+            self.interactor.Render()
+        except:
+            None
+
+    def CreateArrowActor(self, startPoint, endPoint):
+        # Compute a basis
+        normalizedX = [0 for i in range(3)]
+        normalizedY = [0 for i in range(3)]
+        normalizedZ = [0 for i in range(3)]
+
+        # The X axis is a vector from start to end
+        math = vtk.vtkMath()
+        math.Subtract(endPoint, startPoint, normalizedX)
+        length = math.Norm(normalizedX)
+        math.Normalize(normalizedX)
+
+        # The Z axis is an arbitrary vector cross X
+        arbitrary = [0 for i in range(3)]
+        arbitrary[0] = random.uniform(-10, 10)
+        arbitrary[1] = random.uniform(-10, 10)
+        arbitrary[2] = random.uniform(-10, 10)
+        math.Cross(normalizedX, arbitrary, normalizedZ)
+        math.Normalize(normalizedZ)
+
+        # The Y axis is Z cross X
+        math.Cross(normalizedZ, normalizedX, normalizedY)
+        matrix = vtk.vtkMatrix4x4()
+
+        # Create the direction cosine matrix
+        matrix.Identity()
+        for i in range(3):
+            matrix.SetElement(i, 0, normalizedX[i])
+            matrix.SetElement(i, 1, normalizedY[i])
+            matrix.SetElement(i, 2, normalizedZ[i])
+
+        # Apply the transforms arrow 1
+        transform_1 = vtk.vtkTransform()
+        transform_1.Translate(startPoint)
+        transform_1.Concatenate(matrix)
+        transform_1.Scale(length, length, length)
+        # source
+        arrowSource1 = vtk.vtkArrowSource()
+        arrowSource1.SetTipResolution(50)
+        # Create a mapper and actor
+        mapper = vtk.vtkPolyDataMapper()
+        mapper.SetInputConnection(arrowSource1.GetOutputPort())
+        # Transform the polydata
+        transformPD = vtk.vtkTransformPolyDataFilter()
+        transformPD.SetTransform(transform_1)
+        transformPD.SetInputConnection(arrowSource1.GetOutputPort())
+        # mapper transform
+        mapper.SetInputConnection(transformPD.GetOutputPort())
+        # actor
+        actor_arrow = vtk.vtkActor()
+        actor_arrow.SetMapper(mapper)
+
+        return actor_arrow
+
+    def CenterOfMass(self):
+        proj = prj.Project()
+        surface = proj.surface_dict[0].polydata
+        barycenter = [0.0, 0.0, 0.0]
+        n = surface.GetNumberOfPoints()
+        for i in range(n):
+            point = surface.GetPoint(i)
+            barycenter[0] += point[0]
+            barycenter[1] += point[1]
+            barycenter[2] += point[2]
+        barycenter[0] /= n
+        barycenter[1] /= n
+        barycenter[2] /= n
+
+        return barycenter
+
+    def Plane(self, x0, pTarget):
+        v3 = np.array(pTarget) - x0  # normal to the plane
+        v3 = v3 / np.linalg.norm(v3)  # unit vector
+
+        d = np.dot(v3, x0)
+        # prevents division by zero.
+        if v3[0] == 0.0:
+            v3[0] = 1e-09
+
+        x1 = np.array([(d - v3[1] - v3[2]) / v3[0], 1, 1])
+        v2 = x1 - x0
+        v2 = v2 / np.linalg.norm(v2)  # unit vector
+        v1 = np.cross(v3, v2)
+        v1 = v1 / np.linalg.norm(v1)  # unit vector
+        x2 = x0 + v1
+        # calculates the matrix for the change of coordinate systems (from canonical to the plane's).
+        # remember that, in np.dot(M,p), even though p is a line vector (e.g.,np.array([1,2,3])), it is treated as a column for the dot multiplication.
+        M_plane_inv = np.array([[v1[0], v2[0], v3[0], x0[0]],
+                                [v1[1], v2[1], v3[1], x0[1]],
+                                [v1[2], v2[2], v3[2], x0[2]],
+                                [0, 0, 0, 1]])
+
+        return v3, M_plane_inv
+
+    def SetCameraTarget(self):
+        cam_focus = self.target_coord[0:3]
+        cam = self.ren.GetActiveCamera()
+
+        oldcamVTK = vtk.vtkMatrix4x4()
+        oldcamVTK.DeepCopy(cam.GetViewTransformMatrix())
+
+        newvtk = vtk.vtkMatrix4x4()
+        newvtk.Multiply4x4(self.m_img_vtk, oldcamVTK, newvtk)
+
+        transform = vtk.vtkTransform()
+        transform.SetMatrix(newvtk)
+        transform.Update()
+        cam.ApplyTransform(transform)
+
+        cam.Roll(90)
+
+        cam_pos0 = np.array(cam.GetPosition())
+        cam_focus0 = np.array(cam.GetFocalPoint())
+        v0 = cam_pos0 - cam_focus0
+        v0n = np.sqrt(inner1d(v0, v0))
+
+        v1 = (cam_focus[0] - cam_focus0[0], cam_focus[1] - cam_focus0[1], cam_focus[2] - cam_focus0[2])
+        v1n = np.sqrt(inner1d(v1, v1))
+        if not v1n:
+            v1n = 1.0
+        cam_pos = (v1 / v1n) * v0n + cam_focus
+        cam.SetFocalPoint(cam_focus)
+        cam.SetPosition(cam_pos)
+
+
     def CreateBallReference(self):
         """
         Red sphere on volume visualization to reference center of
@@ -545,7 +1077,7 @@ class Viewer(wx.Panel):
         The sphere's radius will be scale times bigger than the average of
         image spacing values.
         """
-        scale = 3.0
+        scale = 2.0
         proj = prj.Project()
         s = proj.spacing
         r = (s[0] + s[1] + s[2]) / 3.0 * scale
@@ -588,6 +1120,129 @@ class Viewer(wx.Panel):
         else:
             self.RemoveBallReference()
+    def CreateObjectPolyData(self, filename):
+        """
+        Coil for navigation rendered in volume viewer.
+        """
+
+        if filename:
+            if filename.lower().endswith('.stl'):
+                reader = vtk.vtkSTLReader()
+            elif filename.lower().endswith('.ply'):
+                reader = vtk.vtkPLYReader()
+            elif filename.lower().endswith('.obj'):
+                reader = vtk.vtkOBJReader()
+            elif filename.lower().endswith('.vtp'):
+                reader = vtk.vtkXMLPolyDataReader()
+            else:
+                wx.MessageBox(_("File format not reconized by InVesalius"), _("Import surface error"))
+                return
+        else:
+            filename = os.path.join(const.OBJ_DIR, "magstim_fig8_coil.stl")
+            reader = vtk.vtkSTLReader()
+
+        if _has_win32api:
+            obj_name = win32api.GetShortPathName(filename).encode(const.FS_ENCODE)
+        else:
+            obj_name = filename.encode(const.FS_ENCODE)
+
+        reader.SetFileName(obj_name)
+        reader.Update()
+        obj_polydata = reader.GetOutput()
+
+        if obj_polydata.GetNumberOfPoints() == 0:
+            wx.MessageBox(_("InVesalius was not able to import this surface"), _("Import surface error"))
+            obj_polydata = None
+
+        return obj_polydata
+
+    def AddObjectActor(self, obj_name):
+        """
+        Coil for navigation rendered in volume viewer.
+        """
+
+        obj_polydata = self.CreateObjectPolyData(obj_name)
+
+        transform = vtk.vtkTransform()
+        transform.RotateZ(90)
+
+        transform_filt = vtk.vtkTransformPolyDataFilter()
+        transform_filt.SetTransform(transform)
+        transform_filt.SetInputData(obj_polydata)
+        transform_filt.Update()
+
+        normals = vtk.vtkPolyDataNormals()
+        normals.SetInputData(transform_filt.GetOutput())
+        normals.SetFeatureAngle(80)
+        normals.AutoOrientNormalsOn()
+        normals.Update()
+
+        obj_mapper = vtk.vtkPolyDataMapper()
+        obj_mapper.SetInputData(normals.GetOutput())
+        obj_mapper.ScalarVisibilityOff()
+        obj_mapper.ImmediateModeRenderingOn()  # improve performance
+
+        self.obj_actor = vtk.vtkActor()
+        self.obj_actor.SetMapper(obj_mapper)
+        self.obj_actor.GetProperty().SetOpacity(0.9)
+        self.obj_actor.SetVisibility(0)
+
+        self.ren.AddActor(self.obj_actor)
+
+        # self.obj_axes = vtk.vtkAxesActor()
+        # self.obj_axes.SetShaftTypeToCylinder()
+        # self.obj_axes.SetXAxisLabelText("x")
+        # self.obj_axes.SetYAxisLabelText("y")
+        # self.obj_axes.SetZAxisLabelText("z")
+        # self.obj_axes.SetTotalLength(50.0, 50.0, 50.0)
+
+        # self.ren.AddActor(self.obj_axes)
+
+    def OnNavigationStatus(self, pubsub_evt):
+        self.nav_status = pubsub_evt.data
+        self.pTarget = self.CenterOfMass()
+        if self.obj_actor and self.nav_status:
+            self.obj_actor.SetVisibility(self.obj_state)
+            if not self.obj_state:
+                self.Refresh()
+
+    def UpdateObjectOrientation(self, pubsub_evt):
+
+        m_img = pubsub_evt.data[0]
+
+        m_img[:3, -1] = np.asmatrix(bases.flip_x_m((m_img[0, -1], m_img[1, -1], m_img[2, -1]))).reshape([3, 1])
+
+        m_img_vtk = vtk.vtkMatrix4x4()
+
+        for row in range(0, 4):
+            for col in range(0, 4):
+                m_img_vtk.SetElement(row, col, m_img[row, col])
+
+        self.obj_actor.SetUserMatrix(m_img_vtk)
+        # self.obj_axes.SetUserMatrix(m_rot_vtk)
+
+        self.Refresh()
+
+    def UpdateTrackObjectState(self, pubsub_evt):
+        if pubsub_evt.data[0]:
+            self.obj_name = pubsub_evt.data[1]
+
+            if not self.obj_actor:
+                self.AddObjectActor(self.obj_name)
+
+        else:
+            if self.obj_actor:
+                self.ren.RemoveActor(self.obj_actor)
+                self.obj_actor = None
+
+        self.Refresh()
+
+    def UpdateShowObjectState(self, pubsub_evt):
+        self.obj_state = pubsub_evt.data
+        if self.obj_actor and not self.obj_state:
+            self.obj_actor.SetVisibility(self.obj_state)
+            self.Refresh()
+
     def __bind_events_wx(self):
         #self.Bind(wx.EVT_SIZE, self.OnSize)
         pass
@@ -613,7 +1268,7 @@ class Viewer(wx.Panel):
                     "LeftButtonReleaseEvent": self.OnReleaseSpinClick,
                     },
               const.STATE_WL:
-                    { 
+                    {
                     "MouseMoveEvent": self.OnWindowLevelMove,
                     "LeftButtonPressEvent": self.OnWindowLevelClick,
                     "LeftButtonReleaseEvent":self.OnWindowLevelRelease
@@ -661,7 +1316,7 @@ class Viewer(wx.Panel):
         else:
             style = vtk.vtkInteractorStyleTrackballCamera()
             self.interactor.SetInteractorStyle(style)
-            self.style = style  
+            self.style = style
             # Check each event available for each mode
             for event in action[state]:
@@ -755,8 +1410,8 @@ class Viewer(wx.Panel):
     def SetVolumeCamera(self, pubsub_evt):
         if self.camera_state:
-            #TODO: exclude dependency on initial focus
-            cam_focus = np.array(bases.flip_x(pubsub_evt.data))
+            # TODO: exclude dependency on initial focus
+            cam_focus = np.array(bases.flip_x(pubsub_evt.data[1][:3]))
             cam = self.ren.GetActiveCamera()
             if self.initial_focus is None:
@@ -764,11 +1419,14 @@ class Viewer(wx.Panel):
             cam_pos0 = np.array(cam.GetPosition())
             cam_focus0 = np.array(cam.GetFocalPoint())
-
             v0 = cam_pos0 - cam_focus0
             v0n = np.sqrt(inner1d(v0, v0))
-            v1 = (cam_focus - self.initial_focus)
+            if self.obj_state:
+                v1 = (cam_focus[0] - self.pTarget[0], cam_focus[1] - self.pTarget[1], cam_focus[2] - self.pTarget[2])
+            else:
+                v1 = (cam_focus - self.initial_focus)
+
             v1n = np.sqrt(inner1d(v1, v1))
             if not v1n:
                 v1n = 1.0
@@ -960,7 +1618,7 @@ class Viewer(wx.Panel):
         cam.SetViewUp(xv,yv,zv)
         cam.SetPosition(xp,yp,zp)
-        self.ren.ResetCameraClippingRange() 
+        self.ren.ResetCameraClippingRange()
         self.ren.ResetCamera()
         self.interactor.Render()
@@ -1020,10 +1678,10 @@ class Viewer(wx.Panel):
         x,y = self.interactor.GetEventPosition()
         self.measure_picker.Pick(x, y, 0, self.ren)
         x, y, z = self.measure_picker.GetPickPosition()
-        
+
         proj = prj.Project()
         radius = min(proj.spacing) * PROP_MEASURE
-        if self.measure_picker.GetActor(): 
+        if self.measure_picker.GetActor():
             # if not self.measures or self.measures[-1].IsComplete():
                 # m = measures.LinearMeasure(self.ren)
                 # m.AddPoint(x, y, z)
@@ -1032,7 +1690,7 @@ class Viewer(wx.Panel):
                 # m = self.measures[-1]
                 # m.AddPoint(x, y, z)
                 # if m.IsComplete():
-                    # Publisher.sendMessage("Add measure to list", 
+                    # Publisher.sendMessage("Add measure to list",
                             # (u"3D", _(u"%.3f mm" % m.GetValue())))
             Publisher.sendMessage("Add measurement point",
                     ((x, y,z), const.LINEAR, const.SURFACE, radius))
@@ -1045,7 +1703,7 @@ class Viewer(wx.Panel):
         proj = prj.Project()
         radius = min(proj.spacing) * PROP_MEASURE
-        if self.measure_picker.GetActor(): 
+        if self.measure_picker.GetActor():
             # if not self.measures or self.measures[-1].IsComplete():
                 # m = measures.AngularMeasure(self.ren)
                 # m.AddPoint(x, y, z)
@@ -124,6 +124,9 @@ class Text(object):
     def ShadowOff(self):
         self.property.ShadowOff()
+    def BoldOn(self):
+        self.property.BoldOn()
+
     def SetSize(self, size):
         self.property.SetFontSize(size)
@@ -135,15 +138,32 @@ class Text(object):
         # With some encoding in some dicom fields (like name) raises a
         # UnicodeEncodeError because they have non-ascii characters. To avoid
         # that we encode in utf-8.
-        
+
 	if sys.platform == 'win32':
-            self.mapper.SetInput(value.encode("utf-8"))  
+            self.mapper.SetInput(value.encode("utf-8"))
         else:
 	    try:
                 self.mapper.SetInput(value.encode("latin-1"))
             except(UnicodeEncodeError):
                 self.mapper.SetInput(value.encode("utf-8"))
+    def SetCoilDistanceValue(self, value):
+        if isinstance(value, int) or isinstance(value, float):
+            value = 'Dist: ' + str("{:06.2f}".format(value)) + ' mm'
+            if sys.platform == 'win32':
+                value += ""  # Otherwise 0 is not shown under win32
+                # With some encoding in some dicom fields (like name) raises a
+                # UnicodeEncodeError because they have non-ascii characters. To avoid
+                # that we encode in utf-8.
+
+        if sys.platform == 'win32':
+            self.mapper.SetInput(value.encode("utf-8"))
+        else:
+            try:
+                self.mapper.SetInput(value.encode("latin-1"))
+            except(UnicodeEncodeError):
+                self.mapper.SetInput(value.encode("utf-8"))
+
     def SetPosition(self, position):
         self.actor.GetPositionCoordinate().SetValue(position[0],
                                                     position[1])
@@ -306,7 +306,7 @@ import wx.lib.colourselect as csel
 import invesalius.constants as const
-[BUTTON_RAYCASTING, BUTTON_VIEW, BUTTON_SLICE_PLANE, BUTTON_3D_STEREO] = [wx.NewId() for num in xrange(4)]
+[BUTTON_RAYCASTING, BUTTON_VIEW, BUTTON_SLICE_PLANE, BUTTON_3D_STEREO, BUTTON_TARGET] = [wx.NewId() for num in xrange(5)]
 RAYCASTING_TOOLS = wx.NewId()
 ID_TO_NAME = {}
@@ -346,6 +346,9 @@ class VolumeToolPanel(wx.Panel):
         BMP_3D_STEREO = wx.Bitmap(os.path.join(const.ICON_DIR, "3D_glasses.png"),
                                     wx.BITMAP_TYPE_PNG)
+        BMP_TARGET = wx.Bitmap(os.path.join(const.ICON_DIR, "target.png"),
+                                    wx.BITMAP_TYPE_PNG)
+
         button_raycasting = pbtn.PlateButton(self, BUTTON_RAYCASTING,"",
                 BMP_RAYCASTING, style=pbtn.PB_STYLE_SQUARE,
@@ -359,6 +362,11 @@ class VolumeToolPanel(wx.Panel):
         BMP_SLICE_PLANE, style=pbtn.PB_STYLE_SQUARE,
         size=(32,32))
+        button_target = self.button_target = pbtn.PlateButton(self, BUTTON_TARGET,"",
+        BMP_TARGET, style=pbtn.PB_STYLE_SQUARE|pbtn.PB_STYLE_TOGGLE,
+        size=(32,32))
+        self.button_target.Enable(0)
+
         self.button_raycasting = button_raycasting
         self.button_stereo = button_stereo
@@ -389,7 +397,11 @@ class VolumeToolPanel(wx.Panel):
         sizer.Add(button_view, 0, wx.TOP|wx.BOTTOM, 1)
         sizer.Add(button_slice_plane, 0, wx.TOP|wx.BOTTOM, 1)
         sizer.Add(button_stereo, 0, wx.TOP|wx.BOTTOM, 1)
+        sizer.Add(button_target, 0, wx.TOP | wx.BOTTOM, 1)
+        self.navigation_status = False
+        self.status_target_select = False
+        self.status_obj_tracker = False
         sizer.Fit(self)
@@ -408,6 +420,8 @@ class VolumeToolPanel(wx.Panel):
         Publisher.subscribe(self.DisablePreset, 'Close project data')
         Publisher.subscribe(self.Uncheck, 'Uncheck image plane menu')
         Publisher.subscribe(self.DisableVolumeCutMenu, 'Disable volume cut menu')
+        Publisher.subscribe(self.StatusTargetSelect, 'Disable or enable coil tracker')
+        Publisher.subscribe(self.StatusObjTracker, 'Status target button')
     def DisablePreset(self, pubsub_evt):
         self.off_item.Check(1)
@@ -419,6 +433,7 @@ class VolumeToolPanel(wx.Panel):
         self.button_view.Bind(wx.EVT_LEFT_DOWN, self.OnButtonView)
         self.button_colour.Bind(csel.EVT_COLOURSELECT, self.OnSelectColour)
         self.button_stereo.Bind(wx.EVT_LEFT_DOWN, self.OnButtonStereo)
+        self.button_target.Bind(wx.EVT_LEFT_DOWN, self.OnButtonTarget)
     def OnButtonRaycasting(self, evt):
         # MENU RELATED TO RAYCASTING TYPES
@@ -433,6 +448,29 @@ class VolumeToolPanel(wx.Panel):
     def OnButtonSlicePlane(self, evt):
         self.button_slice_plane.PopupMenu(self.slice_plane_menu)
+    def StatusObjTracker(self, pubsub_evt):
+        self.status_obj_tracker = pubsub_evt.data
+        self.StatusNavigation()
+
+    def StatusTargetSelect(self, pubsub_evt):
+        self.status_target_select = pubsub_evt.data
+        self.StatusNavigation()
+
+    def StatusNavigation(self):
+        if self.status_target_select and self.status_obj_tracker:
+            self.button_target.Enable(1)
+        else:
+            self.OnButtonTarget(False)
+            self.button_target.Enable(0)
+
+    def OnButtonTarget(self, evt):
+        if not self.button_target.IsPressed() and evt is not False:
+            self.button_target._pressed = True
+            Publisher.sendMessage('Target navigation mode', self.button_target._pressed)
+        elif self.button_target.IsPressed() or evt is False:
+            self.button_target._pressed = False
+            Publisher.sendMessage('Target navigation mode', self.button_target._pressed)
+
     def OnSavePreset(self, evt):
         d = wx.TextEntryDialog(self, _("Preset name"))
         if d.ShowModal() == wx.ID_OK:
@@ -18,10 +18,20 @@
 #    PARTICULAR. Consulte a Licenca Publica Geral GNU para obter mais
 #    detalhes.
 #--------------------------------------------------------------------------
+
 import os
 import random
 import sys
+if sys.platform == 'win32':
+    try:
+        import win32api
+        _has_win32api = True
+    except ImportError:
+        _has_win32api = False
+else:
+    _has_win32api = False
+
 import vtk
 import wx
 import wx.combo
@@ -33,6 +43,7 @@ from wx.lib.wordwrap import wordwrap
 from wx.lib.pubsub import pub as Publisher
 import invesalius.constants as const
+import invesalius.data.coordinates as dco
 import invesalius.gui.widgets.gradient as grad
 import invesalius.session as ses
 import invesalius.utils as utils
@@ -470,6 +481,36 @@ def ShowSaveMarkersDialog(default_filename=None):
     os.chdir(current_dir)
     return filename
+def ShowSaveCoordsDialog(default_filename=None):
+    current_dir = os.path.abspath(".")
+    dlg = wx.FileDialog(None,
+                        _("Save coords as..."),  # title
+                        "",  # last used directory
+                        default_filename,
+                        _("Coordinates files (*.csv)|*.csv"),
+                        wx.SAVE | wx.OVERWRITE_PROMPT)
+    # dlg.SetFilterIndex(0) # default is VTI
+
+    filename = None
+    try:
+        if dlg.ShowModal() == wx.ID_OK:
+            filename = dlg.GetPath()
+            ok = 1
+        else:
+            ok = 0
+    except(wx._core.PyAssertionError):  # TODO: fix win64
+        filename = dlg.GetPath()
+        ok = 1
+
+    if (ok):
+        extension = "csv"
+        if sys.platform != 'win32':
+            if filename.split(".")[-1] != extension:
+                filename = filename + "." + extension
+
+    os.chdir(current_dir)
+    return filename
+
 def ShowLoadMarkersDialog():
     current_dir = os.path.abspath(".")
@@ -500,6 +541,66 @@ def ShowLoadMarkersDialog():
     return filepath
+def ShowSaveRegistrationDialog(default_filename=None):
+    current_dir = os.path.abspath(".")
+    dlg = wx.FileDialog(None,
+                        _("Save object registration as..."),  # title
+                        "",  # last used directory
+                        default_filename,
+                        _("Registration files (*.obr)|*.obr"),
+                        wx.SAVE | wx.OVERWRITE_PROMPT)
+    # dlg.SetFilterIndex(0) # default is VTI
+
+    filename = None
+    try:
+        if dlg.ShowModal() == wx.ID_OK:
+            filename = dlg.GetPath()
+            ok = 1
+        else:
+            ok = 0
+    except(wx._core.PyAssertionError):  # TODO: fix win64
+        filename = dlg.GetPath()
+        ok = 1
+
+    if (ok):
+        extension = "obr"
+        if sys.platform != 'win32':
+            if filename.split(".")[-1] != extension:
+                filename = filename + "." + extension
+
+    os.chdir(current_dir)
+    return filename
+
+
+def ShowLoadRegistrationDialog():
+    current_dir = os.path.abspath(".")
+
+    dlg = wx.FileDialog(None, message=_("Load object registration"),
+                        defaultDir="",
+                        defaultFile="",
+                        wildcard=_("Registration files (*.obr)|*.obr"),
+                        style=wx.OPEN|wx.CHANGE_DIR)
+
+    # inv3 filter is default
+    dlg.SetFilterIndex(0)
+
+    # Show the dialog and retrieve the user response. If it is the OK response,
+    # process the data.
+    filepath = None
+    try:
+        if dlg.ShowModal() == wx.ID_OK:
+            # This returns a Python list of files that were selected.
+            filepath = dlg.GetPath()
+    except(wx._core.PyAssertionError):  # FIX: win64
+        filepath = dlg.GetPath()
+
+    # Destroy the dialog. Don't do this until you are done with it!
+    # BAD things can happen otherwise!
+    dlg.Destroy()
+    os.chdir(current_dir)
+    return filepath
+
+
 class MessageDialog(wx.Dialog):
     def __init__(self, message):
         pre = wx.PreDialog()
@@ -729,7 +830,19 @@ def SurfaceSelectionRequiredForDuplication():
 # Dialogs for neuronavigation mode
 def InvalidFiducials():
-    msg = _("Fiducials are invalid. Select six coordinates.")
+    msg = _("Fiducials are invalid. Select all coordinates.")
+    if sys.platform == 'darwin':
+        dlg = wx.MessageDialog(None, "", msg,
+                               wx.ICON_INFORMATION | wx.OK)
+    else:
+        dlg = wx.MessageDialog(None, msg, "InVesalius 3 - Neuronavigator",
+                               wx.ICON_INFORMATION | wx.OK)
+    dlg.ShowModal()
+    dlg.Destroy()
+
+
+def InvalidObjectRegistration():
+    msg = _("Perform coil registration before navigation.")
     if sys.platform == 'darwin':
         dlg = wx.MessageDialog(None, "", msg,
                                wx.ICON_INFORMATION | wx.OK)
@@ -793,6 +906,7 @@ def NoMarkerSelected():
     dlg.ShowModal()
     dlg.Destroy()
+
 def DeleteAllMarkers():
     msg = _("Do you really want to delete all markers?")
     if sys.platform == 'darwin':
@@ -805,6 +919,38 @@ def DeleteAllMarkers():
     dlg.Destroy()
     return result
+def DeleteTarget():
+    msg = _("Target deleted")
+    if sys.platform == 'darwin':
+        dlg = wx.MessageDialog(None, "", msg,
+                                wx.ICON_INFORMATION | wx.OK)
+    else:
+        dlg = wx.MessageDialog(None, msg, "InVesalius 3 - Neuronavigator",
+                                wx.ICON_INFORMATION | wx.OK)
+    dlg.ShowModal()
+    dlg.Destroy()
+
+def NewTarget():
+    msg = _("New target selected")
+    if sys.platform == 'darwin':
+        dlg = wx.MessageDialog(None, "", msg,
+                                wx.ICON_INFORMATION | wx.OK)
+    else:
+        dlg = wx.MessageDialog(None, msg, "InVesalius 3 - Neuronavigator",
+                                wx.ICON_INFORMATION | wx.OK)
+    dlg.ShowModal()
+    dlg.Destroy()
+
+def InvalidTargetID():
+    msg = _("Sorry, you cannot use 'TARGET' ID")
+    if sys.platform == 'darwin':
+        dlg = wx.MessageDialog(None, "", msg,
+                                wx.ICON_INFORMATION | wx.OK)
+    else:
+        dlg = wx.MessageDialog(None, msg, "InVesalius 3 - Neuronavigator",
+                                wx.ICON_INFORMATION | wx.OK)
+    dlg.ShowModal()
+    dlg.Destroy()
 def EnterMarkerID(default):
     msg = _("Edit marker ID")
@@ -2967,3 +3113,246 @@ class FillHolesAutoDialog(wx.Dialog):
         else:
             self.panel3dcon.Enable(1)
             self.panel2dcon.Enable(0)
+
+
+class ObjectCalibrationDialog(wx.Dialog):
+
+    def __init__(self, nav_prop):
+
+        self.tracker_id = nav_prop[0]
+        self.trk_init = nav_prop[1]
+        self.obj_ref_id = 0
+        self.obj_name = None
+
+        self.obj_fiducials = np.full([5, 3], np.nan)
+        self.obj_orients = np.full([5, 3], np.nan)
+
+        pre = wx.PreDialog()
+        pre.Create(wx.GetApp().GetTopWindow(), -1, _(u"Object calibration"), size=(450, 440),
+                   style=wx.DEFAULT_DIALOG_STYLE | wx.FRAME_FLOAT_ON_PARENT)
+        self.PostCreate(pre)
+
+        self._init_gui()
+        self.LoadObject()
+
+    def _init_gui(self):
+        self.interactor = wxVTKRenderWindowInteractor(self, -1, size=self.GetSize())
+        self.interactor.Enable(1)
+        self.ren = vtk.vtkRenderer()
+        self.interactor.GetRenderWindow().AddRenderer(self.ren)
+
+        # Initialize list of buttons and txtctrls for wx objects
+        self.btns_coord = [None] * 5
+        self.text_actors = [None] * 5
+        self.ball_actors = [None] * 5
+        self.txt_coord = [list(), list(), list(), list(), list()]
+
+        # ComboBox for tracker reference mode
+        tooltip = wx.ToolTip(_(u"Choose the object reference mode"))
+        choice_ref = wx.ComboBox(self, -1, "", size=wx.Size(90, 23),
+                                 choices=const.REF_MODE, style=wx.CB_DROPDOWN | wx.CB_READONLY)
+        choice_ref.SetSelection(self.obj_ref_id)
+        choice_ref.SetToolTip(tooltip)
+        choice_ref.Bind(wx.EVT_COMBOBOX, self.OnChoiceRefMode)
+        choice_ref.Enable(0)
+
+        # Buttons to finish or cancel object registration
+        tooltip = wx.ToolTip(_(u"Registration done"))
+        # btn_ok = wx.Button(self, -1, _(u"Done"), size=wx.Size(90, 30))
+        btn_ok = wx.Button(self, wx.ID_OK, _(u"Done"), size=wx.Size(90, 30))
+        btn_ok.SetToolTip(tooltip)
+
+        extra_sizer = wx.FlexGridSizer(rows=2, cols=1, hgap=5, vgap=30)
+        extra_sizer.AddMany([choice_ref,
+                             btn_ok])
+
+        # Push buttons for object fiducials
+        btns_obj = const.BTNS_OBJ
+        tips_obj = const.TIPS_OBJ
+
+        for k in btns_obj:
+            n = btns_obj[k].keys()[0]
+            lab = btns_obj[k].values()[0]
+            self.btns_coord[n] = wx.Button(self, k, label=lab, size=wx.Size(60, 23))
+            self.btns_coord[n].SetToolTip(wx.ToolTip(tips_obj[n]))
+            self.btns_coord[n].Bind(wx.EVT_BUTTON, self.OnGetObjectFiducials)
+
+        for m in range(0, 5):
+            for n in range(0, 3):
+                self.txt_coord[m].append(wx.StaticText(self, -1, label='-',
+                                                       style=wx.ALIGN_RIGHT, size=wx.Size(40, 23)))
+
+        coord_sizer = wx.GridBagSizer(hgap=20, vgap=5)
+
+        for m in range(0, 5):
+            coord_sizer.Add(self.btns_coord[m], pos=wx.GBPosition(m, 0))
+            for n in range(0, 3):
+                coord_sizer.Add(self.txt_coord[m][n], pos=wx.GBPosition(m, n + 1), flag=wx.TOP, border=5)
+
+        group_sizer = wx.FlexGridSizer(rows=1, cols=2, hgap=50, vgap=5)
+        group_sizer.AddMany([(coord_sizer, 0, wx.LEFT, 20),
+                             (extra_sizer, 0, wx.LEFT, 10)])
+
+        main_sizer = wx.BoxSizer(wx.VERTICAL)
+        main_sizer.Add(self.interactor, 0, wx.EXPAND)
+        main_sizer.Add(group_sizer, 0,
+                       wx.EXPAND|wx.GROW|wx.LEFT|wx.TOP|wx.RIGHT|wx.BOTTOM|wx.ALIGN_CENTER_HORIZONTAL, 10)
+
+        self.SetSizer(main_sizer)
+        main_sizer.Fit(self)
+
+    def ObjectImportDialog(self):
+        msg = _("Would like to use InVesalius default object?")
+        if sys.platform == 'darwin':
+            dlg = wx.MessageDialog(None, "", msg,
+                                   wx.ICON_QUESTION | wx.YES_NO)
+        else:
+            dlg = wx.MessageDialog(None, msg,
+                                   "InVesalius 3",
+                                   wx.ICON_QUESTION | wx.YES_NO)
+        answer = dlg.ShowModal()
+        dlg.Destroy()
+
+        if answer == wx.ID_YES:
+            return 1
+        else:  # answer == wx.ID_NO:
+            return 0
+
+    def LoadObject(self):
+        default = self.ObjectImportDialog()
+        if not default:
+            filename = ShowImportMeshFilesDialog()
+
+            if filename:
+                if filename.lower().endswith('.stl'):
+                    reader = vtk.vtkSTLReader()
+                elif filename.lower().endswith('.ply'):
+                    reader = vtk.vtkPLYReader()
+                elif filename.lower().endswith('.obj'):
+                    reader = vtk.vtkOBJReader()
+                elif filename.lower().endswith('.vtp'):
+                    reader = vtk.vtkXMLPolyDataReader()
+                else:
+                    wx.MessageBox(_("File format not reconized by InVesalius"), _("Import surface error"))
+                    return
+            else:
+                filename = os.path.join(const.OBJ_DIR, "magstim_fig8_coil.stl")
+                reader = vtk.vtkSTLReader()
+        else:
+            filename = os.path.join(const.OBJ_DIR, "magstim_fig8_coil.stl")
+            reader = vtk.vtkSTLReader()
+
+        if _has_win32api:
+            self.obj_name = win32api.GetShortPathName(filename).encode(const.FS_ENCODE)
+        else:
+            self.obj_name = filename.encode(const.FS_ENCODE)
+
+        reader.SetFileName(self.obj_name)
+        reader.Update()
+        polydata = reader.GetOutput()
+
+        if polydata.GetNumberOfPoints() == 0:
+            wx.MessageBox(_("InVesalius was not able to import this surface"), _("Import surface error"))
+
+        transform = vtk.vtkTransform()
+        transform.RotateZ(90)
+
+        transform_filt = vtk.vtkTransformPolyDataFilter()
+        transform_filt.SetTransform(transform)
+        transform_filt.SetInputData(polydata)
+        transform_filt.Update()
+
+        normals = vtk.vtkPolyDataNormals()
+        normals.SetInputData(transform_filt.GetOutput())
+        normals.SetFeatureAngle(80)
+        normals.AutoOrientNormalsOn()
+        normals.Update()
+
+        mapper = vtk.vtkPolyDataMapper()
+        mapper.SetInputData(normals.GetOutput())
+        mapper.ScalarVisibilityOff()
+        mapper.ImmediateModeRenderingOn()
+
+        obj_actor = vtk.vtkActor()
+        obj_actor.SetMapper(mapper)
+
+        self.ball_actors[0], self.text_actors[0] = self.OnCreateObjectText('Left', (0,55,0))
+        self.ball_actors[1], self.text_actors[1] = self.OnCreateObjectText('Right', (0,-55,0))
+        self.ball_actors[2], self.text_actors[2] = self.OnCreateObjectText('Anterior', (23,0,0))
+
+        self.ren.AddActor(obj_actor)
+        self.ren.ResetCamera()
+
+        self.interactor.Render()
+
+    def OnCreateObjectText(self, name, coord):
+        ball_source = vtk.vtkSphereSource()
+        ball_source.SetRadius(3)
+        mapper = vtk.vtkPolyDataMapper()
+        mapper.SetInputConnection(ball_source.GetOutputPort())
+        ball_actor = vtk.vtkActor()
+        ball_actor.SetMapper(mapper)
+        ball_actor.SetPosition(coord)
+        ball_actor.GetProperty().SetColor(1, 0, 0)
+
+        textSource = vtk.vtkVectorText()
+        textSource.SetText(name)
+
+        mapper = vtk.vtkPolyDataMapper()
+        mapper.SetInputConnection(textSource.GetOutputPort())
+        tactor = vtk.vtkFollower()
+        tactor.SetMapper(mapper)
+        tactor.GetProperty().SetColor(1.0, 0.0, 0.0)
+        tactor.SetScale(5)
+        ball_position = ball_actor.GetPosition()
+        tactor.SetPosition(ball_position[0]+5, ball_position[1]+5, ball_position[2]+10)
+        self.ren.AddActor(tactor)
+        tactor.SetCamera(self.ren.GetActiveCamera())
+        self.ren.AddActor(ball_actor)
+        return ball_actor, tactor
+
+    def OnGetObjectFiducials(self, evt):
+        btn_id = const.BTNS_OBJ[evt.GetId()].keys()[0]
+
+        if self.trk_init and self.tracker_id:
+            coord_raw = dco.GetCoordinates(self.trk_init, self.tracker_id, self.obj_ref_id)
+            if self.obj_ref_id and btn_id == 4:
+                coord = coord_raw[2, :]
+            else:
+                coord = coord_raw[0, :]
+        else:
+            NavigationTrackerWarning(0, 'choose')
+
+        # Update text controls with tracker coordinates
+        if coord is not None or np.sum(coord) != 0.0:
+            self.obj_fiducials[btn_id, :] = coord[:3]
+            self.obj_orients[btn_id, :] = coord[3:]
+            for n in [0, 1, 2]:
+                self.txt_coord[btn_id][n].SetLabel(str(round(coord[n], 1)))
+                if self.text_actors[btn_id]:
+                    self.text_actors[btn_id].GetProperty().SetColor(0.0, 1.0, 0.0)
+                    self.ball_actors[btn_id].GetProperty().SetColor(0.0, 1.0, 0.0)
+            self.Refresh()
+        else:
+            NavigationTrackerWarning(0, 'choose')
+
+    def OnChoiceRefMode(self, evt):
+        # When ref mode is changed the tracker coordinates are set to nan
+        # This is for Polhemus FASTRAK wrapper, where the sensor attached to the object can be the stylus (Static
+        # reference - Selection 0 - index 0 for coordinates) or can be a 3rd sensor (Dynamic reference - Selection 1 -
+        # index 2 for coordinates)
+        # I use the index 2 directly here to send to the coregistration module where it is possible to access without
+        # any conditional statement the correct index of coordinates.
+
+        if evt.GetSelection():
+            self.obj_ref_id = 2
+        else:
+            self.obj_ref_id = 0
+        for m in range(0, 5):
+            self.obj_fiducials[m, :] = np.full([1, 3], np.nan)
+            self.obj_orients[m, :] = np.full([1, 3], np.nan)
+            for n in range(0, 3):
+                self.txt_coord[m][n].SetLabel('-')
+
+    def GetValue(self):
+        return self.obj_fiducials, self.obj_orients, self.obj_ref_id, self.obj_name
@@ -707,7 +707,7 @@ class MenuBar(wx.MenuBar):
         sub(self.OnEnableState, "Enable state project")
         sub(self.OnEnableUndo, "Enable undo")
         sub(self.OnEnableRedo, "Enable redo")
-        sub(self.OnEnableNavigation, "Navigation Status")
+        sub(self.OnEnableNavigation, "Navigation status")
         sub(self.OnAddMask, "Add mask")
         sub(self.OnRemoveMasks, "Remove masks")
@@ -19,6 +19,7 @@
 from functools import partial
 import sys
+import os
 import numpy as np
 import wx
@@ -27,15 +28,25 @@ import wx.lib.masked.numctrl
 import wx.lib.foldpanelbar as fpb
 from wx.lib.pubsub import pub as Publisher
 import wx.lib.colourselect as csel
+import wx.lib.platebtn as pbtn
+from math import cos, sin, pi
+from time import sleep
+
+import invesalius.data.transformations as tr
 import invesalius.constants as const
 import invesalius.data.bases as db
 import invesalius.data.coordinates as dco
 import invesalius.data.coregistration as dcr
 import invesalius.data.trackers as dt
 import invesalius.data.trigger as trig
+import invesalius.data.record_coords as rec
 import invesalius.gui.dialogs as dlg
+BTN_NEW = wx.NewId()
+BTN_IMPORT_LOCAL = wx.NewId()
+
+
 class TaskPanel(wx.Panel):
     def __init__(self, parent):
         wx.Panel.__init__(self, parent)
@@ -71,7 +82,6 @@ class InnerTaskPanel(wx.Panel):
         fold_panel = FoldPanel(self)
         fold_panel.SetBackgroundColour(default_colour)
-
         # Add line sizer into main sizer
         main_sizer = wx.BoxSizer(wx.VERTICAL)
         main_sizer.Add(txt_sizer, 0, wx.GROW|wx.EXPAND|wx.LEFT|wx.RIGHT, 5)
@@ -120,7 +130,7 @@ class InnerFoldPanel(wx.Panel):
                                           (10, 350), 0, fpb.FPB_SINGLE_FOLD)
         else:
             fold_panel = fpb.FoldPanelBar(self, -1, wx.DefaultPosition,
-                                          (10, 293), 0, fpb.FPB_SINGLE_FOLD)
+                                          (10, 320), 0, fpb.FPB_SINGLE_FOLD)
         # Fold panel style
         style = fpb.CaptionBarStyle()
         style.SetCaptionStyle(fpb.CAPTIONBAR_GRADIENT_V)
@@ -132,11 +142,19 @@ class InnerFoldPanel(wx.Panel):
         ntw = NeuronavigationPanel(item)
         fold_panel.ApplyCaptionStyle(item, style)
-        fold_panel.AddFoldPanelWindow(item, ntw, spacing= 0,
+        fold_panel.AddFoldPanelWindow(item, ntw, spacing=0,
                                       leftSpacing=0, rightSpacing=0)
         fold_panel.Expand(fold_panel.GetFoldPanel(0))
-        # Fold 2 - Markers panel
+        # Fold 2 - Object registration panel
+        item = fold_panel.AddFoldPanel(_("Object registration"), collapsed=True)
+        otw = ObjectRegistrationPanel(item)
+
+        fold_panel.ApplyCaptionStyle(item, style)
+        fold_panel.AddFoldPanelWindow(item, otw, spacing=0,
+                                      leftSpacing=0, rightSpacing=0)
+
+        # Fold 3 - Markers panel
         item = fold_panel.AddFoldPanel(_("Extra tools"), collapsed=True)
         mtw = MarkersPanel(item)
@@ -147,26 +165,38 @@ class InnerFoldPanel(wx.Panel):
         # Check box for camera update in volume rendering during navigation
         tooltip = wx.ToolTip(_("Update camera in volume"))
-        checkcamera = wx.CheckBox(self, -1, _('Volume camera'))
+        checkcamera = wx.CheckBox(self, -1, _('Vol. camera'))
         checkcamera.SetToolTip(tooltip)
-        checkcamera.SetValue(True)
-        checkcamera.Bind(wx.EVT_CHECKBOX, partial(self.UpdateVolumeCamera, ctrl=checkcamera))
+        checkcamera.SetValue(const.CAM_MODE)
+        checkcamera.Bind(wx.EVT_CHECKBOX, self.OnVolumeCamera)
+        self.checkcamera = checkcamera
-        # Check box for camera update in volume rendering during navigation
+        # Check box for trigger monitoring to create markers from serial port
         tooltip = wx.ToolTip(_("Enable external trigger for creating markers"))
-        checktrigger = wx.CheckBox(self, -1, _('External trigger'))
+        checktrigger = wx.CheckBox(self, -1, _('Ext. trigger'))
         checktrigger.SetToolTip(tooltip)
         checktrigger.SetValue(False)
-        checktrigger.Bind(wx.EVT_CHECKBOX, partial(self.UpdateExternalTrigger, ctrl=checktrigger))
+        checktrigger.Bind(wx.EVT_CHECKBOX, partial(self.OnExternalTrigger, ctrl=checktrigger))
         self.checktrigger = checktrigger
+        # Check box for object position and orientation update in volume rendering during navigation
+        tooltip = wx.ToolTip(_("Show and track TMS coil"))
+        checkobj = wx.CheckBox(self, -1, _('Show coil'))
+        checkobj.SetToolTip(tooltip)
+        checkobj.SetValue(False)
+        checkobj.Disable()
+        checkobj.Bind(wx.EVT_CHECKBOX, self.OnShowObject)
+        self.checkobj = checkobj
+
         if sys.platform != 'win32':
-            checkcamera.SetWindowVariant(wx.WINDOW_VARIANT_SMALL)
+            self.checkcamera.SetWindowVariant(wx.WINDOW_VARIANT_SMALL)
             checktrigger.SetWindowVariant(wx.WINDOW_VARIANT_SMALL)
+            checkobj.SetWindowVariant(wx.WINDOW_VARIANT_SMALL)
         line_sizer = wx.BoxSizer(wx.HORIZONTAL)
         line_sizer.Add(checkcamera, 0, wx.ALIGN_LEFT | wx.RIGHT | wx.LEFT, 5)
-        line_sizer.Add(checktrigger, 1,wx.ALIGN_RIGHT | wx.RIGHT | wx.LEFT, 5)
+        line_sizer.Add(checktrigger, 0, wx.ALIGN_CENTER)
+        line_sizer.Add(checkobj, 0, wx.ALIGN_RIGHT | wx.RIGHT | wx.LEFT, 5)
         line_sizer.Fit(self)
         # Panel sizer to expand fold panel
@@ -175,27 +205,49 @@ class InnerFoldPanel(wx.Panel):
         sizer.Add(line_sizer, 1, wx.GROW | wx.EXPAND)
         sizer.Fit(self)
+        self.track_obj = False
+
         self.SetSizer(sizer)
         self.Update()
         self.SetAutoLayout(1)
     def __bind_events(self):
-        Publisher.subscribe(self.OnTrigger, 'Navigation Status')
+        Publisher.subscribe(self.OnCheckStatus, 'Navigation status')
+        Publisher.subscribe(self.OnShowObject, 'Update track object state')
+        Publisher.subscribe(self.OnVolumeCamera, 'Target navigation mode')
-    def OnTrigger(self, pubsub_evt):
+    def OnCheckStatus(self, pubsub_evt):
         status = pubsub_evt.data
         if status:
             self.checktrigger.Enable(False)
+            self.checkobj.Enable(False)
         else:
             self.checktrigger.Enable(True)
+            if self.track_obj:
+                self.checkobj.Enable(True)
-    def UpdateExternalTrigger(self, evt, ctrl):
+    def OnExternalTrigger(self, evt, ctrl):
         Publisher.sendMessage('Update trigger state', ctrl.GetValue())
-    def UpdateVolumeCamera(self, evt, ctrl):
-        Publisher.sendMessage('Update volume camera state', ctrl.GetValue())
+    def OnShowObject(self, evt):
+        if hasattr(evt, 'data'):
+            if evt.data[0]:
+                self.checkobj.Enable(True)
+                self.track_obj = True
+                Publisher.sendMessage('Status target button', True)
+            else:
+                self.checkobj.Enable(False)
+                self.checkobj.SetValue(False)
+                self.track_obj = False
+                Publisher.sendMessage('Status target button', False)
+        Publisher.sendMessage('Update show object state', self.checkobj.GetValue())
+    def OnVolumeCamera(self, evt):
+        if hasattr(evt, 'data'):
+            if evt.data is True:
+                self.checkcamera.SetValue(0)
+        Publisher.sendMessage('Update volume camera state', self.checkcamera.GetValue())
 class NeuronavigationPanel(wx.Panel):
@@ -215,6 +267,9 @@ class NeuronavigationPanel(wx.Panel):
         self.trk_init = None
         self.trigger = None
         self.trigger_state = False
+        self.obj_reg = None
+        self.obj_reg_status = False
+        self.track_obj = False
         self.tracker_id = const.DEFAULT_TRACKER
         self.ref_mode_id = const.DEFAULT_REF_MODE
@@ -230,6 +285,7 @@ class NeuronavigationPanel(wx.Panel):
         choice_trck.SetToolTip(tooltip)
         choice_trck.SetSelection(const.DEFAULT_TRACKER)
         choice_trck.Bind(wx.EVT_COMBOBOX, partial(self.OnChoiceTracker, ctrl=choice_trck))
+        self.choice_trck = choice_trck
         # ComboBox for tracker reference mode
         tooltip = wx.ToolTip(_("Choose the navigation reference mode"))
@@ -259,7 +315,7 @@ class NeuronavigationPanel(wx.Panel):
             lab = btns_trk[k].values()[0]
             self.btns_coord[n] = wx.Button(self, k, label=lab, size=wx.Size(45, 23))
             self.btns_coord[n].SetToolTip(wx.ToolTip(tips_trk[n-3]))
-            # Excepetion for event of button that set image coordinates
+            # Exception for event of button that set image coordinates
             if n == 6:
                 self.btns_coord[n].Bind(wx.EVT_BUTTON, self.OnSetImageCoordinates)
             else:
@@ -275,12 +331,13 @@ class NeuronavigationPanel(wx.Panel):
         txtctrl_fre.SetBackgroundColour('WHITE')
         txtctrl_fre.SetEditable(0)
         txtctrl_fre.SetToolTip(tooltip)
+        self.txtctrl_fre = txtctrl_fre
         # Toggle button for neuronavigation
         tooltip = wx.ToolTip(_("Start navigation"))
         btn_nav = wx.ToggleButton(self, -1, _("Navigate"), size=wx.Size(80, -1))
         btn_nav.SetToolTip(tooltip)
-        btn_nav.Bind(wx.EVT_TOGGLEBUTTON, partial(self.OnNavigate, btn=(btn_nav, choice_trck, choice_ref, txtctrl_fre)))
+        btn_nav.Bind(wx.EVT_TOGGLEBUTTON, partial(self.OnNavigate, btn=(btn_nav, choice_trck, choice_ref)))
         # Image and tracker coordinates number controls
         for m in range(0, 7):
@@ -288,7 +345,7 @@ class NeuronavigationPanel(wx.Panel):
                 self.numctrls_coord[m].append(
                     wx.lib.masked.numctrl.NumCtrl(parent=self, integerWidth=4, fractionWidth=1))
-        # Sizers to group all GUI objects
+        # Sizer to group all GUI objects
         choice_sizer = wx.FlexGridSizer(rows=1, cols=2, hgap=5, vgap=5)
         choice_sizer.AddMany([(choice_trck, wx.LEFT),
                               (choice_ref, wx.RIGHT)])
@@ -326,8 +383,11 @@ class NeuronavigationPanel(wx.Panel):
     def __bind_events(self):
         Publisher.subscribe(self.LoadImageFiducials, 'Load image fiducials')
         Publisher.subscribe(self.UpdateTriggerState, 'Update trigger state')
+        Publisher.subscribe(self.UpdateTrackObjectState, 'Update track object state')
         Publisher.subscribe(self.UpdateImageCoordinates, 'Set ball reference position')
         Publisher.subscribe(self.OnDisconnectTracker, 'Disconnect tracker')
+        Publisher.subscribe(self.UpdateObjectRegistration, 'Update object registration')
+        Publisher.subscribe(self.OnCloseProject, 'Close project data')
     def LoadImageFiducials(self, pubsub_evt):
         marker_id = pubsub_evt.data[0]
@@ -347,26 +407,40 @@ class NeuronavigationPanel(wx.Panel):
         for m in [0, 1, 2, 6]:
             if m == 6 and self.btns_coord[m].IsEnabled():
                 for n in [0, 1, 2]:
-                    self.numctrls_coord[m][n].SetValue(self.current_coord[n])
+                    self.numctrls_coord[m][n].SetValue(float(self.current_coord[n]))
             elif m != 6 and not self.btns_coord[m].GetValue():
                 # btn_state = self.btns_coord[m].GetValue()
                 # if not btn_state:
                 for n in [0, 1, 2]:
-                    self.numctrls_coord[m][n].SetValue(self.current_coord[n])
+                    self.numctrls_coord[m][n].SetValue(float(self.current_coord[n]))
+
+    def UpdateObjectRegistration(self, pubsub_evt):
+        if pubsub_evt.data:
+            self.obj_reg = pubsub_evt.data
+            self.obj_reg_status = True
+        else:
+            self.obj_reg = None
+            self.obj_reg_status = False
-    def UpdateTriggerState (self, pubsub_evt):
+    def UpdateTrackObjectState(self, pubsub_evt):
+        if pubsub_evt.data[0]:
+            self.track_obj = pubsub_evt.data[0]
+        else:
+            self.track_obj = False
+
+    def UpdateTriggerState(self, pubsub_evt):
         self.trigger_state = pubsub_evt.data
     def OnDisconnectTracker(self, pubsub_evt):
         if self.tracker_id:
-            dt.TrackerConnection(self.tracker_id, 'disconnect')
+            dt.TrackerConnection(self.tracker_id, self.trk_init[0], 'disconnect')
     def OnChoiceTracker(self, evt, ctrl):
         Publisher.sendMessage('Update status text in GUI', _("Configuring tracker ..."))
-        if evt:
+        if hasattr(evt, 'GetSelection'):
             choice = evt.GetSelection()
         else:
-            choice = self.tracker_id
+            choice = 6
         if self.trk_init:
             trck = self.trk_init[0]
@@ -377,26 +451,35 @@ class NeuronavigationPanel(wx.Panel):
         # has been initialized before
         if trck and choice != 6:
             self.ResetTrackerFiducials()
-            self.trk_init = dt.TrackerConnection(self.tracker_id, 'disconnect')
+            Publisher.sendMessage('Update status text in GUI', _("Disconnecting tracker..."))
+            Publisher.sendMessage('Remove sensors ID')
+            self.trk_init = dt.TrackerConnection(self.tracker_id, trck, 'disconnect')
             self.tracker_id = choice
-            if not self.trk_init[0]:
-                self.trk_init = dt.TrackerConnection(self.tracker_id, 'connect')
+            if not self.trk_init[0] and choice:
+                Publisher.sendMessage('Update status text in GUI', _("Tracker disconnected successfully"))
+                self.trk_init = dt.TrackerConnection(self.tracker_id, None, 'connect')
                 if not self.trk_init[0]:
                     dlg.NavigationTrackerWarning(self.tracker_id, self.trk_init[1])
                     ctrl.SetSelection(0)
                     print "Tracker not connected!"
                 else:
+                    Publisher.sendMessage('Update status text in GUI', _("Ready"))
                     ctrl.SetSelection(self.tracker_id)
                     print "Tracker connected!"
         elif choice == 6:
             if trck:
-                self.trk_init = dt.TrackerConnection(self.tracker_id, 'disconnect')
+                Publisher.sendMessage('Update status text in GUI', _("Disconnecting tracker ..."))
+                Publisher.sendMessage('Remove sensors ID')
+                self.trk_init = dt.TrackerConnection(self.tracker_id, trck, 'disconnect')
                 if not self.trk_init[0]:
-                    dlg.NavigationTrackerWarning(self.tracker_id, 'disconnect')
+                    if evt is not False:
+                        dlg.NavigationTrackerWarning(self.tracker_id, 'disconnect')
                     self.tracker_id = 0
                     ctrl.SetSelection(self.tracker_id)
+                    Publisher.sendMessage('Update status text in GUI', _("Tracker disconnected"))
                     print "Tracker disconnected!"
                 else:
+                    Publisher.sendMessage('Update status text in GUI', _("Tracker still connected"))
                     print "Tracker still connected!"
             else:
                 ctrl.SetSelection(self.tracker_id)
@@ -405,34 +488,38 @@ class NeuronavigationPanel(wx.Panel):
             # If trk_init is None try to connect. If doesn't succeed show dialog.
             if choice:
                 self.tracker_id = choice
-                self.trk_init = dt.TrackerConnection(self.tracker_id, 'connect')
+                self.trk_init = dt.TrackerConnection(self.tracker_id, None, 'connect')
                 if not self.trk_init[0]:
                     dlg.NavigationTrackerWarning(self.tracker_id, self.trk_init[1])
                     self.tracker_id = 0
                     ctrl.SetSelection(self.tracker_id)
-        Publisher.sendMessage('Update status text in GUI', _("Ready"))
+                else:
+                    Publisher.sendMessage('Update status text in GUI', _("Ready"))
+
+        Publisher.sendMessage('Update tracker initializer', (self.tracker_id, self.trk_init, self.ref_mode_id))
     def OnChoiceRefMode(self, evt, ctrl):
-        # When ref mode is changed the tracker coords are set to zero
+        # When ref mode is changed the tracker coordinates are set to zero
         self.ref_mode_id = evt.GetSelection()
         self.ResetTrackerFiducials()
         # Some trackers do not accept restarting within this time window
         # TODO: Improve the restarting of trackers after changing reference mode
         # self.OnChoiceTracker(None, ctrl)
+        Publisher.sendMessage('Update tracker initializer', (self.tracker_id, self.trk_init, self.ref_mode_id))
         print "Reference mode changed!"
     def OnSetImageCoordinates(self, evt):
         # FIXME: Cross does not update in last clicked slice, only on the other two
         btn_id = const.BTNS_TRK[evt.GetId()].keys()[0]
-        wx, wy, wz = self.numctrls_coord[btn_id][0].GetValue(), \
-                     self.numctrls_coord[btn_id][1].GetValue(), \
+        ux, uy, uz = self.numctrls_coord[btn_id][0].GetValue(),\
+                     self.numctrls_coord[btn_id][1].GetValue(),\
                      self.numctrls_coord[btn_id][2].GetValue()
-        Publisher.sendMessage('Set ball reference position', (wx, wy, wz))
-        Publisher.sendMessage('Set camera in volume', (wx, wy, wz))
-        Publisher.sendMessage('Co-registered points', (wx, wy, wz))
-        Publisher.sendMessage('Update cross position', (wx, wy, wz))
+        Publisher.sendMessage('Set ball reference position', (ux, uy, uz))
+        # Publisher.sendMessage('Set camera in volume', (ux, uy, uz))
+        Publisher.sendMessage('Co-registered points', (ux, uy, uz, 0., 0., 0.))
+        Publisher.sendMessage('Update cross position', (ux, uy, uz))
     def OnImageFiducials(self, evt):
         btn_id = const.BTNS_IMG_MKS[evt.GetId()].keys()[0]
@@ -441,13 +528,13 @@ class NeuronavigationPanel(wx.Panel):
         if self.btns_coord[btn_id].GetValue():
             coord = self.numctrls_coord[btn_id][0].GetValue(),\
                     self.numctrls_coord[btn_id][1].GetValue(),\
-                    self.numctrls_coord[btn_id][2].GetValue()
+                    self.numctrls_coord[btn_id][2].GetValue(), 0, 0, 0
             self.fiducials[btn_id, :] = coord[0:3]
             Publisher.sendMessage('Create marker', (coord, marker_id))
         else:
             for n in [0, 1, 2]:
-                self.numctrls_coord[btn_id][n].SetValue(self.current_coord[n])
+                self.numctrls_coord[btn_id][n].SetValue(float(self.current_coord[n]))
             self.fiducials[btn_id, :] = np.nan
             Publisher.sendMessage('Delete fiducial marker', marker_id)
@@ -457,7 +544,13 @@ class NeuronavigationPanel(wx.Panel):
         coord = None
         if self.trk_init and self.tracker_id:
-            coord = dco.GetCoordinates(self.trk_init, self.tracker_id, self.ref_mode_id)
+            coord_raw = dco.GetCoordinates(self.trk_init, self.tracker_id, self.ref_mode_id)
+            if self.ref_mode_id:
+                coord = dco.dynamic_reference_m(coord_raw[0, :], coord_raw[1, :])
+            else:
+                coord = coord_raw[0, :]
+                coord[2] = -coord[2]
+
         else:
             dlg.NavigationTrackerWarning(0, 'choose')
@@ -471,7 +564,6 @@ class NeuronavigationPanel(wx.Panel):
         btn_nav = btn[0]
         choice_trck = btn[1]
         choice_ref = btn[2]
-        txtctrl_fre = btn[3]
         nav_id = btn_nav.GetValue()
         if nav_id:
@@ -479,6 +571,9 @@ class NeuronavigationPanel(wx.Panel):
                 dlg.InvalidFiducials()
                 btn_nav.SetValue(False)
+            elif not self.trk_init[0]:
+                dlg.NavigationTrackerWarning(0, 'choose')
+
             else:
                 tooltip = wx.ToolTip(_("Stop neuronavigation"))
                 btn_nav.SetToolTip(tooltip)
@@ -489,28 +584,75 @@ class NeuronavigationPanel(wx.Panel):
                 for btn_c in self.btns_coord:
                     btn_c.Enable(False)
-                m, q1, minv = db.base_creation(self.fiducials[0:3, :])
-                n, q2, ninv = db.base_creation(self.fiducials[3::, :])
+                # fids_head_img = np.zeros([3, 3])
+                # for ic in range(0, 3):
+                #     fids_head_img[ic, :] = np.asarray(db.flip_x_m(self.fiducials[ic, :]))
+                #
+                # m_head_aux, q_head_aux, m_inv_head_aux = db.base_creation(fids_head_img)
+                # m_head = np.asmatrix(np.identity(4))
+                # m_head[:3, :3] = m_head_aux[:3, :3]
+
+                m, q1, minv = db.base_creation_old(self.fiducials[:3, :])
+                n, q2, ninv = db.base_creation_old(self.fiducials[3:, :])
+
+                m_change = tr.affine_matrix_from_points(self.fiducials[3:, :].T, self.fiducials[:3, :].T,
+                                                        shear=False, scale=False)
+
+                # coreg_data = [m_change, m_head]
                 tracker_mode = self.trk_init, self.tracker_id, self.ref_mode_id
                 # FIXME: FRE is taking long to calculate so it updates on GUI delayed to navigation - I think its fixed
                 # TODO: Exhibit FRE in a warning dialog and only starts navigation after user clicks ok
                 fre = db.calculate_fre(self.fiducials, minv, n, q1, q2)
-                txtctrl_fre.SetValue(str(round(fre, 2)))
+                self.txtctrl_fre.SetValue(str(round(fre, 2)))
                 if fre <= 3:
-                    txtctrl_fre.SetBackgroundColour('GREEN')
+                    self.txtctrl_fre.SetBackgroundColour('GREEN')
                 else:
-                    txtctrl_fre.SetBackgroundColour('RED')
+                    self.txtctrl_fre.SetBackgroundColour('RED')
                 if self.trigger_state:
                     self.trigger = trig.Trigger(nav_id)
-                Publisher.sendMessage("Navigation Status", True)
+                Publisher.sendMessage("Navigation status", True)
                 Publisher.sendMessage("Toggle Cross", const.SLICE_STATE_CROSS)
                 Publisher.sendMessage("Hide current mask")
-                self.correg = dcr.Coregistration((minv, n, q1, q2), nav_id, tracker_mode)
+                if self.track_obj:
+                    if self.obj_reg_status:
+                        # obj_reg[0] is object 3x3 fiducial matrix and obj_reg[1] is 3x3 orientation matrix
+                        obj_fiducials, obj_orients, obj_ref_mode, obj_name = self.obj_reg
+
+                        if self.trk_init and self.tracker_id:
+
+                            coreg_data = [m_change, obj_ref_mode]
+
+                            if self.ref_mode_id:
+                                coord_raw = dco.GetCoordinates(self.trk_init, self.tracker_id, self.ref_mode_id)
+                                obj_data = db.object_registration(obj_fiducials, obj_orients, coord_raw, m_change)
+                                coreg_data.extend(obj_data)
+
+                                self.correg = dcr.CoregistrationObjectDynamic(coreg_data, nav_id, tracker_mode)
+                            else:
+                                coord_raw = np.array([None])
+                                obj_data = db.object_registration(obj_fiducials, obj_orients, coord_raw, m_change)
+                                coreg_data.extend(obj_data)
+
+                                self.correg = dcr.CoregistrationObjectStatic(coreg_data, nav_id, tracker_mode)
+
+                        else:
+                            dlg.NavigationTrackerWarning(0, 'choose')
+
+                    else:
+                        dlg.InvalidObjectRegistration()
+
+                else:
+                    coreg_data = [m_change, 0]
+                    if self.ref_mode_id:
+                        # self.correg = dcr.CoregistrationDynamic_old(bases_coreg, nav_id, tracker_mode)
+                        self.correg = dcr.CoregistrationDynamic(coreg_data, nav_id, tracker_mode)
+                    else:
+                        self.correg = dcr.CoregistrationStatic(coreg_data, nav_id, tracker_mode)
         else:
             tooltip = wx.ToolTip(_("Start neuronavigation"))
@@ -527,13 +669,272 @@ class NeuronavigationPanel(wx.Panel):
             self.correg.stop()
-            Publisher.sendMessage("Navigation Status", False)
+            Publisher.sendMessage("Navigation status", False)
+
+    def ResetImageFiducials(self):
+        for m in range(0, 3):
+            self.btns_coord[m].SetValue(False)
+            self.fiducials[m, :] = [np.nan, np.nan, np.nan]
+            for n in range(0, 3):
+                self.numctrls_coord[m][n].SetValue(0.0)
+
+        for n in range(0, 3):
+            self.numctrls_coord[6][n].SetValue(0.0)
     def ResetTrackerFiducials(self):
         for m in range(3, 6):
+            self.fiducials[m, :] = [np.nan, np.nan, np.nan]
             for n in range(0, 3):
                 self.numctrls_coord[m][n].SetValue(0.0)
+        self.txtctrl_fre.SetValue('')
+        self.txtctrl_fre.SetBackgroundColour('WHITE')
+
+    def OnCloseProject(self, pubsub_evt):
+        self.ResetTrackerFiducials()
+        self.ResetImageFiducials()
+        self.OnChoiceTracker(False, self.choice_trck)
+        Publisher.sendMessage('Update object registration', False)
+        Publisher.sendMessage('Update track object state', (False, False))
+        Publisher.sendMessage('Delete all markers', 'close')
+        # TODO: Reset camera initial focus
+        Publisher.sendMessage('Reset cam clipping range')
+
+
+class ObjectRegistrationPanel(wx.Panel):
+    def __init__(self, parent):
+        wx.Panel.__init__(self, parent)
+        default_colour = wx.SystemSettings_GetColour(wx.SYS_COLOUR_MENUBAR)
+        self.SetBackgroundColour(default_colour)
+
+        self.coil_list = const.COIL
+
+        self.nav_prop = None
+        self.obj_fiducials = None
+        self.obj_orients = None
+        self.obj_ref_mode = None
+        self.obj_name = None
+        self.timestamp = const.TIMESTAMP
+
+        self.SetAutoLayout(1)
+        self.__bind_events()
+
+        # Button for creating new coil
+        tooltip = wx.ToolTip(_("Create new coil"))
+        btn_new = wx.Button(self, -1, _("New"), size=wx.Size(65, 23))
+        btn_new.SetToolTip(tooltip)
+        btn_new.Enable(1)
+        btn_new.Bind(wx.EVT_BUTTON, self.OnLinkCreate)
+        self.btn_new = btn_new
+
+        # Button for import config coil file
+        tooltip = wx.ToolTip(_("Load coil configuration file"))
+        btn_load = wx.Button(self, -1, _("Load"), size=wx.Size(65, 23))
+        btn_load.SetToolTip(tooltip)
+        btn_load.Enable(1)
+        btn_load.Bind(wx.EVT_BUTTON, self.OnLinkLoad)
+        self.btn_load = btn_load
+
+        # Save button for object registration
+        tooltip = wx.ToolTip(_(u"Save object registration file"))
+        btn_save = wx.Button(self, -1, _(u"Save"), size=wx.Size(65, 23))
+        btn_save.SetToolTip(tooltip)
+        btn_save.Enable(1)
+        btn_save.Bind(wx.EVT_BUTTON, self.ShowSaveObjectDialog)
+        self.btn_save = btn_save
+
+        # Create a horizontal sizer to represent button save
+        line_save = wx.BoxSizer(wx.HORIZONTAL)
+        line_save.Add(btn_new, 1, wx.LEFT | wx.TOP | wx.RIGHT, 4)
+        line_save.Add(btn_load, 1, wx.LEFT | wx.TOP | wx.RIGHT, 4)
+        line_save.Add(btn_save, 1, wx.LEFT | wx.TOP | wx.RIGHT, 4)
+
+        # Change angles threshold
+        text_angles = wx.StaticText(self, -1, _("Angle threshold [degrees]:"))
+        spin_size_angles = wx.SpinCtrl(self, -1, "", size=wx.Size(50, 23))
+        spin_size_angles.SetRange(1, 99)
+        spin_size_angles.SetValue(const.COIL_ANGLES_THRESHOLD)
+        spin_size_angles.Bind(wx.EVT_TEXT, partial(self.OnSelectAngleThreshold, ctrl=spin_size_angles))
+        spin_size_angles.Bind(wx.EVT_SPINCTRL, partial(self.OnSelectAngleThreshold, ctrl=spin_size_angles))
+
+        # Change dist threshold
+        text_dist = wx.StaticText(self, -1, _("Distance threshold [mm]:"))
+        spin_size_dist = wx.SpinCtrl(self, -1, "", size=wx.Size(50, 23))
+        spin_size_dist.SetRange(1, 99)
+        spin_size_dist.SetValue(const.COIL_ANGLES_THRESHOLD)
+        spin_size_dist.Bind(wx.EVT_TEXT, partial(self.OnSelectDistThreshold, ctrl=spin_size_dist))
+        spin_size_dist.Bind(wx.EVT_SPINCTRL, partial(self.OnSelectDistThreshold, ctrl=spin_size_dist))
+
+        # Change timestamp interval
+        text_timestamp = wx.StaticText(self, -1, _("Timestamp interval [s]:"))
+        spin_timestamp_dist = wx.SpinCtrlDouble(self, -1, "", size=wx.Size(50, 23), inc = 0.1)
+        spin_timestamp_dist.SetRange(0.5, 60.0)
+        spin_timestamp_dist.SetValue(self.timestamp)
+        spin_timestamp_dist.Bind(wx.EVT_TEXT, partial(self.OnSelectTimestamp, ctrl=spin_timestamp_dist))
+        spin_timestamp_dist.Bind(wx.EVT_SPINCTRL, partial(self.OnSelectTimestamp, ctrl=spin_timestamp_dist))
+        self.spin_timestamp_dist = spin_timestamp_dist
+
+        # Create a horizontal sizer to threshold configs
+        line_angle_threshold = wx.BoxSizer(wx.HORIZONTAL)
+        line_angle_threshold.AddMany([(text_angles, 1, wx.EXPAND | wx.GROW | wx.TOP| wx.RIGHT | wx.LEFT, 5),
+                                      (spin_size_angles, 0, wx.ALL | wx.EXPAND | wx.GROW, 5)])
+
+        line_dist_threshold = wx.BoxSizer(wx.HORIZONTAL)
+        line_dist_threshold.AddMany([(text_dist, 1, wx.EXPAND | wx.GROW | wx.TOP| wx.RIGHT | wx.LEFT, 5),
+                                      (spin_size_dist, 0, wx.ALL | wx.EXPAND | wx.GROW, 5)])
+
+        line_timestamp = wx.BoxSizer(wx.HORIZONTAL)
+        line_timestamp.AddMany([(text_timestamp, 1, wx.EXPAND | wx.GROW | wx.TOP| wx.RIGHT | wx.LEFT, 5),
+                                      (spin_timestamp_dist, 0, wx.ALL | wx.EXPAND | wx.GROW, 5)])
+
+        # Check box for trigger monitoring to create markers from serial port
+        checkrecordcoords = wx.CheckBox(self, -1, _('Record coordinates'))
+        checkrecordcoords.SetValue(False)
+        checkrecordcoords.Enable(0)
+        checkrecordcoords.Bind(wx.EVT_CHECKBOX, partial(self.OnRecordCoords, ctrl=checkrecordcoords))
+        self.checkrecordcoords = checkrecordcoords
+
+        # Check box to track object or simply the stylus
+        checktrack = wx.CheckBox(self, -1, _('Track object'))
+        checktrack.SetValue(False)
+        checktrack.Enable(0)
+        checktrack.Bind(wx.EVT_CHECKBOX, partial(self.OnTrackObject, ctrl=checktrack))
+        self.checktrack = checktrack
+
+        line_checks = wx.BoxSizer(wx.HORIZONTAL)
+        line_checks.Add(checkrecordcoords, 0, wx.ALIGN_LEFT | wx.RIGHT | wx.LEFT, 5)
+        line_checks.Add(checktrack, 0, wx.ALIGN_RIGHT | wx.RIGHT | wx.LEFT, 5)
+
+        # Add line sizers into main sizer
+        main_sizer = wx.BoxSizer(wx.VERTICAL)
+        main_sizer.Add(line_save, 0, wx.LEFT | wx.RIGHT | wx.TOP | wx.ALIGN_CENTER_HORIZONTAL, 5)
+        main_sizer.Add(line_angle_threshold, 0, wx.GROW | wx.EXPAND | wx.LEFT | wx.RIGHT | wx.TOP, 5)
+        main_sizer.Add(line_dist_threshold, 0, wx.GROW | wx.EXPAND | wx.LEFT | wx.RIGHT | wx.TOP, 5)
+        main_sizer.Add(line_timestamp, 0, wx.GROW | wx.EXPAND | wx.LEFT | wx.RIGHT | wx.TOP, 5)
+        main_sizer.Add(line_checks, 0, wx.GROW | wx.EXPAND | wx.LEFT | wx.RIGHT | wx.TOP | wx.BOTTOM, 10)
+        main_sizer.Fit(self)
+
+        self.SetSizer(main_sizer)
+        self.Update()
+
+    def __bind_events(self):
+        Publisher.subscribe(self.UpdateTrackerInit, 'Update tracker initializer')
+        Publisher.subscribe(self.UpdateNavigationStatus, 'Navigation status')
+        Publisher.subscribe(self.OnCloseProject, 'Close project data')
+
+    def UpdateTrackerInit(self, pubsub_evt):
+        self.nav_prop = pubsub_evt.data
+
+    def UpdateNavigationStatus(self, pubsub_evt):
+        nav_status = pubsub_evt.data
+        if nav_status:
+            self.checkrecordcoords.Enable(1)
+            self.checktrack.Enable(0)
+            self.btn_save.Enable(0)
+            self.btn_new.Enable(0)
+            self.btn_load.Enable(0)
+        else:
+            self.OnRecordCoords(nav_status, self.checkrecordcoords)
+            self.checkrecordcoords.SetValue(False)
+            self.checkrecordcoords.Enable(0)
+            self.btn_save.Enable(1)
+            self.btn_new.Enable(1)
+            self.btn_load.Enable(1)
+            if self.obj_fiducials is not None:
+                self.checktrack.Enable(1)
+                Publisher.sendMessage('Enable target button', True)
+
+    def OnSelectAngleThreshold(self, evt, ctrl):
+        Publisher.sendMessage('Update angle threshold', ctrl.GetValue())
+
+    def OnSelectDistThreshold(self, evt, ctrl):
+        Publisher.sendMessage('Update dist threshold', ctrl.GetValue())
+
+    def OnSelectTimestamp(self, evt, ctrl):
+        self.timestamp = ctrl.GetValue()
+
+    def OnRecordCoords(self, evt, ctrl):
+        if ctrl.GetValue() and evt:
+            self.spin_timestamp_dist.Enable(0)
+            self.thr_record = rec.Record(ctrl.GetValue(), self.timestamp)
+        elif (not ctrl.GetValue() and evt) or (ctrl.GetValue() and not evt) :
+            self.spin_timestamp_dist.Enable(1)
+            self.thr_record.stop()
+        elif not ctrl.GetValue() and not evt:
+            None
+
+    def OnTrackObject(self, evt, ctrl):
+        Publisher.sendMessage('Update track object state', (evt.GetSelection(), self.obj_name))
+
+    def OnComboCoil(self, evt):
+        # coil_name = evt.GetString()
+        coil_index = evt.GetSelection()
+        Publisher.sendMessage('Change selected coil', self.coil_list[coil_index][1])
+
+    def OnLinkCreate(self, event=None):
+
+        if self.nav_prop:
+            dialog = dlg.ObjectCalibrationDialog(self.nav_prop)
+            try:
+                if dialog.ShowModal() == wx.ID_OK:
+                    self.obj_fiducials, self.obj_orients, self.obj_ref_mode, self.obj_name = dialog.GetValue()
+                    if np.isfinite(self.obj_fiducials).all() and np.isfinite(self.obj_orients).all():
+                        self.checktrack.Enable(1)
+                        Publisher.sendMessage('Update object registration',
+                                              (self.obj_fiducials, self.obj_orients, self.obj_ref_mode, self.obj_name))
+                        Publisher.sendMessage('Update status text in GUI', _("Ready"))
+
+            except wx._core.PyAssertionError:  # TODO FIX: win64
+                pass
+
+        else:
+            dlg.NavigationTrackerWarning(0, 'choose')
+
+    def OnLinkLoad(self, event=None):
+        filename = dlg.ShowLoadRegistrationDialog()
+
+        if filename:
+            data = np.loadtxt(filename, delimiter='\t')
+            self.obj_fiducials = data[:, :3]
+            self.obj_orients = data[:, 3:]
+
+            text_file = open(filename, "r")
+            header = text_file.readline().split('\t')
+            text_file.close()
+
+            self.obj_name = header[1]
+            self.obj_ref_mode = int(header[-1])
+
+            self.checktrack.Enable(1)
+            Publisher.sendMessage('Update object registration',
+                                  (self.obj_fiducials, self.obj_orients, self.obj_ref_mode, self.obj_name))
+            Publisher.sendMessage('Update status text in GUI', _("Ready"))
+            wx.MessageBox(_("Object file successfully loaded"), _("Load"))
+
+    def ShowSaveObjectDialog(self, evt):
+        if np.isnan(self.obj_fiducials).any() or np.isnan(self.obj_orients).any():
+            wx.MessageBox(_("Digitize all object fiducials before saving"), _("Save error"))
+        else:
+            filename = dlg.ShowSaveRegistrationDialog("object_registration.obr")
+            if filename:
+                hdr = 'Object' + "\t" + self.obj_name + "\t" + 'Reference' + "\t" + str('%d' % self.obj_ref_mode)
+                data = np.hstack([self.obj_fiducials, self.obj_orients])
+                np.savetxt(filename, data, fmt='%.4f', delimiter='\t', newline='\n', header=hdr)
+                wx.MessageBox(_("Object file successfully saved"), _("Save"))
+
+    def OnCloseProject(self, pubsub_evt):
+        self.checkrecordcoords.SetValue(False)
+        self.checkrecordcoords.Enable(0)
+        self.checktrack.SetValue(False)
+        self.checktrack.Enable(0)
+
+        self.nav_prop = None
+        self.obj_fiducials = None
+        self.obj_orients = None
+        self.obj_ref_mode = None
+        self.obj_name = None
+        self.timestamp = const.TIMESTAMP
+
 class MarkersPanel(wx.Panel):
     def __init__(self, parent):
@@ -545,9 +946,12 @@ class MarkersPanel(wx.Panel):
         self.__bind_events()
-        self.current_coord = 0, 0, 0
+        self.current_coord = 0, 0, 0, 0, 0, 0
+        self.current_angle = 0, 0, 0
         self.list_coord = []
         self.marker_ind = 0
+        self.tgt_flag = self.tgt_index = None
+        self.nav_status = False
         self.marker_colour = (0.0, 0.0, 1.)
         self.marker_size = 4
@@ -608,8 +1012,8 @@ class MarkersPanel(wx.Panel):
         self.lc.SetColumnWidth(1, 50)
         self.lc.SetColumnWidth(2, 50)
         self.lc.SetColumnWidth(3, 50)
-        self.lc.SetColumnWidth(4, 50)
-        self.lc.Bind(wx.EVT_LIST_ITEM_RIGHT_CLICK, self.OnListEditMarkerId)
+        self.lc.SetColumnWidth(4, 60)
+        self.lc.Bind(wx.EVT_LIST_ITEM_RIGHT_CLICK, self.OnMouseRightDown)
         self.lc.Bind(wx.EVT_LIST_ITEM_ACTIVATED, self.OnItemBlink)
         self.lc.Bind(wx.EVT_LIST_ITEM_DESELECTED, self.OnStopItemBlink)
@@ -625,17 +1029,34 @@ class MarkersPanel(wx.Panel):
         self.Update()
     def __bind_events(self):
-        Publisher.subscribe(self.UpdateCurrentCoord, 'Set ball reference position')
+        Publisher.subscribe(self.UpdateCurrentCoord, 'Co-registered points')
         Publisher.subscribe(self.OnDeleteSingleMarker, 'Delete fiducial marker')
+        Publisher.subscribe(self.OnDeleteAllMarkers, 'Delete all markers')
         Publisher.subscribe(self.OnCreateMarker, 'Create marker')
+        Publisher.subscribe(self.UpdateNavigationStatus, 'Navigation status')
     def UpdateCurrentCoord(self, pubsub_evt):
-        self.current_coord = pubsub_evt.data
+        self.current_coord = pubsub_evt.data[1][:]
+        #self.current_angle = pubsub_evt.data[1][3:]
+
+    def UpdateNavigationStatus(self, pubsub_evt):
+        if pubsub_evt.data is False:
+            sleep(0.5)
+            #self.current_coord[3:] = 0, 0, 0
+            self.nav_status = False
+        else:
+            self.nav_status = True
+
+    def OnMouseRightDown(self, evt):
+        self.OnListEditMarkerId(self.nav_status)
-    def OnListEditMarkerId(self, evt):
+    def OnListEditMarkerId(self, status):
         menu_id = wx.Menu()
-        menu_id.Append(-1, _('Edit ID'))
-        menu_id.Bind(wx.EVT_MENU, self.OnMenuEditMarkerId)
+        edit_id = menu_id.Append(0, _('Edit ID'))
+        menu_id.Bind(wx.EVT_MENU, self.OnMenuEditMarkerId, edit_id)
+        target_menu = menu_id.Append(1, _('Set as target'))
+        menu_id.Bind(wx.EVT_MENU, self.OnMenuSetTarget, target_menu)
+        target_menu.Enable(status)
         self.PopupMenu(menu_id)
         menu_id.Destroy()
@@ -646,20 +1067,64 @@ class MarkersPanel(wx.Panel):
         Publisher.sendMessage('Stop Blink Marker')
     def OnMenuEditMarkerId(self, evt):
-        id_label = dlg.EnterMarkerID(self.lc.GetItemText(self.lc.GetFocusedItem(), 4))
         list_index = self.lc.GetFocusedItem()
+        if evt == 'TARGET':
+            id_label = evt
+        else:
+            id_label = dlg.EnterMarkerID(self.lc.GetItemText(list_index, 4))
+            if id_label == 'TARGET':
+                id_label = ''
+                dlg.InvalidTargetID()
         self.lc.SetStringItem(list_index, 4, id_label)
         # Add the new ID to exported list
-        self.list_coord[list_index][7] = str(id_label)
+        if len(self.list_coord[list_index]) > 8:
+            self.list_coord[list_index][10] = str(id_label)
+        else:
+            self.list_coord[list_index][7] = str(id_label)
+
+    def OnMenuSetTarget(self, evt):
+        if isinstance(evt, int):
+            self.lc.Focus(evt)
+
+        if self.tgt_flag:
+            self.lc.SetItemBackgroundColour(self.tgt_index, 'white')
+            Publisher.sendMessage('Set target transparency', [False, self.tgt_index])
+            self.lc.SetStringItem(self.tgt_index, 4, '')
+            # Add the new ID to exported list
+            if len(self.list_coord[self.tgt_index]) > 8:
+                self.list_coord[self.tgt_index][10] = str('')
+            else:
+                self.list_coord[self.tgt_index][7] = str('')
+
+        self.tgt_index = self.lc.GetFocusedItem()
+        self.lc.SetItemBackgroundColour(self.tgt_index, 'RED')
+
+        Publisher.sendMessage('Update target', self.list_coord[self.tgt_index])
+        Publisher.sendMessage('Set target transparency', [True, self.tgt_index])
+        Publisher.sendMessage('Disable or enable coil tracker', True)
+        self.OnMenuEditMarkerId('TARGET')
+        self.tgt_flag = True
+        dlg.NewTarget()
+
+    def OnDeleteAllMarkers(self, evt):
+        if self.list_coord:
+            if hasattr(evt, 'data'):
+                result = wx.ID_OK
+            else:
+                result = dlg.DeleteAllMarkers()
+
+            if result == wx.ID_OK:
+                self.list_coord = []
+                self.marker_ind = 0
+                Publisher.sendMessage('Remove all markers', self.lc.GetItemCount())
+                self.lc.DeleteAllItems()
+                Publisher.sendMessage('Stop Blink Marker', 'DeleteAll')
-    def OnDeleteAllMarkers(self, pubsub_evt):
-        result = dlg.DeleteAllMarkers()
-        if result == wx.ID_OK:
-            self.list_coord = []
-            self.marker_ind = 0
-            Publisher.sendMessage('Remove all markers', self.lc.GetItemCount())
-            self.lc.DeleteAllItems()
-            Publisher.sendMessage('Stop Blink Marker', 'DeleteAll')
+                if self.tgt_flag:
+                    self.tgt_flag = self.tgt_index = None
+                    Publisher.sendMessage('Disable or enable coil tracker', False)
+                    if not hasattr(evt, 'data'):
+                        dlg.DeleteTarget()
     def OnDeleteSingleMarker(self, evt):
         # OnDeleteSingleMarker is used for both pubsub and button click events
@@ -682,6 +1147,10 @@ class MarkersPanel(wx.Panel):
                 index = None
         if index:
+            if self.tgt_flag and self.tgt_index == index[0]:
+                self.tgt_flag = self.tgt_index = None
+                Publisher.sendMessage('Disable or enable coil tracker', False)
+                dlg.DeleteTarget()
             self.DeleteMarker(index)
         else:
             dlg.NoMarkerSelected()
@@ -711,20 +1180,42 @@ class MarkersPanel(wx.Panel):
         if filepath:
             try:
+                count_line = self.lc.GetItemCount()
                 content = [s.rstrip() for s in open(filepath)]
                 for data in content:
+                    target = None
                     line = [s for s in data.split()]
-                    coord = float(line[0]), float(line[1]), float(line[2])
-                    colour = float(line[3]), float(line[4]), float(line[5])
-                    size = float(line[6])
+                    if len(line) > 8:
+                        coord = float(line[0]), float(line[1]), float(line[2]), float(line[3]), float(line[4]), float(line[5])
+                        colour = float(line[6]), float(line[7]), float(line[8])
+                        size = float(line[9])
+
+                        if len(line) == 11:
+                            for i in const.BTNS_IMG_MKS:
+                                if line[10] in const.BTNS_IMG_MKS[i].values()[0]:
+                                    Publisher.sendMessage('Load image fiducials', (line[10], coord))
+                                elif line[10] == 'TARGET':
+                                    target = count_line
+                        else:
+                            line.append("")
+
+                        self.CreateMarker(coord, colour, size, line[10])
+                        if target is not None:
+                            self.OnMenuSetTarget(target)
-                    if len(line) == 8:
-                        for i in const.BTNS_IMG_MKS:
-                            if line[7] in const.BTNS_IMG_MKS[i].values()[0]:
-                                Publisher.sendMessage('Load image fiducials', (line[7], coord))
                     else:
-                        line.append("")
-                    self.CreateMarker(coord, colour, size, line[7])
+                        coord = float(line[0]), float(line[1]), float(line[2]), 0, 0, 0
+                        colour = float(line[3]), float(line[4]), float(line[5])
+                        size = float(line[6])
+
+                        if len(line) == 8:
+                            for i in const.BTNS_IMG_MKS:
+                                if line[7] in const.BTNS_IMG_MKS[i].values()[0]:
+                                    Publisher.sendMessage('Load image fiducials', (line[7], coord))
+                        else:
+                            line.append("")
+                        self.CreateMarker(coord, colour, size, line[7])
+                    count_line += 1
             except:
                 dlg.InvalidMarkersFile()
@@ -744,14 +1235,16 @@ class MarkersPanel(wx.Panel):
                 text_file = open(filename, "w")
                 list_slice1 = self.list_coord[0]
                 coord = str('%.3f' %self.list_coord[0][0]) + "\t" + str('%.3f' %self.list_coord[0][1]) + "\t" + str('%.3f' %self.list_coord[0][2])
-                properties = str('%.3f' %list_slice1[3]) + "\t" + str('%.3f' %list_slice1[4]) + "\t" + str('%.3f' %list_slice1[5]) + "\t" + str('%.1f' %list_slice1[6]) + "\t" + list_slice1[7]
-                line = coord + "\t" + properties + "\n"
+                angles = str('%.3f' %self.list_coord[0][3]) + "\t" + str('%.3f' %self.list_coord[0][4]) + "\t" + str('%.3f' %self.list_coord[0][5])
+                properties = str('%.3f' %list_slice1[6]) + "\t" + str('%.3f' %list_slice1[7]) + "\t" + str('%.3f' %list_slice1[8]) + "\t" + str('%.1f' %list_slice1[9]) + "\t" + list_slice1[10]
+                line = coord + "\t" + angles + "\t" + properties + "\n"
                 list_slice = self.list_coord[1:]
                 for value in list_slice:
                     coord = str('%.3f' %value[0]) + "\t" + str('%.3f' %value[1]) + "\t" + str('%.3f' %value[2])
-                    properties = str('%.3f' %value[3]) + "\t" + str('%.3f' %value[4]) + "\t" + str('%.3f' %value[5]) + "\t" + str('%.1f' %value[6]) + "\t" + value[7]
-                    line = line + coord + "\t" + properties + "\n"
+                    angles = str('%.3f' % value[3]) + "\t" + str('%.3f' % value[4]) + "\t" + str('%.3f' % value[5])
+                    properties = str('%.3f' %value[6]) + "\t" + str('%.3f' %value[7]) + "\t" + str('%.3f' %value[8]) + "\t" + str('%.1f' %value[9]) + "\t" + value[10]
+                    line = line + coord + "\t" + angles + "\t" +properties + "\n"
                 text_file.writelines(line)
                 text_file.close()
@@ -766,12 +1259,13 @@ class MarkersPanel(wx.Panel):
         # TODO: Use matrix coordinates and not world coordinates as current method.
         # This makes easier for inter-software comprehension.
-        Publisher.sendMessage('Add marker', (self.marker_ind, size, colour,  coord))
+        Publisher.sendMessage('Add marker', (self.marker_ind, size, colour,  coord[0:3]))
         self.marker_ind += 1
         # List of lists with coordinates and properties of a marker
-        line = [coord[0], coord[1], coord[2], colour[0], colour[1], colour[2], self.marker_size, marker_id]
+
+        line = [coord[0], coord[1], coord[2], coord[3], coord[4], coord[5], colour[0], colour[1], colour[2], size, marker_id]
         # Adding current line to a list of all markers already created
         if not self.list_coord:
	@@ -56,6 +56,7 @@ STEREO_ANAGLYPH = _("Anaglyph")		@@ -56,6 +56,7 @@ STEREO_ANAGLYPH = _("Anaglyph")
56	TEXT_SIZE_SMALL = 11	56	TEXT_SIZE_SMALL = 11
57	TEXT_SIZE = 12	57	TEXT_SIZE = 12
58	TEXT_SIZE_LARGE = 16	58	TEXT_SIZE_LARGE = 16
		59	+TEXT_SIZE_EXTRA_LARGE = 20
59	TEXT_COLOUR = (1,1,1)	60	TEXT_COLOUR = (1,1,1)
60		61
61	(X,Y) = (0.03, 0.97)	62	(X,Y) = (0.03, 0.97)
	@@ -678,6 +679,10 @@ DYNAMIC_REF = 1		@@ -678,6 +679,10 @@ DYNAMIC_REF = 1
678	DEFAULT_REF_MODE = DYNAMIC_REF	679	DEFAULT_REF_MODE = DYNAMIC_REF
679	REF_MODE = [_("Static ref."), _("Dynamic ref.")]	680	REF_MODE = [_("Static ref."), _("Dynamic ref.")]
680		681
		682	+DEFAULT_COIL = SELECT
		683	+COIL = [_("Select coil:"), _("Neurosoft Figure-8"),
		684	+ _("Magstim 70 mm"), _("Nexstim")]
		685	+
681	IR1 = wx.NewId()	686	IR1 = wx.NewId()
682	IR2 = wx.NewId()	687	IR2 = wx.NewId()
683	IR3 = wx.NewId()	688	IR3 = wx.NewId()
	@@ -708,5 +713,34 @@ TIPS_TRK = [_("Select left ear with spatial tracker"),		@@ -708,5 +713,34 @@ TIPS_TRK = [_("Select left ear with spatial tracker"),
708	_("Select nasion with spatial tracker"),	713	_("Select nasion with spatial tracker"),
709	_("Show set coordinates in image")]	714	_("Show set coordinates in image")]
710		715
		716	+OBJL = wx.NewId()
		717	+OBJR = wx.NewId()
		718	+OBJA = wx.NewId()
		719	+OBJC = wx.NewId()
		720	+OBJF = wx.NewId()
		721	+
		722	+BTNS_OBJ = {OBJL: {0: _('Left')},
		723	+ OBJR: {1: _('Right')},
		724	+ OBJA: {2: _('Anterior')},
		725	+ OBJC: {3: _('Center')},
		726	+ OBJF: {4: _('Fixed')}}
		727	+
		728	+TIPS_OBJ = [_("Select left object fiducial"),
		729	+ _("Select right object fiducial"),
		730	+ _("Select anterior object fiducial"),
		731	+ _("Select object center"),
		732	+ _("Attach sensor to object")]
		733	+
711	CAL_DIR = os.path.abspath(os.path.join(FILE_PATH, '..', 'navigation', 'mtc_files', 'CalibrationFiles'))	734	CAL_DIR = os.path.abspath(os.path.join(FILE_PATH, '..', 'navigation', 'mtc_files', 'CalibrationFiles'))
712	MAR_DIR = os.path.abspath(os.path.join(FILE_PATH, '..', 'navigation', 'mtc_files', 'Markers'))	735	MAR_DIR = os.path.abspath(os.path.join(FILE_PATH, '..', 'navigation', 'mtc_files', 'Markers'))
		736	+
		737	+#OBJECT TRACKING
		738	+OBJ_DIR = os.path.abspath(os.path.join(FILE_PATH, '..', 'navigation', 'objects'))
		739	+ARROW_SCALE = 3
		740	+ARROW_UPPER_LIMIT = 30
		741	+#COIL_ANGLES_THRESHOLD = 3 * ARROW_SCALE
		742	+COIL_ANGLES_THRESHOLD = 3
		743	+COIL_COORD_THRESHOLD = 3
		744	+TIMESTAMP = 2.0
		745	+
		746	+CAM_MODE = True
1	-from math import sqrt	1	+from math import sqrt, pi
2	import numpy as np	2	import numpy as np
		3	+import invesalius.data.coordinates as dco
		4	+import invesalius.data.transformations as tr
3		5
4		6
5	def angle_calculation(ap_axis, coil_axis):	7	def angle_calculation(ap_axis, coil_axis):
	@@ -19,7 +21,7 @@ def angle_calculation(ap_axis, coil_axis):		@@ -19,7 +21,7 @@ def angle_calculation(ap_axis, coil_axis):
19	return float(angle)	21	return float(angle)
20		22
21		23
22	-def base_creation(fiducials):	24	+def base_creation_old(fiducials):
23	"""	25	"""
24	Calculate the origin and matrix for coordinate system	26	Calculate the origin and matrix for coordinate system
25	transformation.	27	transformation.
	@@ -55,31 +57,70 @@ def base_creation(fiducials):		@@ -55,31 +57,70 @@ def base_creation(fiducials):
55	[g2[0], g2[1], g2[2]],	57	[g2[0], g2[1], g2[2]],
56	[g3[0], g3[1], g3[2]]])	58	[g3[0], g3[1], g3[2]]])
57		59
58	- q.shape = (3, 1)
59	- q = np.matrix(q.copy())
60	m_inv = m.I	60	m_inv = m.I
61		61
62	- # print"M: ", m
63	- # print"q: ", q	62	+ return m, q, m_inv
		63	+
		64	+
		65	+def base_creation(fiducials):
		66	+ """
		67	+ Calculate the origin and matrix for coordinate system
		68	+ transformation.
		69	+ q: origin of coordinate system
		70	+ g1, g2, g3: orthogonal vectors of coordinate system
		71	+
		72	+ :param fiducials: array of 3 rows (p1, p2, p3) and 3 columns (x, y, z) with fiducials coordinates
		73	+ :return: matrix and origin for base transformation
		74	+ """
		75	+
		76	+ p1 = fiducials[0, :]
		77	+ p2 = fiducials[1, :]
		78	+ p3 = fiducials[2, :]
		79	+
		80	+ sub1 = p2 - p1
		81	+ sub2 = p3 - p1
		82	+ lamb = (sub1[0]sub2[0]+sub1[1]sub2[1]+sub1[2]*sub2[2])/np.dot(sub1, sub1)
		83	+
		84	+ q = p1 + lamb*sub1
		85	+ g1 = p3 - q
		86	+ g2 = p1 - q
		87	+
		88	+ if not g1.any():
		89	+ g1 = p2 - q
		90	+
		91	+ g3 = np.cross(g1, g2)
		92	+
		93	+ g1 = g1/sqrt(np.dot(g1, g1))
		94	+ g2 = g2/sqrt(np.dot(g2, g2))
		95	+ g3 = g3/sqrt(np.dot(g3, g3))
		96	+
		97	+ m = np.matrix([[g1[0], g2[0], g3[0]],
		98	+ [g1[1], g2[1], g3[1]],
		99	+ [g1[2], g2[2], g3[2]]])
		100	+
		101	+ m_inv = m.I
64		102
65	return m, q, m_inv	103	return m, q, m_inv
66		104
67		105
68	-def calculate_fre(fiducials, minv, n, q1, q2):	106	+def calculate_fre(fiducials, minv, n, q, o):
69	"""	107	"""
70	Calculate the Fiducial Registration Error for neuronavigation.	108	Calculate the Fiducial Registration Error for neuronavigation.
71		109
72	:param fiducials: array of 6 rows (image and tracker fiducials) and 3 columns (x, y, z) with coordinates	110	:param fiducials: array of 6 rows (image and tracker fiducials) and 3 columns (x, y, z) with coordinates
73	:param minv: inverse matrix given by base creation	111	:param minv: inverse matrix given by base creation
74	:param n: base change matrix given by base creation	112	:param n: base change matrix given by base creation
75	- :param q1: origin of first base
76	- :param q2: origin of second base	113	+ :param q: origin of first base
		114	+ :param o: origin of second base
77	:return: float number of fiducial registration error	115	:return: float number of fiducial registration error
78	"""	116	"""
79		117
80	img = np.zeros([3, 3])	118	img = np.zeros([3, 3])
81	dist = np.zeros([3, 1])	119	dist = np.zeros([3, 1])
82		120
		121	+ q1 = np.mat(q).reshape(3, 1)
		122	+ q2 = np.mat(o).reshape(3, 1)
		123	+
83	p1 = np.mat(fiducials[3, :]).reshape(3, 1)	124	p1 = np.mat(fiducials[3, :]).reshape(3, 1)
84	p2 = np.mat(fiducials[4, :]).reshape(3, 1)	125	p2 = np.mat(fiducials[4, :]).reshape(3, 1)
85	p3 = np.mat(fiducials[5, :]).reshape(3, 1)	126	p3 = np.mat(fiducials[5, :]).reshape(3, 1)
	@@ -133,3 +174,98 @@ def flip_x(point):		@@ -133,3 +174,98 @@ def flip_x(point):
133	x, y, z = point_rot.tolist()[0][:3]	174	x, y, z = point_rot.tolist()[0][:3]
134		175
135	return x, y, z	176	return x, y, z
		177	+
		178	+
		179	+def flip_x_m(point):
		180	+ """
		181	+ Rotate coordinates of a vector by pi around X axis in static reference frame.
		182	+
		183	+ InVesalius also require to multiply the z coordinate by (-1). Possibly
		184	+ because the origin of coordinate system of imagedata is
		185	+ located in superior left corner and the origin of VTK scene coordinate
		186	+ system (polygonal surface) is in the interior left corner. Second
		187	+ possibility is the order of slice stacking
		188	+
		189	+ :param point: list of coordinates x, y and z
		190	+ :return: rotated coordinates
		191	+ """
		192	+
		193	+ point_4 = np.hstack((point, 1.)).reshape([4, 1])
		194	+ point_4[2, 0] = -point_4[2, 0]
		195	+
		196	+ m_rot = np.asmatrix(tr.euler_matrix(pi, 0, 0))
		197	+
		198	+ point_rot = m_rot*point_4
		199	+
		200	+ return point_rot[0, 0], point_rot[1, 0], point_rot[2, 0]
		201	+
		202	+
		203	+def object_registration(fiducials, orients, coord_raw, m_change):
		204	+ """
		205	+
		206	+ :param fiducials: 3x3 array of fiducials translations
		207	+ :param orients: 3x3 array of fiducials orientations in degrees
		208	+ :param coord_raw: nx6 array of coordinates from tracking device where n = 1 is the reference attached to the head
		209	+ :param m_change: 3x3 array representing change of basis from head in tracking system to vtk head system
		210	+ :return:
		211	+ """
		212	+
		213	+ coords_aux = np.hstack((fiducials, orients))
		214	+ mask = np.ones(len(coords_aux), dtype=bool)
		215	+ mask[[3]] = False
		216	+ coords = coords_aux[mask]
		217	+
		218	+ fids_dyn = np.zeros([4, 6])
		219	+ fids_img = np.zeros([4, 6])
		220	+ fids_raw = np.zeros([3, 3])
		221	+
		222	+ # compute fiducials of object with reference to the fixed probe in source frame
		223	+ for ic in range(0, 3):
		224	+ fids_raw[ic, :] = dco.dynamic_reference_m2(coords[ic, :], coords[3, :])[:3]
		225	+
		226	+ # compute initial alignment of probe fixed in the object in source frame
		227	+ t_s0_raw = np.asmatrix(tr.translation_matrix(coords[3, :3]))
		228	+ r_s0_raw = np.asmatrix(tr.euler_matrix(np.radians(coords[3, 3]), np.radians(coords[3, 4]),
		229	+ np.radians(coords[3, 5]), 'rzyx'))
		230	+ s0_raw = np.asmatrix(tr.concatenate_matrices(t_s0_raw, r_s0_raw))
		231	+
		232	+ # compute change of basis for object fiducials in source frame
		233	+ base_obj_raw, q_obj_raw, base_inv_obj_raw = base_creation(fids_raw[:3, :3])
		234	+ r_obj_raw = np.asmatrix(np.identity(4))
		235	+ r_obj_raw[:3, :3] = base_obj_raw[:3, :3]
		236	+ t_obj_raw = np.asmatrix(tr.translation_matrix(q_obj_raw))
		237	+ m_obj_raw = np.asmatrix(tr.concatenate_matrices(t_obj_raw, r_obj_raw))
		238	+
		239	+ for ic in range(0, 4):
		240	+ if coord_raw.any():
		241	+ # compute object fiducials in reference frame
		242	+ fids_dyn[ic, :] = dco.dynamic_reference_m2(coords[ic, :], coord_raw[1, :])
		243	+ fids_dyn[ic, 2] = -fids_dyn[ic, 2]
		244	+ else:
		245	+ # compute object fiducials in source frame
		246	+ fids_dyn[ic, :] = coords[ic, :]
		247	+
		248	+ # compute object fiducials in vtk head frame
		249	+ a, b, g = np.radians(fids_dyn[ic, 3:])
		250	+ T_p = tr.translation_matrix(fids_dyn[ic, :3])
		251	+ R_p = tr.euler_matrix(a, b, g, 'rzyx')
		252	+ M_p = np.asmatrix(tr.concatenate_matrices(T_p, R_p))
		253	+ M_img = np.asmatrix(m_change) * M_p
		254	+
		255	+ angles_img = np.degrees(np.asarray(tr.euler_from_matrix(M_img, 'rzyx')))
		256	+ coord_img = np.asarray(flip_x_m(tr.translation_from_matrix(M_img)))
		257	+
		258	+ fids_img[ic, :] = np.hstack((coord_img, angles_img))
		259	+
		260	+ # compute object base change in vtk head frame
		261	+ base_obj_img, q_obj_img, base_inv_obj_img = base_creation(fids_img[:3, :3])
		262	+ r_obj_img = np.asmatrix(np.identity(4))
		263	+ r_obj_img[:3, :3] = base_obj_img[:3, :3]
		264	+
		265	+ # compute initial alignment of probe fixed in the object in reference (or static) frame
		266	+ s0_trans_dyn = np.asmatrix(tr.translation_matrix(fids_dyn[3, :3]))
		267	+ s0_rot_dyn = np.asmatrix(tr.euler_matrix(np.radians(fids_dyn[3, 3]), np.radians(fids_dyn[3, 4]),
		268	+ np.radians(fids_dyn[3, 5]), 'rzyx'))
		269	+ s0_dyn = np.asmatrix(tr.concatenate_matrices(s0_trans_dyn, s0_rot_dyn))
		270	+
		271	+ return t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img
	@@ -20,10 +20,13 @@		@@ -20,10 +20,13 @@
20	from math import sin, cos	20	from math import sin, cos
21	import numpy as np	21	import numpy as np
22		22
		23	+import invesalius.data.transformations as tr
		24	+
23	from time import sleep	25	from time import sleep
24	from random import uniform	26	from random import uniform
25	from wx.lib.pubsub import pub as Publisher	27	from wx.lib.pubsub import pub as Publisher
26		28
		29	+
27	def GetCoordinates(trck_init, trck_id, ref_mode):	30	def GetCoordinates(trck_init, trck_id, ref_mode):
28		31
29	"""	32	"""
	@@ -54,7 +57,7 @@ def ClaronCoord(trck_init, trck_id, ref_mode):		@@ -54,7 +57,7 @@ def ClaronCoord(trck_init, trck_id, ref_mode):
54	scale = np.array([1.0, 1.0, -1.0])	57	scale = np.array([1.0, 1.0, -1.0])
55	coord = None	58	coord = None
56	k = 0	59	k = 0
57	- # TODO: try to replace while and use some Claron internal computation	60	+ # TODO: try to replace 'while' and use some Claron internal computation
58		61
59	if ref_mode:	62	if ref_mode:
60	while k < 20:	63	while k < 20:
	@@ -77,6 +80,7 @@ def ClaronCoord(trck_init, trck_id, ref_mode):		@@ -77,6 +80,7 @@ def ClaronCoord(trck_init, trck_id, ref_mode):
77	trck.Run()	80	trck.Run()
78	coord = np.array([trck.PositionTooltipX1 * scale[0], trck.PositionTooltipY1 * scale[1],	81	coord = np.array([trck.PositionTooltipX1 * scale[0], trck.PositionTooltipY1 * scale[1],
79	trck.PositionTooltipZ1 * scale[2], trck.AngleX1, trck.AngleY1, trck.AngleZ1])	82	trck.PositionTooltipZ1 * scale[2], trck.AngleX1, trck.AngleY1, trck.AngleZ1])
		83	+
80	k = 30	84	k = 30
81	except AttributeError:	85	except AttributeError:
82	k += 1	86	k += 1
	@@ -104,27 +108,23 @@ def PolhemusCoord(trck, trck_id, ref_mode):		@@ -104,27 +108,23 @@ def PolhemusCoord(trck, trck_id, ref_mode):
104		108
105	def PolhemusWrapperCoord(trck, trck_id, ref_mode):	109	def PolhemusWrapperCoord(trck, trck_id, ref_mode):
106		110
107	- scale = 25.4 * np.array([1., 1.0, -1.0])
108	- coord = None	111	+ trck.Run()
		112	+ scale = 10.0 * np.array([1., 1., 1.])
109		113
110	- if ref_mode:
111	- trck.Run()
112	- probe = np.array([float(trck.PositionTooltipX1), float(trck.PositionTooltipY1),
113	- float(trck.PositionTooltipZ1), float(trck.AngleX1), float(trck.AngleY1),
114	- float(trck.AngleZ1)])
115	- reference = np.array([float(trck.PositionTooltipX2), float(trck.PositionTooltipY2),
116	- float(trck.PositionTooltipZ2), float(trck.AngleX2), float(trck.AngleY2),
117	- float(trck.AngleZ2)])	114	+ coord1 = np.array([float(trck.PositionTooltipX1)scale[0], float(trck.PositionTooltipY1)scale[1],
		115	+ float(trck.PositionTooltipZ1)*scale[2],
		116	+ float(trck.AngleX1), float(trck.AngleY1), float(trck.AngleZ1)])
118		117
119	- if probe.all() and reference.all():
120	- coord = dynamic_reference(probe, reference)
121	- coord = (coord[0] * scale[0], coord[1] * scale[1], coord[2] * scale[2], coord[3], coord[4], coord[5])	118	+ coord2 = np.array([float(trck.PositionTooltipX2)scale[0], float(trck.PositionTooltipY2)scale[1],
		119	+ float(trck.PositionTooltipZ2)*scale[2],
		120	+ float(trck.AngleX2), float(trck.AngleY2), float(trck.AngleZ2)])
		121	+ coord = np.vstack([coord1, coord2])
122		122
123	- else:
124	- trck.Run()
125	- coord = np.array([float(trck.PositionTooltipX1) * scale[0], float(trck.PositionTooltipY1) * scale[1],
126	- float(trck.PositionTooltipZ1) * scale[2], float(trck.AngleX1), float(trck.AngleY1),
127	- float(trck.AngleZ1)])	123	+ if trck_id == 2:
		124	+ coord3 = np.array([float(trck.PositionTooltipX3) * scale[0], float(trck.PositionTooltipY3) * scale[1],
		125	+ float(trck.PositionTooltipZ3) * scale[2],
		126	+ float(trck.AngleX3), float(trck.AngleY3), float(trck.AngleZ3)])
		127	+ coord = np.vstack([coord, coord3])
128		128
129	if trck.StylusButton:	129	if trck.StylusButton:
130	Publisher.sendMessage('PLH Stylus Button On')	130	Publisher.sendMessage('PLH Stylus Button On')
	@@ -213,22 +213,21 @@ def DebugCoord(trk_init, trck_id, ref_mode):		@@ -213,22 +213,21 @@ def DebugCoord(trk_init, trck_id, ref_mode):
213	:param trck_id: id of tracking device	213	:param trck_id: id of tracking device
214	:return: six coordinates x, y, z, alfa, beta and gama	214	:return: six coordinates x, y, z, alfa, beta and gama
215	"""	215	"""
216	- sleep(0.2)
217	- if ref_mode:
218	- probe = np.array([uniform(1, 200), uniform(1, 200), uniform(1, 200),
219	- uniform(1, 200), uniform(1, 200), uniform(1, 200)])
220	- reference = np.array([uniform(1, 200), uniform(1, 200), uniform(1, 200),
221	- uniform(1, 200), uniform(1, 200), uniform(1, 200)])
222		216
223	- coord = dynamic_reference(probe, reference)	217	+ sleep(0.05)
224		218
225	- else:
226	- coord = np.array([uniform(1, 200), uniform(1, 200), uniform(1, 200),
227	- uniform(1, 200), uniform(1, 200), uniform(1, 200)])	219	+ coord1 = np.array([uniform(1, 200), uniform(1, 200), uniform(1, 200),
		220	+ uniform(-180.0, 180.0), uniform(-180.0, 180.0), uniform(-180.0, 180.0)])
		221	+
		222	+ coord2 = np.array([uniform(1, 200), uniform(1, 200), uniform(1, 200),
		223	+ uniform(-180.0, 180.0), uniform(-180.0, 180.0), uniform(-180.0, 180.0)])
		224	+
		225	+ coord3 = np.array([uniform(1, 200), uniform(1, 200), uniform(1, 200),
		226	+ uniform(-180.0, 180.0), uniform(-180.0, 180.0), uniform(-180.0, 180.0)])
228		227
229	Publisher.sendMessage('Sensors ID', [int(uniform(0, 5)), int(uniform(0, 5))])	228	Publisher.sendMessage('Sensors ID', [int(uniform(0, 5)), int(uniform(0, 5))])
230		229
231	- return coord	230	+ return np.vstack([coord1, coord2, coord3])
232		231
233		232
234	def dynamic_reference(probe, reference):	233	def dynamic_reference(probe, reference):
	@@ -245,28 +244,143 @@ def dynamic_reference(probe, reference):		@@ -245,28 +244,143 @@ def dynamic_reference(probe, reference):
245	"""	244	"""
246	a, b, g = np.radians(reference[3:6])	245	a, b, g = np.radians(reference[3:6])
247		246
248	- vet = probe[0:3] - reference[0:3]
249	- vet = np.mat(vet.reshape(3, 1))	247	+ vet = np.asmatrix(probe[0:3] - reference[0:3])
		248	+ # vet = np.mat(vet.reshape(3, 1))
		249	+
		250	+ # Attitude matrix given by Patriot manual
		251	+ # a: rotation of plane (X, Y) around Z axis (azimuth)
		252	+ # b: rotation of plane (X', Z) around Y' axis (elevation)
		253	+ # a: rotation of plane (Y', Z') around X'' axis (roll)
		254	+ m_rot = np.mat([[cos(a) * cos(b), sin(b) * sin(g) * cos(a) - cos(g) * sin(a),
		255	+ cos(a) * sin(b) * cos(g) + sin(a) * sin(g)],
		256	+ [cos(b) * sin(a), sin(b) * sin(g) * sin(a) + cos(g) * cos(a),
		257	+ cos(g) * sin(b) * sin(a) - sin(g) * cos(a)],
		258	+ [-sin(b), sin(g) * cos(b), cos(b) * cos(g)]])
		259	+
		260	+ # coord_rot = m_rot.T * vet
		261	+ coord_rot = vet*m_rot
		262	+ coord_rot = np.squeeze(np.asarray(coord_rot))
		263	+
		264	+ return coord_rot[0], coord_rot[1], -coord_rot[2], probe[3], probe[4], probe[5]
250		265
251	- # Attitude Matrix given by Patriot Manual
252	- Mrot = np.mat([[cos(a) * cos(b), sin(b) * sin(g) * cos(a) - cos(g) * sin(a),
253	- cos(a) * sin(b) * cos(g) + sin(a) * sin(g)],
254	- [cos(b) * sin(a), sin(b) * sin(g) * sin(a) + cos(g) * cos(a),
255	- cos(g) * sin(b) * sin(a) - sin(g) * cos(a)],
256	- [-sin(b), sin(g) * cos(b), cos(b) * cos(g)]])
257		266
258	- coord_rot = Mrot.T * vet	267	+def dynamic_reference_m(probe, reference):
		268	+ """
		269	+ Apply dynamic reference correction to probe coordinates. Uses the alpha, beta and gama
		270	+ rotation angles of reference to rotate the probe coordinate and returns the x, y, z
		271	+ difference between probe and reference. Angles sequences and equation was extracted from
		272	+ Polhemus manual and Attitude matrix in Wikipedia.
		273	+ General equation is:
		274	+ coord = Mrot * (probe - reference)
		275	+ :param probe: sensor one defined as probe
		276	+ :param reference: sensor two defined as reference
		277	+ :return: rotated and translated coordinates
		278	+ """
		279	+
		280	+ a, b, g = np.radians(reference[3:6])
		281	+
		282	+ T = tr.translation_matrix(reference[:3])
		283	+ R = tr.euler_matrix(a, b, g, 'rzyx')
		284	+ M = np.asmatrix(tr.concatenate_matrices(T, R))
		285	+ # M = tr.compose_matrix(angles=np.radians(reference[3:6]), translate=reference[:3])
		286	+ # print M
		287	+ probe_4 = np.vstack((np.asmatrix(probe[:3]).reshape([3, 1]), 1.))
		288	+ coord_rot = M.I * probe_4
259	coord_rot = np.squeeze(np.asarray(coord_rot))	289	coord_rot = np.squeeze(np.asarray(coord_rot))
260		290
261	- return coord_rot[0], coord_rot[1], coord_rot[2], probe[3], probe[4], probe[5]	291	+ return coord_rot[0], coord_rot[1], -coord_rot[2], probe[3], probe[4], probe[5]
		292	+
		293	+def dynamic_reference_m2(probe, reference):
		294	+ """
		295	+ Apply dynamic reference correction to probe coordinates. Uses the alpha, beta and gama
		296	+ rotation angles of reference to rotate the probe coordinate and returns the x, y, z
		297	+ difference between probe and reference. Angles sequences and equation was extracted from
		298	+ Polhemus manual and Attitude matrix in Wikipedia.
		299	+ General equation is:
		300	+ coord = Mrot * (probe - reference)
		301	+ :param probe: sensor one defined as probe
		302	+ :param reference: sensor two defined as reference
		303	+ :return: rotated and translated coordinates
		304	+ """
		305	+
		306	+ a, b, g = np.radians(reference[3:6])
		307	+ a_p, b_p, g_p = np.radians(probe[3:6])
		308	+
		309	+ T = tr.translation_matrix(reference[:3])
		310	+ T_p = tr.translation_matrix(probe[:3])
		311	+ R = tr.euler_matrix(a, b, g, 'rzyx')
		312	+ R_p = tr.euler_matrix(a_p, b_p, g_p, 'rzyx')
		313	+ M = np.asmatrix(tr.concatenate_matrices(T, R))
		314	+ M_p = np.asmatrix(tr.concatenate_matrices(T_p, R_p))
		315	+ # M = tr.compose_matrix(angles=np.radians(reference[3:6]), translate=reference[:3])
		316	+ # print M
		317	+
		318	+ M_dyn = M.I * M_p
		319	+
		320	+ al, be, ga = tr.euler_from_matrix(M_dyn, 'rzyx')
		321	+ coord_rot = tr.translation_from_matrix(M_dyn)
		322	+
		323	+ coord_rot = np.squeeze(coord_rot)
		324	+
		325	+ # probe_4 = np.vstack((np.asmatrix(probe[:3]).reshape([3, 1]), 1.))
		326	+ # coord_rot_test = M.I * probe_4
		327	+ # coord_rot_test = np.squeeze(np.asarray(coord_rot_test))
		328	+ #
		329	+ # print "coord_rot: ", coord_rot
		330	+ # print "coord_rot_test: ", coord_rot_test
		331	+ # print "test: ", np.allclose(coord_rot, coord_rot_test[:3])
		332	+
		333	+ return coord_rot[0], coord_rot[1], coord_rot[2], np.degrees(al), np.degrees(be), np.degrees(ga)
		334	+
		335	+# def dynamic_reference_m3(probe, reference):
		336	+# """
		337	+# Apply dynamic reference correction to probe coordinates. Uses the alpha, beta and gama
		338	+# rotation angles of reference to rotate the probe coordinate and returns the x, y, z
		339	+# difference between probe and reference. Angles sequences and equation was extracted from
		340	+# Polhemus manual and Attitude matrix in Wikipedia.
		341	+# General equation is:
		342	+# coord = Mrot * (probe - reference)
		343	+# :param probe: sensor one defined as probe
		344	+# :param reference: sensor two defined as reference
		345	+# :return: rotated and translated coordinates
		346	+# """
		347	+#
		348	+# a, b, g = np.radians(reference[3:6])
		349	+# a_p, b_p, g_p = np.radians(probe[3:6])
		350	+#
		351	+# T = tr.translation_matrix(reference[:3])
		352	+# T_p = tr.translation_matrix(probe[:3])
		353	+# R = tr.euler_matrix(a, b, g, 'rzyx')
		354	+# R_p = tr.euler_matrix(a_p, b_p, g_p, 'rzyx')
		355	+# M = np.asmatrix(tr.concatenate_matrices(T, R))
		356	+# M_p = np.asmatrix(tr.concatenate_matrices(T_p, R_p))
		357	+# # M = tr.compose_matrix(angles=np.radians(reference[3:6]), translate=reference[:3])
		358	+# # print M
		359	+#
		360	+# M_dyn = M.I * M_p
		361	+#
		362	+# # al, be, ga = tr.euler_from_matrix(M_dyn, 'rzyx')
		363	+# # coord_rot = tr.translation_from_matrix(M_dyn)
		364	+# #
		365	+# # coord_rot = np.squeeze(coord_rot)
		366	+#
		367	+# # probe_4 = np.vstack((np.asmatrix(probe[:3]).reshape([3, 1]), 1.))
		368	+# # coord_rot_test = M.I * probe_4
		369	+# # coord_rot_test = np.squeeze(np.asarray(coord_rot_test))
		370	+# #
		371	+# # print "coord_rot: ", coord_rot
		372	+# # print "coord_rot_test: ", coord_rot_test
		373	+# # print "test: ", np.allclose(coord_rot, coord_rot_test[:3])
		374	+#
		375	+# return M_dyn
262		376
263		377
264	def str2float(data):	378	def str2float(data):
265	"""	379	"""
266	- Converts string detected wth Polhemus device to float array of coordinates. THis method applies	380	+ Converts string detected wth Polhemus device to float array of coordinates. This method applies
267	a correction for the minus sign in string that raises error while splitting the string into coordinates.	381	a correction for the minus sign in string that raises error while splitting the string into coordinates.
268	:param data: string of coordinates read with Polhemus	382	:param data: string of coordinates read with Polhemus
269	- :return: six float coordinates x, y, z, alfa, beta and gama	383	+ :return: six float coordinates x, y, z, alpha, beta and gamma
270	"""	384	"""
271		385
272	count = 0	386	count = 0
	@@ -20,16 +20,123 @@		@@ -20,16 +20,123 @@
20	import threading	20	import threading
21	from time import sleep	21	from time import sleep
22		22
23	-from numpy import mat	23	+from numpy import asmatrix, mat, degrees, radians, identity
24	import wx	24	import wx
25	from wx.lib.pubsub import pub as Publisher	25	from wx.lib.pubsub import pub as Publisher
26		26
27	import invesalius.data.coordinates as dco	27	import invesalius.data.coordinates as dco
		28	+import invesalius.data.transformations as tr
28		29
29	# TODO: Optimize navigation thread. Remove the infinite loop and optimize sleep.	30	# TODO: Optimize navigation thread. Remove the infinite loop and optimize sleep.
30		31
31		32
32	-class Coregistration(threading.Thread):	33	+class CoregistrationStatic(threading.Thread):
		34	+ """
		35	+ Thread to update the coordinates with the fiducial points
		36	+ co-registration method while the Navigation Button is pressed.
		37	+ Sleep function in run method is used to avoid blocking GUI and
		38	+ for better real-time navigation
		39	+ """
		40	+
		41	+ def __init__(self, coreg_data, nav_id, trck_info):
		42	+ threading.Thread.__init__(self)
		43	+ self.coreg_data = coreg_data
		44	+ self.nav_id = nav_id
		45	+ self.trck_info = trck_info
		46	+ self._pause_ = False
		47	+ self.start()
		48	+
		49	+ def stop(self):
		50	+ self._pause_ = True
		51	+
		52	+ def run(self):
		53	+ # m_change = self.coreg_data[0]
		54	+ # obj_ref_mode = self.coreg_data[2]
		55	+ #
		56	+ # trck_init = self.trck_info[0]
		57	+ # trck_id = self.trck_info[1]
		58	+ # trck_mode = self.trck_info[2]
		59	+
		60	+ m_change, obj_ref_mode = self.coreg_data
		61	+ trck_init, trck_id, trck_mode = self.trck_info
		62	+
		63	+ while self.nav_id:
		64	+ coord_raw = dco.GetCoordinates(trck_init, trck_id, trck_mode)
		65	+
		66	+ psi, theta, phi = coord_raw[obj_ref_mode, 3:]
		67	+ t_probe_raw = asmatrix(tr.translation_matrix(coord_raw[obj_ref_mode, :3]))
		68	+
		69	+ t_probe_raw[2, -1] = -t_probe_raw[2, -1]
		70	+
		71	+ m_img = m_change * t_probe_raw
		72	+
		73	+ coord = m_img[0, -1], m_img[1, -1], m_img[2, -1], psi, theta, phi
		74	+
		75	+ wx.CallAfter(Publisher.sendMessage, 'Co-registered points', (m_img, coord))
		76	+
		77	+ # TODO: Optimize the value of sleep for each tracking device.
		78	+ sleep(0.175)
		79	+
		80	+ if self._pause_:
		81	+ return
		82	+
		83	+
		84	+class CoregistrationDynamic(threading.Thread):
		85	+ """
		86	+ Thread to update the coordinates with the fiducial points
		87	+ co-registration method while the Navigation Button is pressed.
		88	+ Sleep function in run method is used to avoid blocking GUI and
		89	+ for better real-time navigation
		90	+ """
		91	+
		92	+ def __init__(self, coreg_data, nav_id, trck_info):
		93	+ threading.Thread.__init__(self)
		94	+ self.coreg_data = coreg_data
		95	+ self.nav_id = nav_id
		96	+ self.trck_info = trck_info
		97	+ self._pause_ = False
		98	+ self.start()
		99	+
		100	+ def stop(self):
		101	+ self._pause_ = True
		102	+
		103	+ def run(self):
		104	+ m_change, obj_ref_mode = self.coreg_data
		105	+ trck_init, trck_id, trck_mode = self.trck_info
		106	+
		107	+ while self.nav_id:
		108	+ coord_raw = dco.GetCoordinates(trck_init, trck_id, trck_mode)
		109	+
		110	+ psi, theta, phi = radians(coord_raw[obj_ref_mode, 3:])
		111	+ r_probe = tr.euler_matrix(psi, theta, phi, 'rzyx')
		112	+ t_probe = tr.translation_matrix(coord_raw[obj_ref_mode, :3])
		113	+ m_probe = asmatrix(tr.concatenate_matrices(t_probe, r_probe))
		114	+
		115	+ psi_ref, theta_ref, phi_ref = radians(coord_raw[1, 3:])
		116	+ r_ref = tr.euler_matrix(psi_ref, theta_ref, phi_ref, 'rzyx')
		117	+ t_ref = tr.translation_matrix(coord_raw[1, :3])
		118	+ m_ref = asmatrix(tr.concatenate_matrices(t_ref, r_ref))
		119	+
		120	+ m_dyn = m_ref.I * m_probe
		121	+ m_dyn[2, -1] = -m_dyn[2, -1]
		122	+
		123	+ m_img = m_change * m_dyn
		124	+
		125	+ scale, shear, angles, trans, persp = tr.decompose_matrix(m_img)
		126	+
		127	+ coord = m_img[0, -1], m_img[1, -1], m_img[2, -1], \
		128	+ degrees(angles[0]), degrees(angles[1]), degrees(angles[2])
		129	+
		130	+ wx.CallAfter(Publisher.sendMessage, 'Co-registered points', (m_img, coord))
		131	+
		132	+ # TODO: Optimize the value of sleep for each tracking device.
		133	+ sleep(0.175)
		134	+
		135	+ if self._pause_:
		136	+ return
		137	+
		138	+
		139	+class CoregistrationDynamic_old(threading.Thread):
33	"""	140	"""
34	Thread to update the coordinates with the fiducial points	141	Thread to update the coordinates with the fiducial points
35	co-registration method while the Navigation Button is pressed.	142	co-registration method while the Navigation Button is pressed.
	@@ -44,10 +151,10 @@ class Coregistration(threading.Thread):		@@ -44,10 +151,10 @@ class Coregistration(threading.Thread):
44	self.trck_info = trck_info	151	self.trck_info = trck_info
45	self._pause_ = False	152	self._pause_ = False
46	self.start()	153	self.start()
47	-	154	+
48	def stop(self):	155	def stop(self):
49	self._pause_ = True	156	self._pause_ = True
50	-	157	+
51	def run(self):	158	def run(self):
52	m_inv = self.bases[0]	159	m_inv = self.bases[0]
53	n = self.bases[1]	160	n = self.bases[1]
	@@ -58,24 +165,199 @@ class Coregistration(threading.Thread):		@@ -58,24 +165,199 @@ class Coregistration(threading.Thread):
58	trck_mode = self.trck_info[2]	165	trck_mode = self.trck_info[2]
59		166
60	while self.nav_id:	167	while self.nav_id:
61	- trck_coord = dco.GetCoordinates(trck_init, trck_id, trck_mode)
62	- trck_xyz = mat([[trck_coord[0]], [trck_coord[1]], [trck_coord[2]]])	168	+ # trck_coord, probe, reference = dco.GetCoordinates(trck_init, trck_id, trck_mode)
		169	+ coord_raw = dco.GetCoordinates(trck_init, trck_id, trck_mode)
63		170
64	- img = q1 + (m_invn)(trck_xyz - q2)	171	+ trck_coord = dco.dynamic_reference(coord_raw[0, :], coord_raw[1, :])
		172	+
		173	+ trck_xyz = mat([[trck_coord[0]], [trck_coord[1]], [trck_coord[2]]])
		174	+ img = q1 + (m_inv * n) * (trck_xyz - q2)
65		175
66	coord = (float(img[0]), float(img[1]), float(img[2]), trck_coord[3],	176	coord = (float(img[0]), float(img[1]), float(img[2]), trck_coord[3],
67	trck_coord[4], trck_coord[5])	177	trck_coord[4], trck_coord[5])
		178	+ angles = coord_raw[0, 3:6]
68		179
69	# Tried several combinations and different locations to send the messages,	180	# Tried several combinations and different locations to send the messages,
70	# however only this one does not block the GUI during navigation.	181	# however only this one does not block the GUI during navigation.
71	- wx.CallAfter(Publisher.sendMessage, 'Co-registered points', coord[0:3])
72	- wx.CallAfter(Publisher.sendMessage, 'Set camera in volume', coord[0:3])	182	+ wx.CallAfter(Publisher.sendMessage, 'Co-registered points', coord)
		183	+ wx.CallAfter(Publisher.sendMessage, 'Set camera in volume', coord)
		184	+ wx.CallAfter(Publisher.sendMessage, 'Update tracker angles', angles)
73		185
74	# TODO: Optimize the value of sleep for each tracking device.	186	# TODO: Optimize the value of sleep for each tracking device.
75	# Debug tracker is not working with 0.175 so changed to 0.2	187	# Debug tracker is not working with 0.175 so changed to 0.2
76	# However, 0.2 is too low update frequency ~5 Hz. Need optimization URGENTLY.	188	# However, 0.2 is too low update frequency ~5 Hz. Need optimization URGENTLY.
77	- #sleep(.3)	189	+ # sleep(.3)
		190	+ sleep(0.175)
		191	+
		192	+ if self._pause_:
		193	+ return
		194	+
		195	+
		196	+class CoregistrationObjectStatic(threading.Thread):
		197	+ """
		198	+ Thread to update the coordinates with the fiducial points
		199	+ co-registration method while the Navigation Button is pressed.
		200	+ Sleep function in run method is used to avoid blocking GUI and
		201	+ for better real-time navigation
		202	+ """
		203	+
		204	+ def __init__(self, coreg_data, nav_id, trck_info):
		205	+ threading.Thread.__init__(self)
		206	+ self.coreg_data = coreg_data
		207	+ self.nav_id = nav_id
		208	+ self.trck_info = trck_info
		209	+ self._pause_ = False
		210	+ self.start()
		211	+
		212	+ def stop(self):
		213	+ self._pause_ = True
		214	+
		215	+ def run(self):
		216	+ # m_change = self.coreg_data[0]
		217	+ # t_obj_raw = self.coreg_data[1]
		218	+ # s0_raw = self.coreg_data[2]
		219	+ # r_s0_raw = self.coreg_data[3]
		220	+ # s0_dyn = self.coreg_data[4]
		221	+ # m_obj_raw = self.coreg_data[5]
		222	+ # r_obj_img = self.coreg_data[6]
		223	+ # obj_ref_mode = self.coreg_data[7]
		224	+ #
		225	+ # trck_init = self.trck_info[0]
		226	+ # trck_id = self.trck_info[1]
		227	+ # trck_mode = self.trck_info[2]
		228	+
		229	+ m_change, obj_ref_mode, t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img = self.coreg_data
		230	+ trck_init, trck_id, trck_mode = self.trck_info
		231	+
		232	+ while self.nav_id:
		233	+ coord_raw = dco.GetCoordinates(trck_init, trck_id, trck_mode)
		234	+
		235	+ as1, bs1, gs1 = radians(coord_raw[obj_ref_mode, 3:])
		236	+ r_probe = asmatrix(tr.euler_matrix(as1, bs1, gs1, 'rzyx'))
		237	+ t_probe_raw = asmatrix(tr.translation_matrix(coord_raw[obj_ref_mode, :3]))
		238	+ t_offset_aux = r_s0_raw.I * r_probe * t_obj_raw
		239	+ t_offset = asmatrix(identity(4))
		240	+ t_offset[:, -1] = t_offset_aux[:, -1]
		241	+ t_probe = s0_raw * t_offset * s0_raw.I * t_probe_raw
		242	+ m_probe = asmatrix(tr.concatenate_matrices(t_probe, r_probe))
		243	+
		244	+ m_probe[2, -1] = -m_probe[2, -1]
		245	+
		246	+ m_img = m_change * m_probe
		247	+ r_obj = r_obj_img * m_obj_raw.I * s0_dyn.I * m_probe * m_obj_raw
		248	+
		249	+ m_img[:3, :3] = r_obj[:3, :3]
		250	+
		251	+ scale, shear, angles, trans, persp = tr.decompose_matrix(m_img)
		252	+
		253	+ coord = m_img[0, -1], m_img[1, -1], m_img[2, -1], \
		254	+ degrees(angles[0]), degrees(angles[1]), degrees(angles[2])
		255	+
		256	+ wx.CallAfter(Publisher.sendMessage, 'Co-registered points', (m_img, coord))
		257	+ wx.CallAfter(Publisher.sendMessage, 'Update object matrix', (m_img, coord))
		258	+
		259	+ # TODO: Optimize the value of sleep for each tracking device.
78	sleep(0.175)	260	sleep(0.175)
79		261
		262	+ # Debug tracker is not working with 0.175 so changed to 0.2
		263	+ # However, 0.2 is too low update frequency ~5 Hz. Need optimization URGENTLY.
		264	+ # sleep(.3)
		265	+
		266	+ # partially working for translate and offset,
		267	+ # but offset is kept always in same axis, have to fix for rotation
		268	+ # M_dyn = M_reference.I * T_stylus
		269	+ # M_dyn[2, -1] = -M_dyn[2, -1]
		270	+ # M_dyn_ch = M_change * M_dyn
		271	+ # ddd = M_dyn_ch[0, -1], M_dyn_ch[1, -1], M_dyn_ch[2, -1]
		272	+ # M_dyn_ch[:3, -1] = asmatrix(db.flip_x_m(ddd)).reshape([3, 1])
		273	+ # M_final = S0 * M_obj_trans_0 * S0.I * M_dyn_ch
		274	+
		275	+ # this works for static reference object rotation
		276	+ # R_dyn = M_vtk * M_obj_rot_raw.I * S0_rot_raw.I * R_stylus * M_obj_rot_raw
		277	+ # this works for dynamic reference in rotation but not in translation
		278	+ # R_dyn = M_vtk * M_obj_rot_raw.I * S0_rot_dyn.I * R_reference.I * R_stylus * M_obj_rot_raw
		279	+
80	if self._pause_:	280	if self._pause_:
81	return	281	return
		282	+
		283	+
		284	+class CoregistrationObjectDynamic(threading.Thread):
		285	+ """
		286	+ Thread to update the coordinates with the fiducial points
		287	+ co-registration method while the Navigation Button is pressed.
		288	+ Sleep function in run method is used to avoid blocking GUI and
		289	+ for better real-time navigation
		290	+ """
		291	+
		292	+ def __init__(self, coreg_data, nav_id, trck_info):
		293	+ threading.Thread.__init__(self)
		294	+ self.coreg_data = coreg_data
		295	+ self.nav_id = nav_id
		296	+ self.trck_info = trck_info
		297	+ self._pause_ = False
		298	+ self.start()
		299	+
		300	+ def stop(self):
		301	+ self._pause_ = True
		302	+
		303	+ def run(self):
		304	+
		305	+ m_change, obj_ref_mode, t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img = self.coreg_data
		306	+ trck_init, trck_id, trck_mode = self.trck_info
		307	+
		308	+ while self.nav_id:
		309	+ coord_raw = dco.GetCoordinates(trck_init, trck_id, trck_mode)
		310	+
		311	+ as1, bs1, gs1 = radians(coord_raw[obj_ref_mode, 3:])
		312	+ r_probe = asmatrix(tr.euler_matrix(as1, bs1, gs1, 'rzyx'))
		313	+ t_probe_raw = asmatrix(tr.translation_matrix(coord_raw[obj_ref_mode, :3]))
		314	+ t_offset_aux = r_s0_raw.I * r_probe * t_obj_raw
		315	+ t_offset = asmatrix(identity(4))
		316	+ t_offset[:, -1] = t_offset_aux[:, -1]
		317	+ t_probe = s0_raw * t_offset * s0_raw.I * t_probe_raw
		318	+ m_probe = asmatrix(tr.concatenate_matrices(t_probe, r_probe))
		319	+
		320	+ a, b, g = radians(coord_raw[1, 3:])
		321	+ r_ref = tr.euler_matrix(a, b, g, 'rzyx')
		322	+ t_ref = tr.translation_matrix(coord_raw[1, :3])
		323	+ m_ref = asmatrix(tr.concatenate_matrices(t_ref, r_ref))
		324	+
		325	+ m_dyn = m_ref.I * m_probe
		326	+ m_dyn[2, -1] = -m_dyn[2, -1]
		327	+
		328	+ m_img = m_change * m_dyn
		329	+ r_obj = r_obj_img * m_obj_raw.I * s0_dyn.I * m_dyn * m_obj_raw
		330	+
		331	+ m_img[:3, :3] = r_obj[:3, :3]
		332	+
		333	+ scale, shear, angles, trans, persp = tr.decompose_matrix(m_img)
		334	+
		335	+ coord = m_img[0, -1], m_img[1, -1], m_img[2, -1],\
		336	+ degrees(angles[0]), degrees(angles[1]), degrees(angles[2])
		337	+
		338	+ wx.CallAfter(Publisher.sendMessage, 'Co-registered points', (m_img, coord))
		339	+ wx.CallAfter(Publisher.sendMessage, 'Update object matrix', (m_img, coord))
		340	+
		341	+ # TODO: Optimize the value of sleep for each tracking device.
		342	+ sleep(0.175)
		343	+
		344	+ # Debug tracker is not working with 0.175 so changed to 0.2
		345	+ # However, 0.2 is too low update frequency ~5 Hz. Need optimization URGENTLY.
		346	+ # sleep(.3)
		347	+
		348	+ # partially working for translate and offset,
		349	+ # but offset is kept always in same axis, have to fix for rotation
		350	+ # M_dyn = M_reference.I * T_stylus
		351	+ # M_dyn[2, -1] = -M_dyn[2, -1]
		352	+ # M_dyn_ch = M_change * M_dyn
		353	+ # ddd = M_dyn_ch[0, -1], M_dyn_ch[1, -1], M_dyn_ch[2, -1]
		354	+ # M_dyn_ch[:3, -1] = asmatrix(db.flip_x_m(ddd)).reshape([3, 1])
		355	+ # M_final = S0 * M_obj_trans_0 * S0.I * M_dyn_ch
		356	+
		357	+ # this works for static reference object rotation
		358	+ # R_dyn = M_vtk * M_obj_rot_raw.I * S0_rot_raw.I * R_stylus * M_obj_rot_raw
		359	+ # this works for dynamic reference in rotation but not in translation
		360	+ # R_dyn = M_vtk * M_obj_rot_raw.I * S0_rot_dyn.I * R_reference.I * R_stylus * M_obj_rot_raw
		361	+
		362	+ if self._pause_:
		363	+ return
82	\ No newline at end of file	364	\ No newline at end of file
	@@ -0,0 +1,67 @@		@@ -0,0 +1,67 @@
		1	+#--------------------------------------------------------------------------
		2	+# Software: InVesalius - Software de Reconstrucao 3D de Imagens Medicas
		3	+# Copyright: (C) 2001 Centro de Pesquisas Renato Archer
		4	+# Homepage: http://www.softwarepublico.gov.br
		5	+# Contact: invesalius@cti.gov.br
		6	+# License: GNU - GPL 2 (LICENSE.txt/LICENCA.txt)
		7	+#--------------------------------------------------------------------------
		8	+# Este programa e software livre; voce pode redistribui-lo e/ou
		9	+# modifica-lo sob os termos da Licenca Publica Geral GNU, conforme
		10	+# publicada pela Free Software Foundation; de acordo com a versao 2
		11	+# da Licenca.
		12	+#
		13	+# Este programa eh distribuido na expectativa de ser util, mas SEM
		14	+# QUALQUER GARANTIA; sem mesmo a garantia implicita de
		15	+# COMERCIALIZACAO ou de ADEQUACAO A QUALQUER PROPOSITO EM
		16	+# PARTICULAR. Consulte a Licenca Publica Geral GNU para obter mais
		17	+# detalhes.
		18	+#--------------------------------------------------------------------------
		19	+
		20	+import threading
		21	+import time
		22	+
		23	+import wx
		24	+from numpy import array, savetxt, hstack,vstack, asarray
		25	+import invesalius.gui.dialogs as dlg
		26	+from wx.lib.pubsub import pub as Publisher
		27	+
		28	+
		29	+class Record(threading.Thread):
		30	+ """
		31	+ Thread created to save obj coords with software during neuronavigation
		32	+ """
		33	+
		34	+ def __init__(self, nav_id, timestamp):
		35	+ threading.Thread.__init__(self)
		36	+ self.nav_id = nav_id
		37	+ self.coord = None
		38	+ self.timestamp = timestamp
		39	+ self.coord_list = array([])
		40	+ self.__bind_events()
		41	+ self._pause_ = False
		42	+ self.start()
		43	+
		44	+ def __bind_events(self):
		45	+ Publisher.subscribe(self.UpdateCurrentCoords, 'Co-registered points')
		46	+
		47	+ def UpdateCurrentCoords(self, pubsub_evt):
		48	+ self.coord = asarray(pubsub_evt.data[1])
		49	+
		50	+ def stop(self):
		51	+ self._pause_ = True
		52	+ #save coords dialog
		53	+ filename = dlg.ShowSaveCoordsDialog("coords.csv")
		54	+ if filename:
		55	+ savetxt(filename, self.coord_list, delimiter=',', fmt='%.4f', header="time, x, y, z, a, b, g", comments="")
		56	+
		57	+ def run(self):
		58	+ initial_time = time.time()
		59	+ while self.nav_id:
		60	+ relative_time = asarray(time.time() - initial_time)
		61	+ time.sleep(self.timestamp)
		62	+ if self.coord_list.size == 0:
		63	+ self.coord_list = hstack((relative_time, self.coord))
		64	+ else:
		65	+ self.coord_list = vstack((self.coord_list, hstack((relative_time, self.coord))))
		66	+ if self._pause_:
		67	+ return
0	\ No newline at end of file	68	\ No newline at end of file
	@@ -238,7 +238,7 @@ class CrossInteractorStyle(DefaultInteractorStyle):		@@ -238,7 +238,7 @@ class CrossInteractorStyle(DefaultInteractorStyle):
238	Publisher.sendMessage('Update cross position', (wx, wy, wz))	238	Publisher.sendMessage('Update cross position', (wx, wy, wz))
239	self.ScrollSlice(coord)	239	self.ScrollSlice(coord)
240	Publisher.sendMessage('Set ball reference position', (wx, wy, wz))	240	Publisher.sendMessage('Set ball reference position', (wx, wy, wz))
241	- Publisher.sendMessage('Set camera in volume', (wx, wy, wz))	241	+ Publisher.sendMessage('Co-registered points', (None, (wx, wy, wz, 0., 0., 0.)))
242		242
243	iren.Render()	243	iren.Render()
244		244
	@@ -124,6 +124,9 @@ class Text(object):		@@ -124,6 +124,9 @@ class Text(object):
124	def ShadowOff(self):	124	def ShadowOff(self):
125	self.property.ShadowOff()	125	self.property.ShadowOff()
126		126
		127	+ def BoldOn(self):
		128	+ self.property.BoldOn()
		129	+
127	def SetSize(self, size):	130	def SetSize(self, size):
128	self.property.SetFontSize(size)	131	self.property.SetFontSize(size)
129		132
	@@ -135,15 +138,32 @@ class Text(object):		@@ -135,15 +138,32 @@ class Text(object):
135	# With some encoding in some dicom fields (like name) raises a	138	# With some encoding in some dicom fields (like name) raises a
136	# UnicodeEncodeError because they have non-ascii characters. To avoid	139	# UnicodeEncodeError because they have non-ascii characters. To avoid
137	# that we encode in utf-8.	140	# that we encode in utf-8.
138	-	141	+
139	if sys.platform == 'win32':	142	if sys.platform == 'win32':
140	- self.mapper.SetInput(value.encode("utf-8"))	143	+ self.mapper.SetInput(value.encode("utf-8"))
141	else:	144	else:
142	try:	145	try:
143	self.mapper.SetInput(value.encode("latin-1"))	146	self.mapper.SetInput(value.encode("latin-1"))
144	except(UnicodeEncodeError):	147	except(UnicodeEncodeError):
145	self.mapper.SetInput(value.encode("utf-8"))	148	self.mapper.SetInput(value.encode("utf-8"))
146		149
		150	+ def SetCoilDistanceValue(self, value):
		151	+ if isinstance(value, int) or isinstance(value, float):
		152	+ value = 'Dist: ' + str("{:06.2f}".format(value)) + ' mm'
		153	+ if sys.platform == 'win32':
		154	+ value += "" # Otherwise 0 is not shown under win32
		155	+ # With some encoding in some dicom fields (like name) raises a
		156	+ # UnicodeEncodeError because they have non-ascii characters. To avoid
		157	+ # that we encode in utf-8.
		158	+
		159	+ if sys.platform == 'win32':
		160	+ self.mapper.SetInput(value.encode("utf-8"))
		161	+ else:
		162	+ try:
		163	+ self.mapper.SetInput(value.encode("latin-1"))
		164	+ except(UnicodeEncodeError):
		165	+ self.mapper.SetInput(value.encode("utf-8"))
		166	+
147	def SetPosition(self, position):	167	def SetPosition(self, position):
148	self.actor.GetPositionCoordinate().SetValue(position[0],	168	self.actor.GetPositionCoordinate().SetValue(position[0],
149	position[1])	169	position[1])
	@@ -306,7 +306,7 @@ import wx.lib.colourselect as csel		@@ -306,7 +306,7 @@ import wx.lib.colourselect as csel
306		306
307	import invesalius.constants as const	307	import invesalius.constants as const
308		308
309	-[BUTTON_RAYCASTING, BUTTON_VIEW, BUTTON_SLICE_PLANE, BUTTON_3D_STEREO] = [wx.NewId() for num in xrange(4)]	309	+[BUTTON_RAYCASTING, BUTTON_VIEW, BUTTON_SLICE_PLANE, BUTTON_3D_STEREO, BUTTON_TARGET] = [wx.NewId() for num in xrange(5)]
310	RAYCASTING_TOOLS = wx.NewId()	310	RAYCASTING_TOOLS = wx.NewId()
311		311
312	ID_TO_NAME = {}	312	ID_TO_NAME = {}
	@@ -346,6 +346,9 @@ class VolumeToolPanel(wx.Panel):		@@ -346,6 +346,9 @@ class VolumeToolPanel(wx.Panel):
346	BMP_3D_STEREO = wx.Bitmap(os.path.join(const.ICON_DIR, "3D_glasses.png"),	346	BMP_3D_STEREO = wx.Bitmap(os.path.join(const.ICON_DIR, "3D_glasses.png"),
347	wx.BITMAP_TYPE_PNG)	347	wx.BITMAP_TYPE_PNG)
348		348
		349	+ BMP_TARGET = wx.Bitmap(os.path.join(const.ICON_DIR, "target.png"),
		350	+ wx.BITMAP_TYPE_PNG)
		351	+
349		352
350	button_raycasting = pbtn.PlateButton(self, BUTTON_RAYCASTING,"",	353	button_raycasting = pbtn.PlateButton(self, BUTTON_RAYCASTING,"",
351	BMP_RAYCASTING, style=pbtn.PB_STYLE_SQUARE,	354	BMP_RAYCASTING, style=pbtn.PB_STYLE_SQUARE,
	@@ -359,6 +362,11 @@ class VolumeToolPanel(wx.Panel):		@@ -359,6 +362,11 @@ class VolumeToolPanel(wx.Panel):
359	BMP_SLICE_PLANE, style=pbtn.PB_STYLE_SQUARE,	362	BMP_SLICE_PLANE, style=pbtn.PB_STYLE_SQUARE,
360	size=(32,32))	363	size=(32,32))
361		364
		365	+ button_target = self.button_target = pbtn.PlateButton(self, BUTTON_TARGET,"",
		366	+ BMP_TARGET, style=pbtn.PB_STYLE_SQUARE\|pbtn.PB_STYLE_TOGGLE,
		367	+ size=(32,32))
		368	+ self.button_target.Enable(0)
		369	+
362	self.button_raycasting = button_raycasting	370	self.button_raycasting = button_raycasting
363	self.button_stereo = button_stereo	371	self.button_stereo = button_stereo
364		372
	@@ -389,7 +397,11 @@ class VolumeToolPanel(wx.Panel):		@@ -389,7 +397,11 @@ class VolumeToolPanel(wx.Panel):
389	sizer.Add(button_view, 0, wx.TOP\|wx.BOTTOM, 1)	397	sizer.Add(button_view, 0, wx.TOP\|wx.BOTTOM, 1)
390	sizer.Add(button_slice_plane, 0, wx.TOP\|wx.BOTTOM, 1)	398	sizer.Add(button_slice_plane, 0, wx.TOP\|wx.BOTTOM, 1)
391	sizer.Add(button_stereo, 0, wx.TOP\|wx.BOTTOM, 1)	399	sizer.Add(button_stereo, 0, wx.TOP\|wx.BOTTOM, 1)
		400	+ sizer.Add(button_target, 0, wx.TOP \| wx.BOTTOM, 1)
392		401
		402	+ self.navigation_status = False
		403	+ self.status_target_select = False
		404	+ self.status_obj_tracker = False
393		405
394	sizer.Fit(self)	406	sizer.Fit(self)
395		407
	@@ -408,6 +420,8 @@ class VolumeToolPanel(wx.Panel):		@@ -408,6 +420,8 @@ class VolumeToolPanel(wx.Panel):
408	Publisher.subscribe(self.DisablePreset, 'Close project data')	420	Publisher.subscribe(self.DisablePreset, 'Close project data')
409	Publisher.subscribe(self.Uncheck, 'Uncheck image plane menu')	421	Publisher.subscribe(self.Uncheck, 'Uncheck image plane menu')
410	Publisher.subscribe(self.DisableVolumeCutMenu, 'Disable volume cut menu')	422	Publisher.subscribe(self.DisableVolumeCutMenu, 'Disable volume cut menu')
		423	+ Publisher.subscribe(self.StatusTargetSelect, 'Disable or enable coil tracker')
		424	+ Publisher.subscribe(self.StatusObjTracker, 'Status target button')
411		425
412	def DisablePreset(self, pubsub_evt):	426	def DisablePreset(self, pubsub_evt):
413	self.off_item.Check(1)	427	self.off_item.Check(1)
	@@ -419,6 +433,7 @@ class VolumeToolPanel(wx.Panel):		@@ -419,6 +433,7 @@ class VolumeToolPanel(wx.Panel):
419	self.button_view.Bind(wx.EVT_LEFT_DOWN, self.OnButtonView)	433	self.button_view.Bind(wx.EVT_LEFT_DOWN, self.OnButtonView)
420	self.button_colour.Bind(csel.EVT_COLOURSELECT, self.OnSelectColour)	434	self.button_colour.Bind(csel.EVT_COLOURSELECT, self.OnSelectColour)
421	self.button_stereo.Bind(wx.EVT_LEFT_DOWN, self.OnButtonStereo)	435	self.button_stereo.Bind(wx.EVT_LEFT_DOWN, self.OnButtonStereo)
		436	+ self.button_target.Bind(wx.EVT_LEFT_DOWN, self.OnButtonTarget)
422		437
423	def OnButtonRaycasting(self, evt):	438	def OnButtonRaycasting(self, evt):
424	# MENU RELATED TO RAYCASTING TYPES	439	# MENU RELATED TO RAYCASTING TYPES
	@@ -433,6 +448,29 @@ class VolumeToolPanel(wx.Panel):		@@ -433,6 +448,29 @@ class VolumeToolPanel(wx.Panel):
433	def OnButtonSlicePlane(self, evt):	448	def OnButtonSlicePlane(self, evt):
434	self.button_slice_plane.PopupMenu(self.slice_plane_menu)	449	self.button_slice_plane.PopupMenu(self.slice_plane_menu)
435		450
		451	+ def StatusObjTracker(self, pubsub_evt):
		452	+ self.status_obj_tracker = pubsub_evt.data
		453	+ self.StatusNavigation()
		454	+
		455	+ def StatusTargetSelect(self, pubsub_evt):
		456	+ self.status_target_select = pubsub_evt.data
		457	+ self.StatusNavigation()
		458	+
		459	+ def StatusNavigation(self):
		460	+ if self.status_target_select and self.status_obj_tracker:
		461	+ self.button_target.Enable(1)
		462	+ else:
		463	+ self.OnButtonTarget(False)
		464	+ self.button_target.Enable(0)
		465	+
		466	+ def OnButtonTarget(self, evt):
		467	+ if not self.button_target.IsPressed() and evt is not False:
		468	+ self.button_target._pressed = True
		469	+ Publisher.sendMessage('Target navigation mode', self.button_target._pressed)
		470	+ elif self.button_target.IsPressed() or evt is False:
		471	+ self.button_target._pressed = False
		472	+ Publisher.sendMessage('Target navigation mode', self.button_target._pressed)
		473	+
436	def OnSavePreset(self, evt):	474	def OnSavePreset(self, evt):
437	d = wx.TextEntryDialog(self, _("Preset name"))	475	d = wx.TextEntryDialog(self, _("Preset name"))
438	if d.ShowModal() == wx.ID_OK:	476	if d.ShowModal() == wx.ID_OK:
	@@ -18,10 +18,20 @@		@@ -18,10 +18,20 @@
18	# PARTICULAR. Consulte a Licenca Publica Geral GNU para obter mais	18	# PARTICULAR. Consulte a Licenca Publica Geral GNU para obter mais
19	# detalhes.	19	# detalhes.
20	#--------------------------------------------------------------------------	20	#--------------------------------------------------------------------------
		21	+
21	import os	22	import os
22	import random	23	import random
23	import sys	24	import sys
24		25
		26	+if sys.platform == 'win32':
		27	+ try:
		28	+ import win32api
		29	+ _has_win32api = True
		30	+ except ImportError:
		31	+ _has_win32api = False
		32	+else:
		33	+ _has_win32api = False
		34	+
25	import vtk	35	import vtk
26	import wx	36	import wx
27	import wx.combo	37	import wx.combo
	@@ -33,6 +43,7 @@ from wx.lib.wordwrap import wordwrap		@@ -33,6 +43,7 @@ from wx.lib.wordwrap import wordwrap
33	from wx.lib.pubsub import pub as Publisher	43	from wx.lib.pubsub import pub as Publisher
34		44
35	import invesalius.constants as const	45	import invesalius.constants as const
		46	+import invesalius.data.coordinates as dco
36	import invesalius.gui.widgets.gradient as grad	47	import invesalius.gui.widgets.gradient as grad
37	import invesalius.session as ses	48	import invesalius.session as ses
38	import invesalius.utils as utils	49	import invesalius.utils as utils
	@@ -470,6 +481,36 @@ def ShowSaveMarkersDialog(default_filename=None):		@@ -470,6 +481,36 @@ def ShowSaveMarkersDialog(default_filename=None):
470	os.chdir(current_dir)	481	os.chdir(current_dir)
471	return filename	482	return filename
472		483
		484	+def ShowSaveCoordsDialog(default_filename=None):
		485	+ current_dir = os.path.abspath(".")
		486	+ dlg = wx.FileDialog(None,
		487	+ _("Save coords as..."), # title
		488	+ "", # last used directory
		489	+ default_filename,
		490	+ _("Coordinates files (.csv)\|.csv"),
		491	+ wx.SAVE \| wx.OVERWRITE_PROMPT)
		492	+ # dlg.SetFilterIndex(0) # default is VTI
		493	+
		494	+ filename = None
		495	+ try:
		496	+ if dlg.ShowModal() == wx.ID_OK:
		497	+ filename = dlg.GetPath()
		498	+ ok = 1
		499	+ else:
		500	+ ok = 0
		501	+ except(wx._core.PyAssertionError): # TODO: fix win64
		502	+ filename = dlg.GetPath()
		503	+ ok = 1
		504	+
		505	+ if (ok):
		506	+ extension = "csv"
		507	+ if sys.platform != 'win32':
		508	+ if filename.split(".")[-1] != extension:
		509	+ filename = filename + "." + extension
		510	+
		511	+ os.chdir(current_dir)
		512	+ return filename
		513	+
473		514
474	def ShowLoadMarkersDialog():	515	def ShowLoadMarkersDialog():
475	current_dir = os.path.abspath(".")	516	current_dir = os.path.abspath(".")
	@@ -500,6 +541,66 @@ def ShowLoadMarkersDialog():		@@ -500,6 +541,66 @@ def ShowLoadMarkersDialog():
500	return filepath	541	return filepath
501		542
502		543
		544	+def ShowSaveRegistrationDialog(default_filename=None):
		545	+ current_dir = os.path.abspath(".")
		546	+ dlg = wx.FileDialog(None,
		547	+ _("Save object registration as..."), # title
		548	+ "", # last used directory
		549	+ default_filename,
		550	+ _("Registration files (.obr)\|.obr"),
		551	+ wx.SAVE \| wx.OVERWRITE_PROMPT)
		552	+ # dlg.SetFilterIndex(0) # default is VTI
		553	+
		554	+ filename = None
		555	+ try:
		556	+ if dlg.ShowModal() == wx.ID_OK:
		557	+ filename = dlg.GetPath()
		558	+ ok = 1
		559	+ else:
		560	+ ok = 0
		561	+ except(wx._core.PyAssertionError): # TODO: fix win64
		562	+ filename = dlg.GetPath()
		563	+ ok = 1
		564	+
		565	+ if (ok):
		566	+ extension = "obr"
		567	+ if sys.platform != 'win32':
		568	+ if filename.split(".")[-1] != extension:
		569	+ filename = filename + "." + extension
		570	+
		571	+ os.chdir(current_dir)
		572	+ return filename
		573	+
		574	+
		575	+def ShowLoadRegistrationDialog():
		576	+ current_dir = os.path.abspath(".")
		577	+
		578	+ dlg = wx.FileDialog(None, message=_("Load object registration"),
		579	+ defaultDir="",
		580	+ defaultFile="",
		581	+ wildcard=_("Registration files (.obr)\|.obr"),
		582	+ style=wx.OPEN\|wx.CHANGE_DIR)
		583	+
		584	+ # inv3 filter is default
		585	+ dlg.SetFilterIndex(0)
		586	+
		587	+ # Show the dialog and retrieve the user response. If it is the OK response,
		588	+ # process the data.
		589	+ filepath = None
		590	+ try:
		591	+ if dlg.ShowModal() == wx.ID_OK:
		592	+ # This returns a Python list of files that were selected.
		593	+ filepath = dlg.GetPath()
		594	+ except(wx._core.PyAssertionError): # FIX: win64
		595	+ filepath = dlg.GetPath()
		596	+
		597	+ # Destroy the dialog. Don't do this until you are done with it!
		598	+ # BAD things can happen otherwise!
		599	+ dlg.Destroy()
		600	+ os.chdir(current_dir)
		601	+ return filepath
		602	+
		603	+
503	class MessageDialog(wx.Dialog):	604	class MessageDialog(wx.Dialog):
504	def __init__(self, message):	605	def __init__(self, message):
505	pre = wx.PreDialog()	606	pre = wx.PreDialog()
	@@ -729,7 +830,19 @@ def SurfaceSelectionRequiredForDuplication():		@@ -729,7 +830,19 @@ def SurfaceSelectionRequiredForDuplication():
729		830
730	# Dialogs for neuronavigation mode	831	# Dialogs for neuronavigation mode
731	def InvalidFiducials():	832	def InvalidFiducials():
732	- msg = _("Fiducials are invalid. Select six coordinates.")	833	+ msg = _("Fiducials are invalid. Select all coordinates.")
		834	+ if sys.platform == 'darwin':
		835	+ dlg = wx.MessageDialog(None, "", msg,
		836	+ wx.ICON_INFORMATION \| wx.OK)
		837	+ else:
		838	+ dlg = wx.MessageDialog(None, msg, "InVesalius 3 - Neuronavigator",
		839	+ wx.ICON_INFORMATION \| wx.OK)
		840	+ dlg.ShowModal()
		841	+ dlg.Destroy()
		842	+
		843	+
		844	+def InvalidObjectRegistration():
		845	+ msg = _("Perform coil registration before navigation.")
733	if sys.platform == 'darwin':	846	if sys.platform == 'darwin':
734	dlg = wx.MessageDialog(None, "", msg,	847	dlg = wx.MessageDialog(None, "", msg,
735	wx.ICON_INFORMATION \| wx.OK)	848	wx.ICON_INFORMATION \| wx.OK)
	@@ -793,6 +906,7 @@ def NoMarkerSelected():		@@ -793,6 +906,7 @@ def NoMarkerSelected():
793	dlg.ShowModal()	906	dlg.ShowModal()
794	dlg.Destroy()	907	dlg.Destroy()
795		908
		909	+
796	def DeleteAllMarkers():	910	def DeleteAllMarkers():
797	msg = _("Do you really want to delete all markers?")	911	msg = _("Do you really want to delete all markers?")
798	if sys.platform == 'darwin':	912	if sys.platform == 'darwin':
	@@ -805,6 +919,38 @@ def DeleteAllMarkers():		@@ -805,6 +919,38 @@ def DeleteAllMarkers():
805	dlg.Destroy()	919	dlg.Destroy()
806	return result	920	return result
807		921
		922	+def DeleteTarget():
		923	+ msg = _("Target deleted")
		924	+ if sys.platform == 'darwin':
		925	+ dlg = wx.MessageDialog(None, "", msg,
		926	+ wx.ICON_INFORMATION \| wx.OK)
		927	+ else:
		928	+ dlg = wx.MessageDialog(None, msg, "InVesalius 3 - Neuronavigator",
		929	+ wx.ICON_INFORMATION \| wx.OK)
		930	+ dlg.ShowModal()
		931	+ dlg.Destroy()
		932	+
		933	+def NewTarget():
		934	+ msg = _("New target selected")
		935	+ if sys.platform == 'darwin':
		936	+ dlg = wx.MessageDialog(None, "", msg,
		937	+ wx.ICON_INFORMATION \| wx.OK)
		938	+ else:
		939	+ dlg = wx.MessageDialog(None, msg, "InVesalius 3 - Neuronavigator",
		940	+ wx.ICON_INFORMATION \| wx.OK)
		941	+ dlg.ShowModal()
		942	+ dlg.Destroy()
		943	+
		944	+def InvalidTargetID():
		945	+ msg = _("Sorry, you cannot use 'TARGET' ID")
		946	+ if sys.platform == 'darwin':
		947	+ dlg = wx.MessageDialog(None, "", msg,
		948	+ wx.ICON_INFORMATION \| wx.OK)
		949	+ else:
		950	+ dlg = wx.MessageDialog(None, msg, "InVesalius 3 - Neuronavigator",
		951	+ wx.ICON_INFORMATION \| wx.OK)
		952	+ dlg.ShowModal()
		953	+ dlg.Destroy()
808		954
809	def EnterMarkerID(default):	955	def EnterMarkerID(default):
810	msg = _("Edit marker ID")	956	msg = _("Edit marker ID")
	@@ -2967,3 +3113,246 @@ class FillHolesAutoDialog(wx.Dialog):		@@ -2967,3 +3113,246 @@ class FillHolesAutoDialog(wx.Dialog):
2967	else:	3113	else:
2968	self.panel3dcon.Enable(1)	3114	self.panel3dcon.Enable(1)
2969	self.panel2dcon.Enable(0)	3115	self.panel2dcon.Enable(0)
		3116	+
		3117	+
		3118	+class ObjectCalibrationDialog(wx.Dialog):
		3119	+
		3120	+ def __init__(self, nav_prop):
		3121	+
		3122	+ self.tracker_id = nav_prop[0]
		3123	+ self.trk_init = nav_prop[1]
		3124	+ self.obj_ref_id = 0
		3125	+ self.obj_name = None
		3126	+
		3127	+ self.obj_fiducials = np.full([5, 3], np.nan)
		3128	+ self.obj_orients = np.full([5, 3], np.nan)
		3129	+
		3130	+ pre = wx.PreDialog()
		3131	+ pre.Create(wx.GetApp().GetTopWindow(), -1, _(u"Object calibration"), size=(450, 440),
		3132	+ style=wx.DEFAULT_DIALOG_STYLE \| wx.FRAME_FLOAT_ON_PARENT)
		3133	+ self.PostCreate(pre)
		3134	+
		3135	+ self._init_gui()
		3136	+ self.LoadObject()
		3137	+
		3138	+ def _init_gui(self):
		3139	+ self.interactor = wxVTKRenderWindowInteractor(self, -1, size=self.GetSize())
		3140	+ self.interactor.Enable(1)
		3141	+ self.ren = vtk.vtkRenderer()
		3142	+ self.interactor.GetRenderWindow().AddRenderer(self.ren)
		3143	+
		3144	+ # Initialize list of buttons and txtctrls for wx objects
		3145	+ self.btns_coord = [None] * 5
		3146	+ self.text_actors = [None] * 5
		3147	+ self.ball_actors = [None] * 5
		3148	+ self.txt_coord = [list(), list(), list(), list(), list()]
		3149	+
		3150	+ # ComboBox for tracker reference mode
		3151	+ tooltip = wx.ToolTip(_(u"Choose the object reference mode"))
		3152	+ choice_ref = wx.ComboBox(self, -1, "", size=wx.Size(90, 23),
		3153	+ choices=const.REF_MODE, style=wx.CB_DROPDOWN \| wx.CB_READONLY)
		3154	+ choice_ref.SetSelection(self.obj_ref_id)
		3155	+ choice_ref.SetToolTip(tooltip)
		3156	+ choice_ref.Bind(wx.EVT_COMBOBOX, self.OnChoiceRefMode)
		3157	+ choice_ref.Enable(0)
		3158	+
		3159	+ # Buttons to finish or cancel object registration
		3160	+ tooltip = wx.ToolTip(_(u"Registration done"))
		3161	+ # btn_ok = wx.Button(self, -1, _(u"Done"), size=wx.Size(90, 30))
		3162	+ btn_ok = wx.Button(self, wx.ID_OK, _(u"Done"), size=wx.Size(90, 30))
		3163	+ btn_ok.SetToolTip(tooltip)
		3164	+
		3165	+ extra_sizer = wx.FlexGridSizer(rows=2, cols=1, hgap=5, vgap=30)
		3166	+ extra_sizer.AddMany([choice_ref,
		3167	+ btn_ok])
		3168	+
		3169	+ # Push buttons for object fiducials
		3170	+ btns_obj = const.BTNS_OBJ
		3171	+ tips_obj = const.TIPS_OBJ
		3172	+
		3173	+ for k in btns_obj:
		3174	+ n = btns_obj[k].keys()[0]
		3175	+ lab = btns_obj[k].values()[0]
		3176	+ self.btns_coord[n] = wx.Button(self, k, label=lab, size=wx.Size(60, 23))
		3177	+ self.btns_coord[n].SetToolTip(wx.ToolTip(tips_obj[n]))
		3178	+ self.btns_coord[n].Bind(wx.EVT_BUTTON, self.OnGetObjectFiducials)
		3179	+
		3180	+ for m in range(0, 5):
		3181	+ for n in range(0, 3):
		3182	+ self.txt_coord[m].append(wx.StaticText(self, -1, label='-',
		3183	+ style=wx.ALIGN_RIGHT, size=wx.Size(40, 23)))
		3184	+
		3185	+ coord_sizer = wx.GridBagSizer(hgap=20, vgap=5)
		3186	+
		3187	+ for m in range(0, 5):
		3188	+ coord_sizer.Add(self.btns_coord[m], pos=wx.GBPosition(m, 0))
		3189	+ for n in range(0, 3):
		3190	+ coord_sizer.Add(self.txt_coord[m][n], pos=wx.GBPosition(m, n + 1), flag=wx.TOP, border=5)
		3191	+
		3192	+ group_sizer = wx.FlexGridSizer(rows=1, cols=2, hgap=50, vgap=5)
		3193	+ group_sizer.AddMany([(coord_sizer, 0, wx.LEFT, 20),
		3194	+ (extra_sizer, 0, wx.LEFT, 10)])
		3195	+
		3196	+ main_sizer = wx.BoxSizer(wx.VERTICAL)
		3197	+ main_sizer.Add(self.interactor, 0, wx.EXPAND)
		3198	+ main_sizer.Add(group_sizer, 0,
		3199	+ wx.EXPAND\|wx.GROW\|wx.LEFT\|wx.TOP\|wx.RIGHT\|wx.BOTTOM\|wx.ALIGN_CENTER_HORIZONTAL, 10)
		3200	+
		3201	+ self.SetSizer(main_sizer)
		3202	+ main_sizer.Fit(self)
		3203	+
		3204	+ def ObjectImportDialog(self):
		3205	+ msg = _("Would like to use InVesalius default object?")
		3206	+ if sys.platform == 'darwin':
		3207	+ dlg = wx.MessageDialog(None, "", msg,
		3208	+ wx.ICON_QUESTION \| wx.YES_NO)
		3209	+ else:
		3210	+ dlg = wx.MessageDialog(None, msg,
		3211	+ "InVesalius 3",
		3212	+ wx.ICON_QUESTION \| wx.YES_NO)
		3213	+ answer = dlg.ShowModal()
		3214	+ dlg.Destroy()
		3215	+
		3216	+ if answer == wx.ID_YES:
		3217	+ return 1
		3218	+ else: # answer == wx.ID_NO:
		3219	+ return 0
		3220	+
		3221	+ def LoadObject(self):
		3222	+ default = self.ObjectImportDialog()
		3223	+ if not default:
		3224	+ filename = ShowImportMeshFilesDialog()
		3225	+
		3226	+ if filename:
		3227	+ if filename.lower().endswith('.stl'):
		3228	+ reader = vtk.vtkSTLReader()
		3229	+ elif filename.lower().endswith('.ply'):
		3230	+ reader = vtk.vtkPLYReader()
		3231	+ elif filename.lower().endswith('.obj'):
		3232	+ reader = vtk.vtkOBJReader()
		3233	+ elif filename.lower().endswith('.vtp'):
		3234	+ reader = vtk.vtkXMLPolyDataReader()
		3235	+ else:
		3236	+ wx.MessageBox(_("File format not reconized by InVesalius"), _("Import surface error"))
		3237	+ return
		3238	+ else:
		3239	+ filename = os.path.join(const.OBJ_DIR, "magstim_fig8_coil.stl")
		3240	+ reader = vtk.vtkSTLReader()
		3241	+ else:
		3242	+ filename = os.path.join(const.OBJ_DIR, "magstim_fig8_coil.stl")
		3243	+ reader = vtk.vtkSTLReader()
		3244	+
		3245	+ if _has_win32api:
		3246	+ self.obj_name = win32api.GetShortPathName(filename).encode(const.FS_ENCODE)
		3247	+ else:
		3248	+ self.obj_name = filename.encode(const.FS_ENCODE)
		3249	+
		3250	+ reader.SetFileName(self.obj_name)
		3251	+ reader.Update()
		3252	+ polydata = reader.GetOutput()
		3253	+
		3254	+ if polydata.GetNumberOfPoints() == 0:
		3255	+ wx.MessageBox(_("InVesalius was not able to import this surface"), _("Import surface error"))
		3256	+
		3257	+ transform = vtk.vtkTransform()
		3258	+ transform.RotateZ(90)
		3259	+
		3260	+ transform_filt = vtk.vtkTransformPolyDataFilter()
		3261	+ transform_filt.SetTransform(transform)
		3262	+ transform_filt.SetInputData(polydata)
		3263	+ transform_filt.Update()
		3264	+
		3265	+ normals = vtk.vtkPolyDataNormals()
		3266	+ normals.SetInputData(transform_filt.GetOutput())
		3267	+ normals.SetFeatureAngle(80)
		3268	+ normals.AutoOrientNormalsOn()
		3269	+ normals.Update()
		3270	+
		3271	+ mapper = vtk.vtkPolyDataMapper()
		3272	+ mapper.SetInputData(normals.GetOutput())
		3273	+ mapper.ScalarVisibilityOff()
		3274	+ mapper.ImmediateModeRenderingOn()
		3275	+
		3276	+ obj_actor = vtk.vtkActor()
		3277	+ obj_actor.SetMapper(mapper)
		3278	+
		3279	+ self.ball_actors[0], self.text_actors[0] = self.OnCreateObjectText('Left', (0,55,0))
		3280	+ self.ball_actors[1], self.text_actors[1] = self.OnCreateObjectText('Right', (0,-55,0))
		3281	+ self.ball_actors[2], self.text_actors[2] = self.OnCreateObjectText('Anterior', (23,0,0))
		3282	+
		3283	+ self.ren.AddActor(obj_actor)
		3284	+ self.ren.ResetCamera()
		3285	+
		3286	+ self.interactor.Render()
		3287	+
		3288	+ def OnCreateObjectText(self, name, coord):
		3289	+ ball_source = vtk.vtkSphereSource()
		3290	+ ball_source.SetRadius(3)
		3291	+ mapper = vtk.vtkPolyDataMapper()
		3292	+ mapper.SetInputConnection(ball_source.GetOutputPort())
		3293	+ ball_actor = vtk.vtkActor()
		3294	+ ball_actor.SetMapper(mapper)
		3295	+ ball_actor.SetPosition(coord)
		3296	+ ball_actor.GetProperty().SetColor(1, 0, 0)
		3297	+
		3298	+ textSource = vtk.vtkVectorText()
		3299	+ textSource.SetText(name)
		3300	+
		3301	+ mapper = vtk.vtkPolyDataMapper()
		3302	+ mapper.SetInputConnection(textSource.GetOutputPort())
		3303	+ tactor = vtk.vtkFollower()
		3304	+ tactor.SetMapper(mapper)
		3305	+ tactor.GetProperty().SetColor(1.0, 0.0, 0.0)
		3306	+ tactor.SetScale(5)
		3307	+ ball_position = ball_actor.GetPosition()
		3308	+ tactor.SetPosition(ball_position[0]+5, ball_position[1]+5, ball_position[2]+10)
		3309	+ self.ren.AddActor(tactor)
		3310	+ tactor.SetCamera(self.ren.GetActiveCamera())
		3311	+ self.ren.AddActor(ball_actor)
		3312	+ return ball_actor, tactor
		3313	+
		3314	+ def OnGetObjectFiducials(self, evt):
		3315	+ btn_id = const.BTNS_OBJ[evt.GetId()].keys()[0]
		3316	+
		3317	+ if self.trk_init and self.tracker_id:
		3318	+ coord_raw = dco.GetCoordinates(self.trk_init, self.tracker_id, self.obj_ref_id)
		3319	+ if self.obj_ref_id and btn_id == 4:
		3320	+ coord = coord_raw[2, :]
		3321	+ else:
		3322	+ coord = coord_raw[0, :]
		3323	+ else:
		3324	+ NavigationTrackerWarning(0, 'choose')
		3325	+
		3326	+ # Update text controls with tracker coordinates
		3327	+ if coord is not None or np.sum(coord) != 0.0:
		3328	+ self.obj_fiducials[btn_id, :] = coord[:3]
		3329	+ self.obj_orients[btn_id, :] = coord[3:]
		3330	+ for n in [0, 1, 2]:
		3331	+ self.txt_coord[btn_id][n].SetLabel(str(round(coord[n], 1)))
		3332	+ if self.text_actors[btn_id]:
		3333	+ self.text_actors[btn_id].GetProperty().SetColor(0.0, 1.0, 0.0)
		3334	+ self.ball_actors[btn_id].GetProperty().SetColor(0.0, 1.0, 0.0)
		3335	+ self.Refresh()
		3336	+ else:
		3337	+ NavigationTrackerWarning(0, 'choose')
		3338	+
		3339	+ def OnChoiceRefMode(self, evt):
		3340	+ # When ref mode is changed the tracker coordinates are set to nan
		3341	+ # This is for Polhemus FASTRAK wrapper, where the sensor attached to the object can be the stylus (Static
		3342	+ # reference - Selection 0 - index 0 for coordinates) or can be a 3rd sensor (Dynamic reference - Selection 1 -
		3343	+ # index 2 for coordinates)
		3344	+ # I use the index 2 directly here to send to the coregistration module where it is possible to access without
		3345	+ # any conditional statement the correct index of coordinates.
		3346	+
		3347	+ if evt.GetSelection():
		3348	+ self.obj_ref_id = 2
		3349	+ else:
		3350	+ self.obj_ref_id = 0
		3351	+ for m in range(0, 5):
		3352	+ self.obj_fiducials[m, :] = np.full([1, 3], np.nan)
		3353	+ self.obj_orients[m, :] = np.full([1, 3], np.nan)
		3354	+ for n in range(0, 3):
		3355	+ self.txt_coord[m][n].SetLabel('-')
		3356	+
		3357	+ def GetValue(self):
		3358	+ return self.obj_fiducials, self.obj_orients, self.obj_ref_id, self.obj_name
	@@ -707,7 +707,7 @@ class MenuBar(wx.MenuBar):		@@ -707,7 +707,7 @@ class MenuBar(wx.MenuBar):
707	sub(self.OnEnableState, "Enable state project")	707	sub(self.OnEnableState, "Enable state project")
708	sub(self.OnEnableUndo, "Enable undo")	708	sub(self.OnEnableUndo, "Enable undo")
709	sub(self.OnEnableRedo, "Enable redo")	709	sub(self.OnEnableRedo, "Enable redo")
710	- sub(self.OnEnableNavigation, "Navigation Status")	710	+ sub(self.OnEnableNavigation, "Navigation status")
711		711
712	sub(self.OnAddMask, "Add mask")	712	sub(self.OnAddMask, "Add mask")
713	sub(self.OnRemoveMasks, "Remove masks")	713	sub(self.OnRemoveMasks, "Remove masks")