Merge branch 'master' into multimodal_tracking

# Conflicts: # invesalius/constants.py # invesalius/data/coordinates.py # invesalius/gui/dialogs.py # invesalius/gui/task_navigator.py

Merge branch 'master' into multimodal_tracking
# Conflicts: # invesalius/constants.py # invesalius/data/coordinates.py # invesalius/gui/dialogs.py # invesalius/gui/task_navigator.py
Renan
2 parents 329c784e 490d87a8
Showing 22 changed files with 1783 additions and 1196 deletions Show diff stats
.gitignore
app.py
environment.yml
invesalius/constants.py
invesalius/control.py
invesalius/data/bases.py
invesalius/data/converters.py
invesalius/data/coordinates.py
invesalius/data/coregistration.py
invesalius/data/imagedata_utils.py
invesalius/data/serial_port_connection.py
invesalius/data/styles.py
invesalius/data/trigger.py
invesalius/data/viewer_volume.py
invesalius/gui/dialogs.py
invesalius/gui/task_navigator.py
invesalius/navigation/icp.py
invesalius/navigation/navigation.py
invesalius/navigation/tracker.py
invesalius/net/pedal_connection.py
-invesalius/*.classpath
-invesalius/*.hg
-invesalius/*.log
-invesalius/*.project
-invesalius/*.pyc
-invesalius/*.swn
-invesalius/*.swo
-invesalius/*.swp
-invesalius/data/*.log
-invesalius/data/*.pyc
-invesalius/data/*.pyd
-invesalius/gui/*.log
-invesalius/gui/*.pyc
-invesalius/gui/widgets/*.log
-invesalius/gui/widgets/*.pyc
-invesalius/reader/*.log
-invesalius/reader/*.pyc
-
-.idea
-
-*.pyc
-*.swp
+# Created by https://www.toptal.com/developers/gitignore/api/vim,python,intellij,visualstudiocode,direnv
+# Edit at https://www.toptal.com/developers/gitignore?templates=vim,python,intellij,visualstudiocode,direnv
+
+### direnv ###
+.direnv
+.envrc
+
+### Intellij ###
+# Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio, WebStorm and Rider
+# Reference: https://intellij-support.jetbrains.com/hc/en-us/articles/206544839
+.idea/
+
+# CMake
+cmake-build-*/
+
+# File-based project format
+*.iws
+
+# IntelliJ
+out/
+
+# mpeltonen/sbt-idea plugin
+.idea_modules/
+
+# JIRA plugin
+atlassian-ide-plugin.xml
+
+# Crashlytics plugin (for Android Studio and IntelliJ)
+com_crashlytics_export_strings.xml
+crashlytics.properties
+crashlytics-build.properties
+fabric.properties
+
+### Python ###
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
 *.so
-tags
-*.c
  
-.idea
-build
-*.patch
-*.tgz
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+share/python-wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
  
-*.pyd
-*.cpp
-*.diff
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.nox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+*.py,cover
+.hypothesis/
+.pytest_cache/
+cover/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+db.sqlite3
+db.sqlite3-journal
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+.pybuilder/
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# IPython
+profile_default/
+ipython_config.py
+
+# pyenv
+#   For a library or package, you might want to ignore these files since the code is
+#   intended to run in multiple environments; otherwise, check them in:
+# .python-version
+
+# pipenv
+#   According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
+#   However, in case of collaboration, if having platform-specific dependencies or dependencies
+#   having no cross-platform support, pipenv may install dependencies that don't work, or not
+#   install all needed dependencies.
+#Pipfile.lock
+
+# PEP 582; used by e.g. github.com/David-OConnor/pyflow
+__pypackages__/
+
+# Celery stuff
+celerybeat-schedule
+celerybeat.pid
+
+# SageMath parsed files
+*.sage.py
  
-*.directory
+# Environments
+.env
+.venv
+env/
+venv/
+ENV/
+env.bak/
+venv.bak/
+myenv/
  
+# Spyder project settings
+.spyderproject
+.spyproject
  
-# latex
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
+.dmypy.json
+dmypy.json
+
+# Pyre type checker
+.pyre/
+
+# pytype static type analyzer
+.pytype/
+
+# Cython debug symbols
+cython_debug/
+
+### Vim ###
+# Swap
+[._]*.s[a-v][a-z]
+!*.svg  # comment out if you don't need vector files
+[._]*.sw[a-p]
+[._]s[a-rt-v][a-z]
+[._]ss[a-gi-z]
+[._]sw[a-p]
+
+# Session
+Session.vim
+Sessionx.vim
+
+# Temporary
+.netrwhist
+*~
+# Auto-generated tag files
+tags
+# Persistent undo
+[._]*.un~
+
+### VisualStudioCode ###
+.vscode/
+*.code-workspace
+
+# Local History for Visual Studio Code
+.history/
+
+### VisualStudioCode Patch ###
+# Ignore all local history of files
+.history
+.ionide
+
+### LaTeX ###
+## Core latex/pdflatex auxiliary files:
 *.aux
+*.lof
+*.log
+*.lot
+*.fls
+*.out
 *.toc
+*.fmt
+*.fot
+*.cb
+*.cb2
+.*.lb
+
+## Intermediate documents:
+*.dvi
+*.xdv
+*-converted-to.*
+# these rules might exclude image files for figures etc.
+# *.ps
+# *.eps
+# *.pdf
+
+## Generated if empty string is given at "Please type another file name for output:"
+.pdf
+
+## Bibliography auxiliary files (bibtex/biblatex/biber):
 *.bbl
+*.bcf
 *.blg
-*.fls
+*-blx.aux
+*-blx.bib
+*.run.xml
+
+## Build tool auxiliary files:
 *.fdb_latexmk
+*.synctex
+*.synctex(busy)
 *.synctex.gz
-*.out
-*.log
+*.synctex.gz(busy)
+*.pdfsync
+
+# End of https://www.toptal.com/developers/gitignore/api/vim,python,intellij,visualstudiocode,direnv
+
+### Cython generated files ###
+*.c
+*.cpp
+
+### Patches and diffs ###
+*.patch
+*.diff
@@ -209,9 +209,9 @@ class Inv3SplashScreen(SplashScreen):
  
             else:
  
-                path = os.path.join(".","icons", icon_file)
+                path = os.path.join(inv_paths.ICON_DIR, icon_file)
                 if not os.path.exists(path):
-                    path = os.path.join(".", "icons", "splash_en.png")
+                    path = os.path.join(inv_paths.ICON_DIR, "splash_en.png")
  
             bmp = wx.Image(path).ConvertToBitmap()
  
@@ -3,25 +3,23 @@ channels:
   - bioconda
 dependencies:
   - python=3.7
-  - cython==0.29.22
-  - pillow==8.1.1
-  - wxpython==4.1.0
+  - cython==0.29.24
+  - pillow==8.3.2
   - pypubsub==4.0.3
   - configparser==5.0.1
   - h5py==2.10.0
   - imageio==2.9.0
   - nibabel==3.2.1
-  - numpy==1.20.1
-  - pooch==1.4.0
+  - numpy==1.21.2
   - psutil==5.8.0
   - pyserial==3.5
-  - scikit-image==0.18.1
-  - scipy==1.6.1
-  - vtk==9.0.1
-  - wxpython==4.1.0
+  - scikit-image==0.18.3
+  - scipy==1.7.1
+  - vtk==9.0.3
+  - wxpython==4.1.1
   - pip
   - pip:
-    - python-gdcm==3.0.8.1
+    - python-gdcm==3.0.9.1
     - plaidml-keras==0.7.0
     - theano==1.0.5
     - pyacvd==0.2.3
@@ -659,6 +659,8 @@ BOOLEAN_XOR = 4
  
 MARKER_COLOUR = (1.0, 1.0, 0.)
 MARKER_SIZE = 2
+
+CALIBRATION_TRACKER_SAMPLES = 10
 FIDUCIAL_REGISTRATION_ERROR_THRESHOLD = 3.0
  
 SELECT = 0
@@ -760,21 +762,42 @@ 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")]
+OBJECT_FIDUCIALS = [
+    {
+        'fiducial_index': 0,
+        'button_id': OBJL,
+        'label': _('Left'),
+        'tip': _("Select left object fiducial"),
+    },
+    {
+        'fiducial_index': 1,
+        'button_id': OBJR,
+        'label': _('Right'),
+        'tip': _("Select right object fiducial"),
+    },
+    {
+        'fiducial_index': 2,
+        'button_id': OBJA,
+        'label': _('Anterior'),
+        'tip': _("Select anterior object fiducial"),
+    },
+    {
+        'fiducial_index': 3,
+        'button_id': OBJC,
+        'label': _('Center'),
+        'tip': _("Select object center"),
+    },
+    {
+        'fiducial_index': 4,
+        'button_id': OBJF,
+        'label': _('Fixed'),
+        'tip': _("Attach sensor to object"),
+    },
+]
  
 MTC_PROBE_NAME = "1Probe"
 MTC_REF_NAME = "2Ref"
-MTC_OBJ_NAME = "3bigcoil"
+MTC_OBJ_NAME = "3Coil"
  
 # Object tracking
 ARROW_SCALE = 6
@@ -805,8 +828,9 @@ TREKKER_CONFIG = {&#39;seed_max&#39;: 1, &#39;step_size&#39;: 0.1, &#39;min_fod&#39;: 0.1, &#39;probe_qualit
                   'write_interval': 50, 'numb_threads': '', 'max_lenth': 200,
                   'min_lenth': 20, 'max_sampling_step': 100}
  
-WILDCARD_MARKER_FILES = _("Marker scanner coord files (*.mkss)|*.mkss") + "|" +\
-                        _("Marker files (*.mks)|*.mks")
+MARKER_FILE_MAGICK_STRING = "INVESALIUS3_MARKER_FILE_"
+CURRENT_MARKER_FILE_VERSION = 0
+WILDCARD_MARKER_FILES = _("Marker scanner coord files (*.mkss)|*.mkss")
  
 ROBOT_ElFIN_IP = ['Select robot IP:', '143.107.220.251', '169.254.153.251', '127.0.0.1']
 ROBOT_ElFIN_PORT = 10003
@@ -915,6 +915,16 @@ class Controller():
                 matrix, matrix_filename = self.OpenOtherFiles(group)
                 self.CreateOtherProject(name, matrix, matrix_filename)
                 self.LoadProject()
+                if group.affine.any():
+                    # TODO: replace the inverse of the affine by the actual affine in the whole code
+                    # remove scaling factor for non-unitary voxel dimensions
+                    # self.affine = image_utils.world2invspace(affine=group.affine)
+                    scale, shear, angs, trans, persp = tr.decompose_matrix(group.affine)
+                    self.affine = np.linalg.inv(tr.compose_matrix(scale=None, shear=shear,
+                                                                angles=angs, translate=trans, perspective=persp))
+                    # print("repos_img: {}".format(repos_img))
+                    self.Slice.affine = self.affine
+                    Publisher.sendMessage('Update affine matrix', affine=self.affine)
                 Publisher.sendMessage("Enable state project", state=True)
             else:
                 dialog.ImportInvalidFiles(ftype="Others")
@@ -1043,17 +1053,6 @@ class Controller():
         self.Slice.window_level = wl
         self.Slice.window_width = ww
  
-        if group.affine.any():
-            # TODO: replace the inverse of the affine by the actual affine in the whole code
-            # remove scaling factor for non-unitary voxel dimensions
-            # self.affine = image_utils.world2invspace(affine=group.affine)
-            scale, shear, angs, trans, persp = tr.decompose_matrix(group.affine)
-            self.affine = np.linalg.inv(tr.compose_matrix(scale=None, shear=shear,
-                                                          angles=angs, translate=trans, perspective=persp))
-            # print("repos_img: {}".format(repos_img))
-            self.Slice.affine = self.affine
-            Publisher.sendMessage('Update affine matrix', affine=self.affine)
-
         scalar_range = int(scalar_range[0]), int(scalar_range[1])
         Publisher.sendMessage('Update threshold limits list',
                               threshold_range=scalar_range)
@@ -188,10 +188,18 @@ def object_registration(fiducials, orients, coord_raw, m_change):
         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 = tr.translation_matrix(coords[3, :3])
-    r_s0_raw = tr.euler_matrix(np.radians(coords[3, 3]), np.radians(coords[3, 4]),
-                             np.radians(coords[3, 5]), 'rzyx')
-    s0_raw = tr.concatenate_matrices(t_s0_raw, r_s0_raw)
+
+    # XXX: Some duplicate processing is done here: the Euler angles are calculated once by
+    #      the lines below, and then again in dco.coordinates_to_transformation_matrix.
+    #
+    a, b, g = np.radians(coords[3, 3:])
+    r_s0_raw = tr.euler_matrix(a, b, g, axes='rzyx')
+
+    s0_raw = dco.coordinates_to_transformation_matrix(
+        position=coords[3, :3],
+        orientation=coords[3, 3:],
+        axes='rzyx',
+    )
  
     # compute change of basis for object fiducials in source frame
     base_obj_raw, q_obj_raw = base_creation(fids_raw[:3, :3])
@@ -210,10 +218,11 @@ def object_registration(fiducials, orients, coord_raw, m_change):
         fids_dyn[ic, 2] = -fids_dyn[ic, 2]
  
         # 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 = tr.concatenate_matrices(T_p, R_p)
+        M_p = dco.coordinates_to_transformation_matrix(
+            position=fids_dyn[ic, :3],
+            orientation=fids_dyn[ic, 3:],
+            axes='rzyx',
+        )
         M_img = m_change @ M_p
  
         angles_img = np.degrees(np.asarray(tr.euler_from_matrix(M_img, 'rzyx')))
@@ -228,10 +237,11 @@ def object_registration(fiducials, orients, coord_raw, m_change):
     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 = tr.translation_matrix(fids_dyn[3, :3])
-    s0_rot_dyn = tr.euler_matrix(np.radians(fids_dyn[3, 3]), np.radians(fids_dyn[3, 4]),
-                                             np.radians(fids_dyn[3, 5]), 'rzyx')
-    s0_dyn = tr.concatenate_matrices(s0_trans_dyn, s0_rot_dyn)
+    s0_dyn = dco.coordinates_to_transformation_matrix(
+        position=fids_dyn[3, :3],
+        orientation=fids_dyn[3, 3:],
+        axes='rzyx',
+    )
  
     return t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img
  
@@ -149,6 +149,7 @@ def np_rgba_to_vtk(n_array, spacing=(1.0, 1.0, 1.0)):
 # Based on http://gdcm.sourceforge.net/html/ConvertNumpy_8py-example.html
 def gdcm_to_numpy(image, apply_intercep_scale=True):
     map_gdcm_np = {
+        gdcm.PixelFormat.SINGLEBIT: np.uint8,
         gdcm.PixelFormat.UINT8: np.uint8,
         gdcm.PixelFormat.INT8: np.int8,
         gdcm.PixelFormat.UINT12: np.uint16,
@@ -177,6 +178,8 @@ def gdcm_to_numpy(image, apply_intercep_scale=True):
     np_array = np.frombuffer(
         gdcm_array.encode("utf-8", errors="surrogateescape"), dtype=dtype
     )
+    if pf.GetScalarType() == gdcm.PixelFormat.SINGLEBIT:
+        np_array = np.unpackbits(np_array)
     np_array.shape = shape
     np_array = np_array.squeeze()
  
@@ -39,9 +39,9 @@ class TrackerCoordinates():
     def SetCoordinates(self, coord, markers_flag):
         self.coord = coord
         self.markers_flag = markers_flag
+        wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag)
  
     def GetCoordinates(self):
-        wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag)
         return self.coord, self.markers_flag
  
  
@@ -179,7 +179,7 @@ def PolarisCoord(trck_init, trck_id, ref_mode):
  
     coord = np.vstack([coord1, coord2, coord3])
  
-    return coord, [trck.probeID, trck.refID, trck.coilID]
+    return coord, [trck.probeID, trck.refID, trck.objID]
  
 def ElfinCoord(trck_init):
     if len(trck_init) > 2:
@@ -384,11 +384,45 @@ def DebugCoordRandom(trk_init, trck_id, ref_mode):
     # coord4 = np.array([uniform(1, 200), uniform(1, 200), uniform(1, 200),
     #                    uniform(-180.0, 180.0), uniform(-180.0, 180.0), uniform(-180.0, 180.0)])
  
-    #Publisher.sendMessage('Sensors ID', probe_id=int(uniform(0, 5)), ref_id=int(uniform(0, 5)), obj_id=int(uniform(0, 5)))
-
     return np.vstack([coord1, coord2, coord3, coord4]), [int(uniform(0, 5)), int(uniform(0, 5)), int(uniform(0, 5))]
  
  
+def coordinates_to_transformation_matrix(position, orientation, axes='sxyz'):
+    """
+    Transform vectors consisting of position and orientation (in Euler angles) in 3d-space into a 4x4
+    transformation matrix that combines the rotation and translation.
+    :param position: A vector of three coordinates.
+    :param orientation: A vector of three Euler angles in degrees.
+    :param axes: The order in which the rotations are done for the axes. See transformations.py for details. Defaults to 'sxyz'.
+    :return: The transformation matrix (4x4).
+    """
+    a, b, g = np.radians(orientation)
+
+    r_ref = tr.euler_matrix(a, b, g, axes=axes)
+    t_ref = tr.translation_matrix(position)
+
+    m_img = tr.concatenate_matrices(t_ref, r_ref)
+
+    return m_img
+
+
+def transformation_matrix_to_coordinates(matrix, axes='sxyz'):
+    """
+    Given a matrix that combines the rotation and translation, return the position and the orientation
+    determined by the matrix. The orientation is given as three Euler angles.
+    The inverse of coordinates_of_transformation_matrix when the parameter 'axes' matches.
+    :param matrix: A 4x4 transformation matrix.
+    :param axes: The order in which the rotations are done for the axes. See transformations.py for details. Defaults to 'sxyz'.
+    :return: The position (a vector of length 3) and Euler angles for the orientation in degrees (a vector of length 3).
+    """
+    angles = tr.euler_from_matrix(matrix, axes=axes)
+    angles_as_deg = np.degrees(angles)
+
+    translation = tr.translation_from_matrix(matrix)
+
+    return translation, angles_as_deg
+
+
 def dynamic_reference(probe, reference):
     """
     Apply dynamic reference correction to probe coordinates. Uses the alpha, beta and gama
@@ -435,11 +469,11 @@ def dynamic_reference_m(probe, reference):
     :param reference: sensor two defined as reference
     :return: rotated and translated coordinates
     """
-    a, b, g = np.radians(reference[3:6])
-
-    trans = tr.translation_matrix(reference[:3])
-    rot = tr.euler_matrix(a, b, g, 'rzyx')
-    affine = tr.concatenate_matrices(trans, rot)
+    affine = coordinates_to_transformation_matrix(
+        position=reference[:3],
+        orientation=reference[3:],
+        axes='rzyx',
+    )
     probe_4 = np.vstack((probe[:3].reshape([3, 1]), 1.))
     coord_rot = np.linalg.inv(affine) @ probe_4
     # minus sign to the z coordinate
@@ -479,15 +513,16 @@ def dynamic_reference_m2(probe, 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 = tr.concatenate_matrices(T, R)
-    M_p = tr.concatenate_matrices(T_p, R_p)
+    M = coordinates_to_transformation_matrix(
+        position=reference[:3],
+        orientation=reference[3:],
+        axes='rzyx',
+    )
+    M_p = coordinates_to_transformation_matrix(
+        position=probe[:3],
+        orientation=probe[3:],
+        axes='rzyx',
+    )
  
     M_dyn = np.linalg.inv(M) @ M_p
  
@@ -69,12 +69,11 @@ def object_to_reference(coord_raw, m_probe):
     :return: 4 x 4 numpy double array
     :rtype: numpy.ndarray
     """
-
-    a, b, g = np.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 = tr.concatenate_matrices(t_ref, r_ref)
-
+    m_ref = dco.coordinates_to_transformation_matrix(
+        position=coord_raw[1, :3],
+        orientation=coord_raw[1, 3:],
+        axes='rzyx',
+    )
     m_dyn = np.linalg.inv(m_ref) @ m_probe
     return m_dyn
  
@@ -108,20 +107,25 @@ def corregistrate_object_dynamic(inp, coord_raw, ref_mode_id, icp):
  
     # transform raw marker coordinate to object center
     m_probe = object_marker_to_center(coord_raw, obj_ref_mode, t_obj_raw, s0_raw, r_s0_raw)
+
     # transform object center to reference marker if specified as dynamic reference
     if ref_mode_id:
         m_probe_ref = object_to_reference(coord_raw, m_probe)
     else:
         m_probe_ref = m_probe
+
     # invert y coordinate
     m_probe_ref[2, -1] = -m_probe_ref[2, -1]
+
     # corregistrate from tracker to image space
     m_img = tracker_to_image(m_change, m_probe_ref, r_obj_img, m_obj_raw, s0_dyn)
     if icp[0]:
         m_img = bases.transform_icp(m_img, icp[1])
+
     # compute rotation angles
-    _, _, angles, _, _ = tr.decompose_matrix(m_img)
-    # create output coordiante list
+    angles = tr.euler_from_matrix(m_img, axes='sxyz')
+
+    # create output coordinate list
     coord = m_img[0, -1], m_img[1, -1], m_img[2, -1], \
             np.degrees(angles[0]), np.degrees(angles[1]), np.degrees(angles[2])
  
@@ -132,10 +136,11 @@ def UpdateICP(self, m_icp, flag):
     self.icp = flag
  
 def compute_marker_transformation(coord_raw, obj_ref_mode):
-    psi, theta, phi = np.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 = tr.concatenate_matrices(t_probe, r_probe)
+    m_probe = dco.coordinates_to_transformation_matrix(
+        position=coord_raw[obj_ref_mode, :3],
+        orientation=coord_raw[obj_ref_mode, 3:],
+        axes='rzyx',
+    )
     return m_probe
  
  
@@ -145,6 +150,7 @@ def corregistrate_dynamic(inp, coord_raw, ref_mode_id, icp):
  
     # transform raw marker coordinate to object center
     m_probe = compute_marker_transformation(coord_raw, obj_ref_mode)
+
     # transform object center to reference marker if specified as dynamic reference
     if ref_mode_id:
         m_ref = compute_marker_transformation(coord_raw, 1)
@@ -154,6 +160,7 @@ def corregistrate_dynamic(inp, coord_raw, ref_mode_id, icp):
  
     # invert y coordinate
     m_probe_ref[2, -1] = -m_probe_ref[2, -1]
+
     # corregistrate from tracker to image space
     m_img = m_change @ m_probe_ref
  
@@ -161,8 +168,9 @@ def corregistrate_dynamic(inp, coord_raw, ref_mode_id, icp):
         m_img = bases.transform_icp(m_img, icp[1])
  
     # compute rotation angles
-    _, _, angles, _, _ = tr.decompose_matrix(m_img)
-    # create output coordiante list
+    angles = tr.euler_from_matrix(m_img, axes='sxyz')
+
+    # create output coordinate list
     coord = m_img[0, -1], m_img[1, -1], m_img[2, -1],\
             np.degrees(angles[0]), np.degrees(angles[1]), np.degrees(angles[2])
  
@@ -204,7 +212,7 @@ class CoordinateCorregistrate(threading.Thread):
         coreg_data = self.coreg_data
         view_obj = 1
  
-        trck_init, trck_id, trck_mode = self.tracker.GetTrackerInfo()
+        trck_init, trck_id = self.tracker.GetTrackerInfo()
  
         # print('CoordCoreg: event {}'.format(self.event.is_set()))
         while not self.event.is_set():
@@ -284,7 +292,7 @@ class CoordinateCorregistrateNoObject(threading.Thread):
         coreg_data = self.coreg_data
         view_obj = 0
  
-        trck_init, trck_id, trck_mode = self.tracker.GetTrackerInfo()
+        trck_init, trck_id = self.tracker.GetTrackerInfo()
         # print('CoordCoreg: event {}'.format(self.event.is_set()))
         while not self.event.is_set():
             try:
@@ -34,6 +34,7 @@ from vtk.util import numpy_support
  
 import invesalius.constants as const
 import invesalius.data.converters as converters
+import invesalius.data.coordinates as dco
 import invesalius.data.slice_ as sl
 import invesalius.data.transformations as tr
 import invesalius.reader.bitmap_reader as bitmap_reader
@@ -555,18 +556,23 @@ def image_normalize(image, min_=0.0, max_=1.0, output_dtype=np.int16):
     return output
  
  
+# TODO: Add a description of different coordinate systems, namely:
+#       - the world coordinate system,
+#       - the voxel coordinate system.
+#       - InVesalius's internal coordinate system,
+#
 def convert_world_to_voxel(xyz, affine):
     """
     Convert a coordinate from the world space ((x, y, z); scanner space; millimeters) to the
     voxel space ((i, j, k)). This is achieved by multiplying a coordinate by the inverse
     of the affine transformation.
+
     More information: https://nipy.org/nibabel/coordinate_systems.html
+
     :param xyz: a list or array of 3 coordinates (x, y, z) in the world coordinates
     :param affine: a 4x4 array containing the image affine transformation in homogeneous coordinates
     :return: a 1x3 array with the point coordinates in image space (i, j, k)
     """
-
-    # print("xyz: ", xyz, "\naffine", affine)
     # convert xyz coordinate to 1x4 homogeneous coordinates array
     xyz_homo = np.hstack((xyz, 1.0)).reshape([4, 1])
     ijk_homo = np.linalg.inv(affine) @ xyz_homo
@@ -575,6 +581,65 @@ def convert_world_to_voxel(xyz, affine):
     return ijk
  
  
+def convert_invesalius_to_voxel(position):
+    """
+    Convert position from InVesalius space to the voxel space.
+
+    The two spaces are otherwise identical, but InVesalius space has a reverted y-axis
+    (increasing y-coordinate moves posterior in InVesalius space, but anterior in the voxel space).
+
+    For instance, if the size of the voxel image is 256 x 256 x 160, the y-coordinate 0 in
+    InVesalius space corresponds to the y-coordinate 255 in the voxel space.
+
+    :param position: a vector of 3 coordinates (x, y, z) in InVesalius space.
+    :return: a vector of 3 coordinates in the voxel space
+    """
+    slice = sl.Slice()
+    return np.array((position[0], slice.matrix.shape[1] - position[1] - 1, position[2]))
+
+
+def convert_invesalius_to_world(position, orientation):
+    """
+    Convert position and orientation from InVesalius space to the world space.
+
+    The axis definition for the Euler angles returned is 'sxyz', see transformations.py for more
+    information.
+
+    Uses 'affine' matrix defined in the project created or opened by the user. If it is
+    undefined, return Nones as the coordinates for both position and orientation.
+
+    More information: https://nipy.org/nibabel/coordinate_systems.html
+
+    :param position: a vector of 3 coordinates in InVesalius space.
+    :param orientation: a vector of 3 Euler angles in InVesalius space.
+    :return: a pair consisting of 3 coordinates and 3 Euler angles in the world space, or Nones if
+             'affine' matrix is not defined in the project.
+    """
+    slice = sl.Slice()
+
+    if slice.affine is None:
+        position_world = (None, None, None)
+        orientation_world = (None, None, None)
+
+        return position_world, orientation_world
+
+    position_voxel = convert_invesalius_to_voxel(position)
+
+    M_invesalius = dco.coordinates_to_transformation_matrix(
+        position=position_voxel,
+        orientation=orientation,
+        axes='sxyz',
+    )
+    M_world = np.linalg.inv(slice.affine) @ M_invesalius
+
+    position_world, orientation_world = dco.transformation_matrix_to_coordinates(
+        M_world,
+        axes='sxyz',
+    )
+
+    return position_world, orientation_world
+
+
 def create_grid(xy_range, z_range, z_offset, spacing):
     x = np.arange(xy_range[0], xy_range[1] + 1, spacing)
     y = np.arange(xy_range[0], xy_range[1] + 1, spacing)
@@ -0,0 +1,106 @@
+#--------------------------------------------------------------------------
+# 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 queue
+import threading
+import time
+
+import wx
+from invesalius.pubsub import pub as Publisher
+
+
+class SerialPortConnection(threading.Thread):
+    BINARY_PULSE = b'\x01'
+
+    def __init__(self, port, serial_port_queue, event, sleep_nav):
+        """
+        Thread created to communicate using the serial port to interact with software during neuronavigation.
+        """
+        threading.Thread.__init__(self, name='Serial port')
+
+        self.connection = None
+        self.stylusplh = False
+
+        self.port = port
+        self.serial_port_queue = serial_port_queue
+        self.event = event
+        self.sleep_nav = sleep_nav
+
+    def Connect(self):
+        if self.port is None:
+            print("Serial port init error: COM port is unset.")
+            return
+        try:
+            import serial
+            self.connection = serial.Serial(self.port, baudrate=115200, timeout=0)
+            print("Connection to port {} opened.".format(self.port))
+
+            Publisher.sendMessage('Serial port connection', state=True)
+        except:
+            print("Serial port init error: Connecting to port {} failed.".format(self.port))
+
+    def Disconnect(self):
+        if self.connection:
+            self.connection.close()
+            print("Connection to port {} closed.".format(self.port))
+
+            Publisher.sendMessage('Serial port connection', state=False)
+
+    def SendPulse(self):
+        try:
+            self.connection.write(self.BINARY_PULSE)
+            Publisher.sendMessage('Serial port pulse triggered')
+        except:
+            print("Error: Serial port could not be written into.")
+
+    def run(self):
+        while not self.event.is_set():
+            trigger_on = False
+            try:
+                lines = self.connection.readlines()
+            except:
+                print("Error: Serial port could not be read.")
+
+            if lines:
+                trigger_on = True
+
+            if self.stylusplh:
+                trigger_on = True
+                self.stylusplh = False
+
+            try:
+                self.serial_port_queue.put_nowait(trigger_on)
+            except queue.Full:
+                print("Error: Serial port queue full.")
+
+            time.sleep(self.sleep_nav)
+
+            # XXX: This is needed here because the serial port queue has to be read
+            #      at least as fast as it is written into, otherwise it will eventually
+            #      become full. Reading is done in another thread, which has the same
+            #      sleeping parameter sleep_nav between consecutive runs as this thread.
+            #      However, a single run of that thread takes longer to finish than a
+            #      single run of this thread, causing that thread to lag behind. Hence,
+            #      the additional sleeping here to ensure that this thread lags behind the
+            #      other thread and not the other way around. However, it would be nice to
+            #      handle the timing dependencies between the threads in a more robust way.
+            #
+            time.sleep(0.3)
+        else:
+            self.Disconnect()
@@ -2600,6 +2600,7 @@ class SelectMaskPartsInteractorStyle(DefaultInteractorStyle):
 class FFillSegmentationConfig(metaclass=utils.Singleton):
     def __init__(self):
         self.dlg_visible = False
+        self.dlg = None
         self.target = "2D"
         self.con_2d = 4
         self.con_3d = 6
@@ -2634,7 +2635,6 @@ class FloodFillSegmentInteractorStyle(DefaultInteractorStyle):
         self.slice_data = viewer.slice_data
  
         self.config = FFillSegmentationConfig()
-        self.dlg_ffill = None
  
         self._progr_title = _(u"Region growing")
         self._progr_msg = _(u"Segmenting ...")
@@ -2652,14 +2652,14 @@ class FloodFillSegmentInteractorStyle(DefaultInteractorStyle):
                 self.config.t1 = int(_max)
  
             self.config.dlg_visible = True
-            self.dlg_ffill = dialogs.FFillSegmentationOptionsDialog(self.config)
-            self.dlg_ffill.Show()
+            self.config.dlg = dialogs.FFillSegmentationOptionsDialog(self.config)
+            self.config.dlg.Show()
  
     def CleanUp(self):
-        if (self.dlg_ffill is not None) and (self.config.dlg_visible):
+        if (self.config.dlg is not None) and (self.config.dlg_visible):
             self.config.dlg_visible = False
-            self.dlg_ffill.Destroy()
-            self.dlg_ffill = None
+            self.config.dlg.Destroy()
+            self.config.dlg = None
  
     def OnFFClick(self, obj, evt):
         if (self.viewer.slice_.buffer_slices[self.orientation].mask is None):
@@ -2787,22 +2787,28 @@ class FloodFillSegmentInteractorStyle(DefaultInteractorStyle):
             with futures.ThreadPoolExecutor(max_workers=1) as executor:
                 future = executor.submit(self.do_rg_confidence, image, mask, (x, y, z), bstruct)
  
-                dlg = wx.ProgressDialog(self._progr_title, self._progr_msg, parent=wx.GetApp().GetTopWindow(), style=wx.PD_APP_MODAL|wx.PD_AUTO_HIDE)
+                self.config.dlg.panel_ffill_progress.Enable()
+                self.config.dlg.panel_ffill_progress.StartTimer()
                 while not future.done():
-                    dlg.Pulse()
+                    self.config.dlg.panel_ffill_progress.Pulse()
+                    self.config.dlg.Update()
                     time.sleep(0.1)
-                dlg.Destroy()
+                self.config.dlg.panel_ffill_progress.StopTimer()
+                self.config.dlg.panel_ffill_progress.Disable()
                 out_mask = future.result()
         else:
             out_mask = np.zeros_like(mask)
             with futures.ThreadPoolExecutor(max_workers=1) as executor:
                 future = executor.submit(floodfill.floodfill_threshold, image, [[x, y, z]], t0, t1, 1, bstruct, out_mask)
  
-                dlg = wx.ProgressDialog(self._progr_title, self._progr_msg, parent=wx.GetApp().GetTopWindow(), style=wx.PD_APP_MODAL|wx.PD_AUTO_HIDE)
+                self.config.dlg.panel_ffill_progress.Enable()
+                self.config.dlg.panel_ffill_progress.StartTimer()
                 while not future.done():
-                    dlg.Pulse()
+                    self.config.dlg.panel_ffill_progress.Pulse()
+                    self.config.dlg.Update()
                     time.sleep(0.1)
-                dlg.Destroy()
+                self.config.dlg.panel_ffill_progress.StopTimer()
+                self.config.dlg.panel_ffill_progress.Disable()
  
         mask[out_mask.astype('bool')] = self.config.fill_value
  
@@ -1,168 +0,0 @@
-#--------------------------------------------------------------------------
-# 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
-from time import sleep
-
-import wx
-from invesalius.pubsub import pub as Publisher
-
-
-class Trigger(threading.Thread):
-    """
-    Thread created to use external trigger to interact with software during neuronavigation
-    """
-
-    def __init__(self, nav_id):
-        threading.Thread.__init__(self)
-        self.trigger_init = None
-        self.stylusplh = False
-        self.COM = False
-        self.nav_id = nav_id
-        self.__bind_events()
-        try:
-            import serial
-
-            self.trigger_init = serial.Serial('COM5', baudrate=115200, timeout=0)
-            self.COM = True
-
-        except:
-            #wx.MessageBox(_('Connection with port COM1 failed'), _('Communication error'), wx.OK | wx.ICON_ERROR)
-            print("Trigger init error: Connection with port COM1 failed")
-            self.COM = False
-
-        self._pause_ = False
-        self.start()
-
-    def __bind_events(self):
-        Publisher.subscribe(self.OnStylusPLH, 'PLH Stylus Button On')
-
-    def OnStylusPLH(self):
-        self.stylusplh = True
-
-    def stop(self):
-        self._pause_ = True
-
-    def run(self):
-        while self.nav_id:
-            if self.COM:
-                self.trigger_init.write(b'0')
-                sleep(0.3)
-                lines = self.trigger_init.readlines()
-                # Following lines can simulate a trigger in 3 sec repetitions
-                # sleep(3)
-                # lines = True
-                if lines:
-                    wx.CallAfter(Publisher.sendMessage, 'Create marker')
-                    sleep(0.5)
-
-            if self.stylusplh:
-                wx.CallAfter(Publisher.sendMessage, 'Create marker')
-                sleep(0.5)
-                self.stylusplh = False
-
-            sleep(0.175)
-            if self._pause_:
-                if self.trigger_init:
-                    self.trigger_init.close()
-                return
-
-
-class TriggerNew(threading.Thread):
-
-    def __init__(self, trigger_queue, event, sle):
-        """Class (threading) to compute real time tractography data for visualization in a single loop.
-
-        Different than ComputeTractsThread because it does not keep adding tracts to the bundle until maximum,
-        is reached. It actually compute all requested tracts at once. (Might be deleted in the future)!
-        Tracts are computed using the Trekker library by Baran Aydogan (https://dmritrekker.github.io/)
-        For VTK visualization, each tract (fiber) is a constructed as a tube and many tubes combined in one
-        vtkMultiBlockDataSet named as a branch. Several branches are combined in another vtkMultiBlockDataSet named as
-        bundle, to obtain fast computation and visualization. The bundle dataset is mapped to a single vtkActor.
-        Mapper and Actor are computer in the data/viewer_volume.py module for easier handling in the invesalius 3D scene.
-
-        Sleep function in run method is used to avoid blocking GUI and more fluent, real-time navigation
-
-        :param inp: List of inputs: trekker instance, affine numpy array, seed_offset, seed_radius, n_threads
-        :type inp: list
-        :param affine_vtk: Affine matrix in vtkMatrix4x4 instance to update objects position in 3D scene
-        :type affine_vtk: vtkMatrix4x4
-        :param coord_queue: Queue instance that manage coordinates read from tracking device and coregistered
-        :type coord_queue: queue.Queue
-        :param visualization_queue: Queue instance that manage coordinates to be visualized
-        :type visualization_queue: queue.Queue
-        :param event: Threading event to coordinate when tasks as done and allow UI release
-        :type event: threading.Event
-        :param sle: Sleep pause in seconds
-        :type sle: float
-        """
-
-        threading.Thread.__init__(self, name='Trigger')
-
-        self.trigger_init = None
-        self.stylusplh = False
-        # self.COM = False
-        # self.__bind_events()
-        try:
-            import serial
-
-            self.trigger_init = serial.Serial('COM5', baudrate=115200, timeout=0)
-            # self.COM = True
-
-        except:
-            #wx.MessageBox(_('Connection with port COM1 failed'), _('Communication error'), wx.OK | wx.ICON_ERROR)
-            print("Trigger init error: Connection with port COM failed")
-            # self.COM = False
-            pass
-
-        # self.coord_queue = coord_queue
-        self.trigger_queue = trigger_queue
-        self.event = event
-        self.sle = sle
-
-    def run(self):
-
-        while not self.event.is_set():
-            trigger_on = False
-            try:
-                self.trigger_init.write(b'0')
-                sleep(0.3)
-                lines = self.trigger_init.readlines()
-                # Following lines can simulate a trigger in 3 sec repetitions
-                # sleep(3)
-                # lines = True
-                if lines:
-                    trigger_on = True
-                    # wx.CallAfter(Publisher.sendMessage, 'Create marker')
-
-                if self.stylusplh:
-                    trigger_on = True
-                    # wx.CallAfter(Publisher.sendMessage, 'Create marker')
-                    self.stylusplh = False
-
-                self.trigger_queue.put_nowait(trigger_on)
-                sleep(self.sle)
-
-            except:
-                print("Trigger not read, error")
-                pass
-
-        else:
-            if self.trigger_init:
-                self.trigger_init.close()
@@ -36,6 +36,7 @@ from scipy.spatial import distance
 from imageio import imsave
  
 import invesalius.constants as const
+import invesalius.data.coordinates as dco
 import invesalius.data.slice_ as sl
 import invesalius.data.styles_3d as styles
 import invesalius.data.transformations as tr
@@ -192,6 +193,7 @@ class Viewer(wx.Panel):
         self.dummy_coil_actor = None
         self.target_mode = False
         self.polydata = None
+        self.use_default_object = True
         self.anglethreshold = const.COIL_ANGLES_THRESHOLD
         self.distthreshold = const.COIL_COORD_THRESHOLD
         self.angle_arrow_projection_threshold = const.COIL_ANGLE_ARROW_PROJECTION_THRESHOLD
@@ -278,7 +280,7 @@ class Viewer(wx.Panel):
         Publisher.subscribe(self.HideAllMarkers, 'Hide all markers')
         Publisher.subscribe(self.ShowAllMarkers, 'Show all markers')
         Publisher.subscribe(self.RemoveAllMarkers, 'Remove all markers')
-        Publisher.subscribe(self.RemoveMarker, 'Remove marker')
+        Publisher.subscribe(self.RemoveMultipleMarkers, 'Remove multiple markers')
         Publisher.subscribe(self.BlinkMarker, 'Blink Marker')
         Publisher.subscribe(self.StopBlinkMarker, 'Stop Blink Marker')
         Publisher.subscribe(self.SetNewColor, 'Set new color')
@@ -343,18 +345,23 @@ class Viewer(wx.Panel):
         if style == const.SLICE_STATE_CROSS:
             self._mode_cross = True
             # self._check_and_set_ball_visibility()
+            #if not self.actor_peel:
             self._ball_ref_visibility = True
+            #else:
+            #    self._ball_ref_visibility = False
             # if self._to_show_ball:
-            if not self.ball_actor:
+            if not self.ball_actor: #and not self.actor_peel:
                 self.CreateBallReference()
-
+                #self.ball_actor.SetVisibility(1)
+            #else:
+             #   self.ball_actor.SetVisibility(0)
             self.interactor.Render()
  
     def _uncheck_ball_reference(self, style):
         if style == const.SLICE_STATE_CROSS:
             self._mode_cross = False
             # self.RemoveBallReference()
-            self._ball_ref_visibility = False
+            self._ball_ref_visibility = True
             if self.ball_actor:
                 self.ren.RemoveActor(self.ball_actor)
                 self.ball_actor = None
@@ -671,7 +678,7 @@ class Viewer(wx.Panel):
         self.staticballs = []
         self.UpdateRender()
  
-    def RemoveMarker(self, index):
+    def RemoveMultipleMarkers(self, index):
         for i in reversed(index):
             self.ren.RemoveActor(self.staticballs[i])
             del self.staticballs[i]
@@ -702,7 +709,7 @@ class Viewer(wx.Panel):
             self.index = False
  
     def SetNewColor(self, index, color):
-        self.staticballs[index].GetProperty().SetColor(color)
+        self.staticballs[index].GetProperty().SetColor([round(s/255.0, 3) for s in color])
         self.Refresh()
  
     def OnTargetMarkerTransparency(self, status, index):
@@ -980,10 +987,12 @@ class Viewer(wx.Panel):
  
         vtk_colors = vtk.vtkNamedColors()
  
-        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 = dco.coordinates_to_transformation_matrix(
+            position=self.target_coord[:3],
+            orientation=self.target_coord[3:],
+            axes='sxyz',
+        )
+        m_img = np.asmatrix(m_img)
  
         m_img_vtk = vtk.vtkMatrix4x4()
  
@@ -1019,10 +1028,10 @@ class Viewer(wx.Panel):
         self.aim_actor = aim_actor
         self.ren.AddActor(aim_actor)
  
-        if self.polydata:
-            obj_polydata = self.polydata
-        else:
+        if self.use_default_object:
             obj_polydata = self.CreateObjectPolyData(os.path.join(inv_paths.OBJ_DIR, "magstim_fig8_coil_no_handle.stl"))
+        else:
+            obj_polydata = self.polydata
  
         transform = vtk.vtkTransform()
         transform.RotateZ(90)
@@ -1238,6 +1247,7 @@ class Viewer(wx.Panel):
     #     self.UpdateCameraBallPosition(None, position)
  
     def UpdateCameraBallPosition(self, position):
+        #if not self.actor_peel:
         coord_flip = list(position[:3])
         coord_flip[1] = -coord_flip[1]
         self.ball_actor.SetPosition(coord_flip)
@@ -1328,6 +1338,9 @@ class Viewer(wx.Panel):
         self.ren.AddActor(self.x_actor)
         self.ren.AddActor(self.y_actor)
         self.ren.AddActor(self.z_actor)
+        self.x_actor.SetVisibility(0)
+        self.y_actor.SetVisibility(0)
+        self.z_actor.SetVisibility(0)
         #self.ren.AddActor(self.obj_projection_arrow_actor)
         #self.ren.AddActor(self.object_orientation_torus_actor)
         # self.obj_axes = vtk.vtkAxesActor()
@@ -1442,6 +1455,8 @@ class Viewer(wx.Panel):
             self.ren.RemoveActor(self.object_orientation_torus_actor)
             self.ren.RemoveActor(self.obj_projection_arrow_actor)
             self.actor_peel = None
+            self.ball_actor.SetVisibility(1)
+
         if flag and actor:
             self.ren.AddActor(actor)
             self.actor_peel = actor
@@ -1499,7 +1514,7 @@ class Viewer(wx.Panel):
  
                     self.ren.AddActor(self.obj_projection_arrow_actor)
                     self.ren.AddActor(self.object_orientation_torus_actor)
-
+                    self.ball_actor.SetVisibility(0)
                     self.obj_projection_arrow_actor.SetPosition(closestPoint)
                     self.obj_projection_arrow_actor.SetOrientation(coil_dir)
  
@@ -1520,6 +1535,8 @@ class Viewer(wx.Panel):
             self.ren.RemoveActor(self.obj_projection_arrow_actor)
             self.ren.RemoveActor(self.object_orientation_torus_actor)
             self.ren.RemoveActor(self.x_actor)
+            self.ball_actor.SetVisibility(1)
+
             #self.ren.RemoveActor(self.y_actor)
         self.Refresh()
  
@@ -1531,9 +1548,9 @@ class Viewer(wx.Panel):
             self.pTarget = self.CenterOfMass()
             if self.obj_actor:
                 self.obj_actor.SetVisibility(self.obj_state)
-                self.x_actor.SetVisibility(self.obj_state)
-                self.y_actor.SetVisibility(self.obj_state)
-                self.z_actor.SetVisibility(self.obj_state)
+                #self.x_actor.SetVisibility(self.obj_state)
+                #self.y_actor.SetVisibility(self.obj_state)
+                #self.z_actor.SetVisibility(self.obj_state)
                 #self.object_orientation_torus_actor.SetVisibility(self.obj_state)
                 #self.obj_projection_arrow_actor.SetVisibility(self.obj_state)
         self.Refresh()
@@ -1599,10 +1616,11 @@ class Viewer(wx.Panel):
             self.GetCellIntersection(p1, norm, coil_norm, coil_dir)
         self.Refresh()
  
-    def UpdateTrackObjectState(self, evt=None, flag=None, obj_name=None, polydata=None):
+    def UpdateTrackObjectState(self, evt=None, flag=None, obj_name=None, polydata=None, use_default_object=True):
         if flag:
             self.obj_name = obj_name
             self.polydata = polydata
+            self.use_default_object = use_default_object
             if not self.obj_actor:
                 self.AddObjectActor(self.obj_name)
         else:
@@ -1630,7 +1648,10 @@ class Viewer(wx.Panel):
             self.x_actor.SetVisibility(self.obj_state)
             self.y_actor.SetVisibility(self.obj_state)
             self.z_actor.SetVisibility(self.obj_state)
-
+            #if self.actor_peel:
+            #    self.ball_actor.SetVisibility(0)
+            #else:
+            #    self.ball_actor.SetVisibility(1)
         self.Refresh()
  
     def OnUpdateTracts(self, root=None, affine_vtk=None, coord_offset=None):
@@ -2563,6 +2563,41 @@ class PanelFFillConfidence(wx.Panel):
         self.config.confid_iters = self.spin_iters.GetValue()
  
  
+class PanelFFillProgress(wx.Panel):
+    def __init__(self, parent, ID=-1, style=wx.TAB_TRAVERSAL|wx.NO_BORDER):
+        wx.Panel.__init__(self, parent, ID, style=style)
+        self._init_gui()
+
+    def _init_gui(self):
+        self.progress = wx.Gauge(self, -1)
+        self.lbl_progress_caption = wx.StaticText(self, -1, _("Elapsed time:"))
+        self.lbl_time = wx.StaticText(self, -1, _("00:00:00"))
+
+        main_sizer = wx.BoxSizer(wx.VERTICAL)
+        main_sizer.Add(self.progress, 0, wx.EXPAND | wx.ALL, 5)
+        time_sizer = wx.BoxSizer(wx.HORIZONTAL)
+        time_sizer.Add(self.lbl_progress_caption, 0, wx.EXPAND, 0)
+        time_sizer.Add(self.lbl_time, 1, wx.EXPAND | wx.LEFT, 5)
+        main_sizer.Add(time_sizer, 0, wx.EXPAND | wx.ALL, 5)
+
+        self.SetSizer(main_sizer)
+        main_sizer.Fit(self)
+        main_sizer.SetSizeHints(self)
+
+    def StartTimer(self):
+        self.t0 = time.time()
+
+    def StopTimer(self):
+        fmt = "%H:%M:%S"
+        self.lbl_time.SetLabel(time.strftime(fmt, time.gmtime(time.time() - self.t0)))
+        self.progress.SetValue(0)
+
+    def Pulse(self):
+        fmt = "%H:%M:%S"
+        self.lbl_time.SetLabel(time.strftime(fmt, time.gmtime(time.time() - self.t0)))
+        self.progress.Pulse()
+
+
 class FFillOptionsDialog(wx.Dialog):
     def __init__(self, title, config):
         wx.Dialog.__init__(self, wx.GetApp().GetTopWindow(), -1, title, style=wx.DEFAULT_DIALOG_STYLE|wx.FRAME_FLOAT_ON_PARENT|wx.STAY_ON_TOP)
@@ -2820,6 +2855,10 @@ class FFillSegmentationOptionsDialog(wx.Dialog):
         self.panel_ffill_confidence.SetMinSize((250, -1))
         self.panel_ffill_confidence.Hide()
  
+        self.panel_ffill_progress = PanelFFillProgress(self, -1, style=wx.TAB_TRAVERSAL)
+        self.panel_ffill_progress.SetMinSize((250, -1))
+        # self.panel_ffill_progress.Hide()
+
         self.close_btn = wx.Button(self, wx.ID_CLOSE)
  
         # Sizer
@@ -2876,11 +2915,16 @@ class FFillSegmentationOptionsDialog(wx.Dialog):
             sizer.Add(0, 0, (12, 0))
         except TypeError:
             sizer.AddStretchSpacer((12, 0))
-        sizer.Add(self.close_btn, (13, 0), (1, 6), flag=wx.ALIGN_RIGHT|wx.RIGHT, border=5)
+        sizer.Add(self.panel_ffill_progress, (13, 0), (1, 6), flag=wx.ALIGN_RIGHT|wx.RIGHT, border=5)
         try:
             sizer.Add(0, 0, (14, 0))
         except TypeError:
             sizer.AddStretchSpacer((14, 0))
+        sizer.Add(self.close_btn, (15, 0), (1, 6), flag=wx.ALIGN_RIGHT|wx.RIGHT, border=5)
+        try:
+            sizer.Add(0, 0, (16, 0))
+        except TypeError:
+            sizer.AddStretchSpacer((16, 0))
  
         self.SetSizer(sizer)
         sizer.Fit(self)
@@ -3263,14 +3307,17 @@ class MaskDensityDialog(wx.Dialog):
  
 class ObjectCalibrationDialog(wx.Dialog):
  
-    def __init__(self, nav_prop):
+    def __init__(self, tracker, pedal_connection):
+        self.tracker = tracker
+        self.pedal_connection = pedal_connection
  
-        self.tracker_id = nav_prop[0]
-        self.trk_init = nav_prop[1]
-        self.TrackerCoordinates = nav_prop[2]
+        self.trk_init, self.tracker_id = tracker.GetTrackerInfo()
+        #self.TrackerCoordinates = nav_prop[2]
         self.obj_ref_id = 2
         self.obj_name = None
         self.polydata = None
+        self.use_default_object = False
+        self.object_fiducial_being_set = None
  
         self.obj_fiducials = np.full([5, 3], np.nan)
         self.obj_orients = np.full([5, 3], np.nan)
@@ -3281,6 +3328,11 @@ class ObjectCalibrationDialog(wx.Dialog):
         self._init_gui()
         self.LoadObject()
  
+        self.__bind_events()
+
+    def __bind_events(self):
+        Publisher.subscribe(self.SetObjectFiducial, 'Set object fiducial')
+
     def _init_gui(self):
         self.interactor = wxVTKRenderWindowInteractor(self, -1, size=self.GetSize())
         self.interactor.Enable(1)
@@ -3298,7 +3350,7 @@ class ObjectCalibrationDialog(wx.Dialog):
         choice_ref = wx.ComboBox(self, -1, "", size=wx.Size(90, 23),
                                  choices=const.REF_MODE, style=wx.CB_DROPDOWN | wx.CB_READONLY)
         choice_ref.SetToolTip(tooltip)
-        choice_ref.Bind(wx.EVT_COMBOBOX, self.OnChoiceRefMode)
+        choice_ref.Bind(wx.EVT_COMBOBOX, self.OnChooseReferenceMode)
         choice_ref.SetSelection(1)
         choice_ref.Enable(1)
         if self.tracker_id == const.PATRIOT or self.tracker_id == const.ISOTRAKII:
@@ -3331,15 +3383,17 @@ class ObjectCalibrationDialog(wx.Dialog):
                              choice_sensor])
  
         # Push buttons for object fiducials
-        btns_obj = const.BTNS_OBJ
-        tips_obj = const.TIPS_OBJ
+        for object_fiducial in const.OBJECT_FIDUCIALS:
+            index = object_fiducial['fiducial_index']
+            label = object_fiducial['label']
+            button_id = object_fiducial['button_id']
+            tip = object_fiducial['tip']
+
+            ctrl = wx.ToggleButton(self, button_id, label=label, size=wx.Size(60, 23))
+            ctrl.SetToolTip(wx.ToolTip(tip))
+            ctrl.Bind(wx.EVT_TOGGLEBUTTON, partial(self.OnObjectFiducialButton, index, ctrl=ctrl))
  
-        for k in btns_obj:
-            n = list(btns_obj[k].keys())[0]
-            lab = list(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)
+            self.btns_coord[index] = ctrl
  
         for m in range(0, 5):
             for n in range(0, 3):
@@ -3383,8 +3437,9 @@ class ObjectCalibrationDialog(wx.Dialog):
             return 0
  
     def LoadObject(self):
-        default = self.ObjectImportDialog()
-        if not default:
+        self.use_default_object = self.ObjectImportDialog()
+
+        if not self.use_default_object:
             filename = ShowImportMeshFilesDialog()
  
             if filename:
@@ -3397,11 +3452,17 @@ class ObjectCalibrationDialog(wx.Dialog):
                 elif filename.lower().endswith('.vtp'):
                     reader = vtk.vtkXMLPolyDataReader()
                 else:
-                    wx.MessageBox(_("File format not reconized by InVesalius"), _("Import surface error"))
+                    wx.MessageBox(_("File format not recognized by InVesalius"), _("Import surface error"))
                     return
             else:
                 filename = os.path.join(inv_paths.OBJ_DIR, "magstim_fig8_coil.stl")
                 reader = vtk.vtkSTLReader()
+
+                # XXX: If the user cancels the dialog for importing the coil mesh file, the current behavior is to
+                #      use the default object after all. A more logical behavior in that case would be to cancel the
+                #      whole object calibration, but implementing that would need larger refactoring.
+                #
+                self.use_default_object = True
         else:
             filename = os.path.join(inv_paths.OBJ_DIR, "magstim_fig8_coil.stl")
             reader = vtk.vtkSTLReader()
@@ -3476,39 +3537,87 @@ class ObjectCalibrationDialog(wx.Dialog):
         self.ren.AddActor(ball_actor)
         return ball_actor, tactor
  
-    def OnGetObjectFiducials(self, evt):
-        btn_id = list(const.BTNS_OBJ[evt.GetId()].keys())[0]
+    def OnObjectFiducialButton(self, index, evt, ctrl):
+        if not self.tracker.IsTrackerInitialized():
+            ShowNavigationTrackerWarning(0, 'choose')
+            return
  
-        if self.trk_init and self.tracker_id:
-            coord_raw, markers_flag = self.TrackerCoordinates.GetCoordinates()
-            if self.obj_ref_id and btn_id == 4:
-                if self.tracker_id == const.ROBOT:
-                    trck_init_robot = self.trk_init[1][0]
-                    coord = trck_init_robot.Run()
-                else:
-                    coord = coord_raw[self.obj_ref_id, :]
+        # TODO: The code below until the end of the function is essentially copy-paste from
+        #       OnTrackerFiducials function in NeuronavigationPanel class. Probably the easiest
+        #       way to deduplicate this would be to create a Fiducial class, which would contain
+        #       this code just once.
+        #
+
+        # Do not allow several object fiducials to be set at the same time.
+        if self.object_fiducial_being_set is not None and self.object_fiducial_being_set != index:
+            ctrl.SetValue(False)
+            return
+
+        # Called when the button for setting the object fiducial is enabled and either pedal is pressed
+        # or the button is pressed again.
+        #
+        def set_fiducial_callback(state):
+            if state:
+                Publisher.sendMessage('Set object fiducial', fiducial_index=index)
+                if self.pedal_connection is not None:
+                    self.pedal_connection.remove_callback('fiducial')
+
+                ctrl.SetValue(False)
+                self.object_fiducial_being_set = None
+
+        if ctrl.GetValue():
+            self.object_fiducial_being_set = index
+
+            if self.pedal_connection is not None:
+                self.pedal_connection.add_callback('fiducial', set_fiducial_callback)
+        else:
+            set_fiducial_callback(True)
+
+    def SetObjectFiducial(self, fiducial_index):
+        coord, coord_raw = self.tracker.GetTrackerCoordinates(
+            # XXX: Always use static reference mode when getting the coordinates. This is what the
+            #      code did previously, as well. At some point, it should probably be thought through
+            #      if this is actually what we want or if it should be changed somehow.
+            #
+            ref_mode_id=const.STATIC_REF,
+            n_samples=const.CALIBRATION_TRACKER_SAMPLES,
+        )
+
+        # XXX: The condition below happens when setting the "fixed" coordinate in the object calibration.
+        #      The case is not handled by GetTrackerCoordinates function, therefore redo some computation
+        #      that is already done once by GetTrackerCoordinates, namely, invert the y-coordinate.
+        #
+        #      (What is done here does not seem to be completely consistent with "always use static reference
+        #      mode" principle above, but it's hard to come up with a simple change to increase the consistency
+        #      and not change the function to the point of potentially breaking it.)
+        #
+        if self.obj_ref_id and fiducial_index == 4:
+            if self.tracker_id == const.ROBOT:
+                trck_init_robot = self.trk_init[1][0]
+                coord = trck_init_robot.Run()
             else:
-                coord = coord_raw[0, :]
+                coord = coord_raw[self.obj_ref_id, :]
         else:
-            ShowNavigationTrackerWarning(0, 'choose')
+            coord = coord_raw[0, :]
+        coord[2] = -coord[2]
  
-        if btn_id == 3:
+        if fiducial_index == 3:
             coord = np.zeros([6,])
  
         # 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.obj_fiducials[fiducial_index, :] = coord[:3]
+            self.obj_orients[fiducial_index, :] = coord[3:]
+            for i in [0, 1, 2]:
+                self.txt_coord[fiducial_index][i].SetLabel(str(round(coord[i], 1)))
+                if self.text_actors[fiducial_index]:
+                    self.text_actors[fiducial_index].GetProperty().SetColor(0.0, 1.0, 0.0)
+                    self.ball_actors[fiducial_index].GetProperty().SetColor(0.0, 1.0, 0.0)
             self.Refresh()
         else:
             ShowNavigationTrackerWarning(0, 'choose')
  
-    def OnChoiceRefMode(self, evt):
+    def OnChooseReferenceMode(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 -
@@ -3516,13 +3625,14 @@ class ObjectCalibrationDialog(wx.Dialog):
         # 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():
+        if evt.GetSelection() == 1:
             self.obj_ref_id = 2
             if self.tracker_id in [const.FASTRAK, const.DEBUGTRACKRANDOM, const.DEBUGTRACKAPPROACH]:
                 self.choice_sensor.Show(self.obj_ref_id)
         else:
             self.obj_ref_id = 0
             self.choice_sensor.Show(self.obj_ref_id)
+
         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)
@@ -3539,7 +3649,7 @@ class ObjectCalibrationDialog(wx.Dialog):
             self.obj_ref_id = 0
  
     def GetValue(self):
-        return self.obj_fiducials, self.obj_orients, self.obj_ref_id, self.obj_name, self.polydata
+        return self.obj_fiducials, self.obj_orients, self.obj_ref_id, self.obj_name, self.polydata, self.use_default_object
  
 class ICPCorregistrationDialog(wx.Dialog):
  
@@ -17,11 +17,11 @@
 #    detalhes.
 #--------------------------------------------------------------------------
  
+import dataclasses
 from functools import partial
+import itertools
 import csv
-import os
 import queue
-import sys
 import time
 import threading
  
@@ -42,10 +42,8 @@ except ImportError:
 import wx
  
 try:
-    import wx.lib.agw.hyperlink as hl
     import wx.lib.agw.foldpanelbar as fpb
 except ImportError:
-    import wx.lib.hyperlink as hl
     import wx.lib.foldpanelbar as fpb
  
 import wx.lib.colourselect as csel
@@ -54,23 +52,22 @@ from invesalius.pubsub import pub as Publisher
 from time import sleep
  
 import invesalius.constants as const
-import invesalius.data.bases as db
  
 if has_trekker:
     import invesalius.data.brainmesh_handler as brain
  
-import invesalius.data.coordinates as dco
-import invesalius.data.coregistration as dcr
+import invesalius.data.imagedata_utils as imagedata_utils
 import invesalius.data.slice_ as sl
-import invesalius.data.trackers as dt
 import invesalius.data.tractography as dti
-import invesalius.data.transformations as tr
-import invesalius.data.trigger as trig
 import invesalius.data.record_coords as rec
 import invesalius.data.vtk_utils as vtk_utils
 import invesalius.gui.dialogs as dlg
 import invesalius.project as prj
 from invesalius import utils
+from invesalius.gui import utils as gui_utils
+from invesalius.navigation.icp import ICP
+from invesalius.navigation.navigation import Navigation
+from invesalius.navigation.tracker import Tracker
  
 HAS_PEDAL_CONNECTION = True
 try:
@@ -165,6 +162,12 @@ class InnerFoldPanel(wx.Panel):
  
         fold_panel = fpb.FoldPanelBar(self, -1, wx.DefaultPosition,
                                       (10, 310), 0, fpb.FPB_SINGLE_FOLD)
+
+        # Initialize Tracker and PedalConnection objects here so that they are available to several panels.
+        #
+        tracker = Tracker()
+        pedal_connection = PedalConnection() if HAS_PEDAL_CONNECTION else None
+
         # Fold panel style
         style = fpb.CaptionBarStyle()
         style.SetCaptionStyle(fpb.CAPTIONBAR_GRADIENT_V)
@@ -173,7 +176,7 @@ class InnerFoldPanel(wx.Panel):
  
         # Fold 1 - Navigation panel
         item = fold_panel.AddFoldPanel(_("Neuronavigation"), collapsed=True)
-        ntw = NeuronavigationPanel(item)
+        ntw = NeuronavigationPanel(item, tracker, pedal_connection)
  
         fold_panel.ApplyCaptionStyle(item, style)
         fold_panel.AddFoldPanelWindow(item, ntw, spacing=0,
@@ -182,7 +185,7 @@ class InnerFoldPanel(wx.Panel):
  
         # Fold 2 - Object registration panel
         item = fold_panel.AddFoldPanel(_("Object registration"), collapsed=True)
-        otw = ObjectRegistrationPanel(item)
+        otw = ObjectRegistrationPanel(item, tracker, pedal_connection)
  
         fold_panel.ApplyCaptionStyle(item, style)
         fold_panel.AddFoldPanelWindow(item, otw, spacing=0,
@@ -222,13 +225,13 @@ class InnerFoldPanel(wx.Panel):
         checkcamera.Bind(wx.EVT_CHECKBOX, self.OnVolumeCamera)
         self.checkcamera = checkcamera
  
-        # 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, _('Ext. trigger'))
-        checktrigger.SetToolTip(tooltip)
-        checktrigger.SetValue(False)
-        checktrigger.Bind(wx.EVT_CHECKBOX, partial(self.OnExternalTrigger, ctrl=checktrigger))
-        self.checktrigger = checktrigger
+        # Check box to create markers from serial port
+        tooltip = wx.ToolTip(_("Enable serial port communication for creating markers"))
+        checkbox_serial_port = wx.CheckBox(self, -1, _('Serial port'))
+        checkbox_serial_port.SetToolTip(tooltip)
+        checkbox_serial_port.SetValue(False)
+        checkbox_serial_port.Bind(wx.EVT_CHECKBOX, partial(self.OnEnableSerialPort, ctrl=checkbox_serial_port))
+        self.checkbox_serial_port = checkbox_serial_port
  
         # Check box for object position and orientation update in volume rendering during navigation
         tooltip = wx.ToolTip(_("Show and track TMS coil"))
@@ -241,12 +244,12 @@ class InnerFoldPanel(wx.Panel):
  
         #  if sys.platform != 'win32':
         self.checkcamera.SetWindowVariant(wx.WINDOW_VARIANT_SMALL)
-        checktrigger.SetWindowVariant(wx.WINDOW_VARIANT_SMALL)
+        checkbox_serial_port.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, 0, wx.ALIGN_CENTER)
+        line_sizer.Add(checkbox_serial_port, 0, wx.ALIGN_CENTER)
         line_sizer.Add(checkobj, 0, wx.RIGHT | wx.LEFT, 5)
         line_sizer.Fit(self)
  
@@ -277,17 +280,24 @@ class InnerFoldPanel(wx.Panel):
  
     def OnCheckStatus(self, nav_status, vis_status):
         if nav_status:
-            self.checktrigger.Enable(False)
+            self.checkbox_serial_port.Enable(False)
             self.checkobj.Enable(False)
         else:
-            self.checktrigger.Enable(True)
+            self.checkbox_serial_port.Enable(True)
             if self.track_obj:
                 self.checkobj.Enable(True)
  
-    def OnExternalTrigger(self, evt, ctrl):
-        Publisher.sendMessage('Update trigger state', trigger_state=ctrl.GetValue())
+    def OnEnableSerialPort(self, evt, ctrl):
+        com_port = None
+        if ctrl.GetValue():
+            from wx import ID_OK
+            dlg_port = dlg.SetCOMport()
+            if dlg_port.ShowModal() == ID_OK:
+                com_port = dlg_port.GetValue()
  
-    def OnShowObject(self, evt=None, flag=None, obj_name=None, polydata=None):
+        Publisher.sendMessage('Update serial port', serial_port=com_port)
+
+    def OnShowObject(self, evt=None, flag=None, obj_name=None, polydata=None, use_default_object=True):
         if not evt:
             if flag:
                 self.checkobj.Enable(True)
@@ -308,432 +318,8 @@ class InnerFoldPanel(wx.Panel):
         Publisher.sendMessage('Update volume camera state', camera_state=self.checkcamera.GetValue())
  
  
-class Navigation():
-    def __init__(self):
-        self.image_fiducials = np.full([3, 3], np.nan)
-        self.correg = None
-        self.current_coord = 0, 0, 0
-        self.target = None
-        self.trigger = None
-        self.trigger_state = False
-        self.obj_reg = None
-        self.track_obj = False
-        self.m_change = None
-        self.all_fiducials = np.zeros((6, 6))
-
-        self.event = threading.Event()
-
-        self.coord_queue = QueueCustom(maxsize=1)
-        self.objattarget_queue = QueueCustom(maxsize=1)
-        self.icp_queue = QueueCustom(maxsize=1)
-        self.robottarget_queue = QueueCustom(maxsize=1)
-        # self.visualization_queue = QueueCustom(maxsize=1)
-        self.trigger_queue = QueueCustom(maxsize=1)
-        self.coord_tracts_queue = QueueCustom(maxsize=1)
-        self.tracts_queue = QueueCustom(maxsize=1)
-
-        # Tractography parameters
-        self.trk_inp = None
-        self.trekker = None
-        self.n_threads = None
-        self.view_tracts = False
-        self.peel_loaded = False
-        self.enable_act = False
-        self.act_data = None
-        self.n_tracts = const.N_TRACTS
-        self.seed_offset = const.SEED_OFFSET
-        self.seed_radius = const.SEED_RADIUS
-        self.sleep_nav = const.SLEEP_NAVIGATION
-
-    def SetImageFiducial(self, fiducial_index, coord):
-        self.image_fiducials[fiducial_index, :] = coord
-
-        print("Set image fiducial {} to coordinates {}".format(fiducial_index, coord))
-
-    def AreImageFiducialsSet(self):
-        return not np.isnan(self.image_fiducials).any()
-
-    def UpdateFiducialRegistrationError(self, tracker):
-        tracker_fiducials, tracker_fiducials_raw = tracker.GetTrackerFiducials()
-        ref_mode_id = tracker.GetReferenceMode()
-
-        self.all_fiducials = np.vstack([self.image_fiducials, tracker_fiducials])
-
-        self.fre = db.calculate_fre(tracker_fiducials_raw, self.all_fiducials, ref_mode_id, self.m_change)
-
-    def GetFiducialRegistrationError(self, icp):
-        fre = icp.icp_fre if icp.use_icp else self.fre
-        return fre, fre <= const.FIDUCIAL_REGISTRATION_ERROR_THRESHOLD
-
-    def StartNavigation(self, tracker):
-        tracker_fiducials, tracker_fiducials_raw = tracker.GetTrackerFiducials()
-        ref_mode_id = tracker.GetReferenceMode()
-
-        # initialize jobs list
-        jobs_list = []
-
-        if self.event.is_set():
-            self.event.clear()
-
-        vis_components = [self.trigger_state, self.view_tracts, self.peel_loaded]
-        vis_queues = [self.coord_queue, self.trigger_queue, self.tracts_queue, self.icp_queue, self.robottarget_queue]
-
-        Publisher.sendMessage("Navigation status", nav_status=True, vis_status=vis_components)
-
-        self.all_fiducials = np.vstack([self.image_fiducials, tracker_fiducials])
-
-        # fiducials matrix
-        m_change = tr.affine_matrix_from_points(self.all_fiducials[3:, :].T, self.all_fiducials[:3, :].T,
-                                                shear=False, scale=False)
-        self.m_change = m_change
-
-        errors = False
-
-        if self.track_obj:
-            # if object tracking is selected
-            if self.obj_reg is None:
-                # check if object registration was performed
-                wx.MessageBox(_("Perform coil registration before navigation."), _("InVesalius 3"))
-                errors = True
-            else:
-                # if object registration was correctly performed continue with navigation
-                # 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
-
-                coreg_data = [m_change, obj_ref_mode]
-
-                if ref_mode_id:
-                    coord_raw, markers_flag = tracker.TrackerCoordinates.GetCoordinates()
-                else:
-                    coord_raw = np.array([None])
-
-                obj_data = db.object_registration(obj_fiducials, obj_orients, coord_raw, m_change)
-                coreg_data.extend(obj_data)
-
-                queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue, self.objattarget_queue]
-                jobs_list.append(dcr.CoordinateCorregistrate(ref_mode_id, tracker, coreg_data,
-                                                                self.view_tracts, queues,
-                                                                self.event, self.sleep_nav, tracker.tracker_id,
-                                                                self.target))
-        else:
-            coreg_data = (m_change, 0)
-            queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue]
-            jobs_list.append(dcr.CoordinateCorregistrateNoObject(ref_mode_id, tracker, coreg_data,
-                                                                    self.view_tracts, queues,
-                                                                    self.event, self.sleep_nav))
-
-        if not errors:
-            #TODO: Test the trigger thread
-            if self.trigger_state:
-                # self.trigger = trig.Trigger(nav_id)
-                jobs_list.append(trig.TriggerNew(self.trigger_queue, self.event, self.sleep_nav))
-
-            if self.view_tracts:
-                # initialize Trekker parameters
-                slic = sl.Slice()
-                prj_data = prj.Project()
-                matrix_shape = tuple(prj_data.matrix_shape)
-                affine = slic.affine.copy()
-                affine[1, -1] -= matrix_shape[1]
-                affine_vtk = vtk_utils.numpy_to_vtkMatrix4x4(affine)
-                Publisher.sendMessage("Update marker offset state", create=True)
-                self.trk_inp = self.trekker, affine, self.seed_offset, self.n_tracts, self.seed_radius,\
-                                self.n_threads, self.act_data, affine_vtk, matrix_shape[1]
-                # print("Appending the tract computation thread!")
-                queues = [self.coord_tracts_queue, self.tracts_queue]
-                if self.enable_act:
-                    jobs_list.append(dti.ComputeTractsACTThread(self.trk_inp, queues, self.event, self.sleep_nav))
-                else:
-                    jobs_list.append(dti.ComputeTractsThread(self.trk_inp, queues, self.event, self.sleep_nav))
-
-            jobs_list.append(UpdateNavigationScene(vis_queues, vis_components,
-                                                    self.event, self.sleep_nav))
-
-            for jobs in jobs_list:
-                # jobs.daemon = True
-                jobs.start()
-                # del jobs
-
-    def StopNavigation(self):
-        self.event.set()
-
-        self.coord_queue.clear()
-        self.coord_queue.join()
-
-        if self.trigger_state:
-            self.trigger_queue.clear()
-            self.trigger_queue.join()
-        if self.view_tracts:
-            self.coord_tracts_queue.clear()
-            self.coord_tracts_queue.join()
-
-            self.tracts_queue.clear()
-            self.tracts_queue.join()
-
-        vis_components = [self.trigger_state, self.view_tracts,  self.peel_loaded]
-        Publisher.sendMessage("Navigation status", nav_status=False, vis_status=vis_components)
-
-class Tracker():
-    def __init__(self):
-        self.trk_init = None
-        self.tracker_id = const.DEFAULT_TRACKER
-        self.ref_mode_id = const.DEFAULT_REF_MODE
-
-        self.tracker_fiducials = np.full([3, 3], np.nan)
-        self.tracker_fiducials_raw = np.zeros((6, 6))
-        self.m_tracker_fiducials_raw = np.zeros((6, 4, 4))
-
-        self.tracker_connected = False
-
-        self.event_coord = threading.Event()
-        self.event_robot = threading.Event()
-        self.TrackerCoordinates = dco.TrackerCoordinates()
-
-    def SetTracker(self, new_tracker):
-        if new_tracker:
-            self.DisconnectTracker()
-
-            self.trk_init = dt.TrackerConnection(new_tracker, None, 'connect')
-            if not self.trk_init[0]:
-                dlg.ShowNavigationTrackerWarning(self.tracker_id, self.trk_init[1])
-
-                self.tracker_id = 0
-                self.tracker_connected = False
-            else:
-                self.tracker_id = new_tracker
-                self.tracker_connected = True
-                dco.ReceiveCoordinates(self.trk_init, self.tracker_id, self.TrackerCoordinates, self.event_coord).start()
-
-            Publisher.sendMessage('Update tracker initializer',
-                               nav_prop=(self.tracker_id, self.trk_init, self.TrackerCoordinates, self.ref_mode_id))
-
-    def DisconnectTracker(self):
-        if self.tracker_connected:
-            self.ResetTrackerFiducials()
-            Publisher.sendMessage('Update status text in GUI',
-                                    label=_("Disconnecting tracker ..."))
-            Publisher.sendMessage('Remove sensors ID')
-            Publisher.sendMessage('Remove object data')
-            self.trk_init = dt.TrackerConnection(self.tracker_id, self.trk_init[0], 'disconnect')
-
-            if not self.trk_init[0]:
-                self.tracker_connected = False
-                self.tracker_id = 0
-
-                self.event_coord.set()
-                self.event_robot.set()
-                # if self.event_coord.is_set():
-                #     self.event_coord.clear()
-
-                Publisher.sendMessage('Update status text in GUI',
-                                        label=_("Tracker disconnected"))
-                print("Tracker disconnected!")
-            else:
-                Publisher.sendMessage('Update status text in GUI',
-                                        label=_("Tracker still connected"))
-                print("Tracker still connected!")
-
-    def IsTrackerInitialized(self):
-        return self.trk_init and self.tracker_id and self.tracker_connected
-
-    def AreTrackerFiducialsSet(self):
-        return not np.isnan(self.tracker_fiducials).any()
-
-    def SetTrackerFiducial(self, fiducial_index):
-        coord = None
-
-        coord_raw, markers_flag = self.TrackerCoordinates.GetCoordinates()
-
-        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]
-
-        # Update tracker fiducial with tracker coordinates
-        self.tracker_fiducials[fiducial_index, :] = coord[0:3]
-
-        assert 0 <= fiducial_index <= 2, "Fiducial index out of range (0-2): {}".format(fiducial_index)
-
-        self.tracker_fiducials_raw[2 * fiducial_index, :] = coord_raw[0, :]
-        self.tracker_fiducials_raw[2 * fiducial_index + 1, :] = coord_raw[1, :]
-
-        self.m_tracker_fiducials_raw[2 * fiducial_index, :] = dcr.compute_marker_transformation(coord_raw, 0)
-        self.m_tracker_fiducials_raw[2 * fiducial_index + 1, :] = dcr.compute_marker_transformation(coord_raw, 1)
-
-        print("Set tracker fiducial {} to coordinates {}.".format(fiducial_index, coord[0:3]))
-
-    def ResetTrackerFiducials(self):
-        for m in range(3):
-            self.tracker_fiducials[m, :] = [np.nan, np.nan, np.nan]
-
-    def GetTrackerFiducials(self):
-        return self.tracker_fiducials, self.tracker_fiducials_raw
-
-    def GetMatrixTrackerFiducials(self):
-        m_probe_ref_left = np.linalg.inv(self.m_tracker_fiducials_raw[1]) @ self.m_tracker_fiducials_raw[0]
-        m_probe_ref_right = np.linalg.inv(self.m_tracker_fiducials_raw[3]) @ self.m_tracker_fiducials_raw[2]
-        m_probe_ref_nasion = np.linalg.inv(self.m_tracker_fiducials_raw[5]) @ self.m_tracker_fiducials_raw[4]
-
-        return [m_probe_ref_left, m_probe_ref_right, m_probe_ref_nasion]
-
-    def GetTrackerInfo(self):
-        return self.trk_init, self.tracker_id, self.ref_mode_id
-
-    def SetReferenceMode(self, value):
-        self.ref_mode_id = value
-
-        # When ref mode is changed the tracker coordinates are set to zero
-        self.ResetTrackerFiducials()
-
-        # Some trackers do not accept restarting within this time window
-        # TODO: Improve the restarting of trackers after changing reference mode
-        Publisher.sendMessage('Update tracker initializer',
-                              nav_prop=(self.tracker_id, self.trk_init, self.TrackerCoordinates, self.ref_mode_id))
-
-    def GetReferenceMode(self):
-        return self.ref_mode_id
-
-    def UpdateUI(self, selection_ctrl, numctrls_fiducial, txtctrl_fre):
-        if self.tracker_connected:
-            selection_ctrl.SetSelection(self.tracker_id)
-        else:
-            selection_ctrl.SetSelection(0)
-
-        # Update tracker location in the UI.
-        for m in range(3):
-            coord = self.tracker_fiducials[m, :]
-            for n in range(0, 3):
-                value = 0.0 if np.isnan(coord[n]) else float(coord[n])
-                numctrls_fiducial[m][n].SetValue(value)
-
-        txtctrl_fre.SetValue('')
-        txtctrl_fre.SetBackgroundColour('WHITE')
-
-    def get_trackers(self):
-        return const.TRACKERS
-
-class ICP():
-    def __init__(self):
-        self.use_icp = False
-        self.m_icp = None
-        self.icp_fre = None
-
-    def StartICP(self, navigation, tracker):
-        if not self.use_icp:
-            if dlg.ICPcorregistration(navigation.fre):
-                Publisher.sendMessage('Stop navigation')
-                use_icp, self.m_icp = self.OnICP(tracker, navigation.m_change)
-                if use_icp:
-                    self.icp_fre = db.calculate_fre(tracker.tracker_fiducials_raw, navigation.all_fiducials,
-                                                    tracker.ref_mode_id, navigation.m_change, self.m_icp)
-                    self.SetICP(navigation, use_icp)
-                else:
-                    print("ICP canceled")
-                Publisher.sendMessage('Start navigation')
-
-    def OnICP(self, tracker, m_change):
-        ref_mode_id = tracker.GetReferenceMode()
-
-        dialog = dlg.ICPCorregistrationDialog(nav_prop=(m_change, tracker.tracker_id, tracker.trk_init, ref_mode_id))
-
-        if dialog.ShowModal() == wx.ID_OK:
-            m_icp, point_coord, transformed_points, prev_error, final_error = dialog.GetValue()
-            # TODO: checkbox in the dialog to transfer the icp points to 3D viewer
-            #create markers
-            # for i in range(len(point_coord)):
-            #     img_coord = point_coord[i][0],-point_coord[i][1],point_coord[i][2], 0, 0, 0
-            #     transf_coord = transformed_points[i][0],-transformed_points[i][1],transformed_points[i][2], 0, 0, 0
-            #     Publisher.sendMessage('Create marker', coord=img_coord, marker_id=None, colour=(1,0,0))
-            #     Publisher.sendMessage('Create marker', coord=transf_coord, marker_id=None, colour=(0,0,1))
-            if m_icp is not None:
-                dlg.ReportICPerror(prev_error, final_error)
-                use_icp = True
-            else:
-                use_icp = False
-
-            return use_icp, m_icp
-
-        else:
-            return self.use_icp, self.m_icp
-
-    def SetICP(self, navigation, use_icp):
-        self.use_icp = use_icp
-        navigation.icp_queue.put_nowait([self.use_icp, self.m_icp])
-
-    def ResetICP(self):
-        self.use_icp = False
-        self.m_icp = None
-        self.icp_fre = None
-
-class Robot():
-    def __init__(self):
-        self.trk_init = None
-        self.robottarget_queue = None
-        self.objattarget_queue = None
-        self.process_tracker = None
-
-        self.__bind_events()
-
-    def __bind_events(self):
-        Publisher.subscribe(self.OnSendCoordinates, 'Send coord to robot')
-        Publisher.subscribe(self.OnUpdateRobotTargetMatrix, 'Robot target matrix')
-        Publisher.subscribe(self.OnObjectTarget, 'Coil at target')
-
-    def OnRobotConnection(self, tracker, robotcoordinates):
-        if not tracker.trk_init[0][0] or not tracker.trk_init[1][0]:
-            dlg.ShowNavigationTrackerWarning(tracker.tracker_id, tracker.trk_init[1])
-            tracker.tracker_id = 0
-            tracker.tracker_connected = False
-        else:
-            tracker.trk_init.append(robotcoordinates)
-            self.process_tracker = elfin_process.TrackerProcessing()
-            dlg_correg_robot = dlg.CreateTransformationMatrixRobot(tracker)
-            if dlg_correg_robot.ShowModal() == wx.ID_OK:
-                M_tracker_2_robot = dlg_correg_robot.GetValue()
-                db.transform_tracker_2_robot.M_tracker_2_robot = M_tracker_2_robot
-                self.robot_server = tracker.trk_init[1][0]
-                self.trk_init = tracker.trk_init
-            else:
-                dlg.ShowNavigationTrackerWarning(tracker.tracker_id, 'disconnect')
-                tracker.trk_init = None
-                tracker.tracker_id = 0
-                tracker.tracker_connected = False
-
-        Publisher.sendMessage('Update tracker initializer',
-                              nav_prop=(tracker.tracker_id, tracker.trk_init, tracker.TrackerCoordinates, tracker.ref_mode_id))
-
-    def StartRobotNavigation(self, tracker, robotcoordinates, coord_queue):
-        if tracker.event_robot.is_set():
-            tracker.event_robot.clear()
-        elfin_process.ControlRobot(self.trk_init, tracker, robotcoordinates,
-                                   [coord_queue, self.robottarget_queue,
-                                    self.objattarget_queue],
-                                   self.process_tracker, tracker.event_robot).start()
-
-    def OnSendCoordinates(self, coord):
-        self.robot_server.SendCoordinates(coord)
-
-    def OnUpdateRobotTargetMatrix(self, robot_tracker_flag, m_change_robot2ref):
-        try:
-            self.robottarget_queue.put_nowait([robot_tracker_flag, m_change_robot2ref])
-        except queue.Full:
-            print('full target')
-            pass
-
-    def OnObjectTarget(self, state):
-        try:
-            if self.objattarget_queue:
-                self.objattarget_queue.put_nowait(state)
-        except queue.Full:
-            #print('full flag target')
-            pass
-
-    def SetRobotQueues(self, queues):
-        self.robottarget_queue, self.objattarget_queue = queues
-
 class NeuronavigationPanel(wx.Panel):
-    def __init__(self, parent):
+    def __init__(self, parent, tracker, pedal_connection):
         wx.Panel.__init__(self, parent)
         try:
             default_colour = wx.SystemSettings.GetColour(wx.SYS_COLOUR_MENUBAR)
@@ -746,14 +332,18 @@ class NeuronavigationPanel(wx.Panel):
         self.__bind_events()
  
         # Initialize global variables
-        self.pedal_connection = PedalConnection() if HAS_PEDAL_CONNECTION else None
-        self.tracker = Tracker()
-        self.navigation = Navigation()
+        self.pedal_connection = pedal_connection
+        self.navigation = Navigation(
+            pedal_connection=pedal_connection,
+        )
         self.icp = ICP()
+        self.tracker = tracker
         self.robot = Robot()
         self.robotcoordinates = elfin_process.RobotCoordinates()
  
         self.nav_status = False
+        self.tracker_fiducial_being_set = None
+        self.current_coord = 0, 0, 0
  
         # Initialize list of buttons and numctrls for wx objects
         self.btns_set_fiducial = [None, None, None, None, None, None]
@@ -761,7 +351,7 @@ class NeuronavigationPanel(wx.Panel):
  
         # ComboBox for spatial tracker device selection
         tracker_options = [_("Select tracker:")] + self.tracker.get_trackers()
-        select_tracker_elem = wx.ComboBox(self, -1, "", size = (145,-1),
+        select_tracker_elem = wx.ComboBox(self, -1, "", size=(145, -1),
                                           choices=tracker_options, style=wx.CB_DROPDOWN|wx.CB_READONLY)
  
         tooltip = wx.ToolTip(_("Choose the tracking device"))
@@ -786,9 +376,12 @@ class NeuronavigationPanel(wx.Panel):
             label = fiducial['label']
             tip = fiducial['tip']
  
-            self.btns_set_fiducial[n] = wx.ToggleButton(self, button_id, label=label, size=wx.Size(45, 23))
-            self.btns_set_fiducial[n].SetToolTip(wx.ToolTip(tip))
-            self.btns_set_fiducial[n].Bind(wx.EVT_TOGGLEBUTTON, partial(self.OnImageFiducials, n))
+            ctrl = wx.ToggleButton(self, button_id, label=label)
+            ctrl.SetMinSize((gui_utils.calc_width_needed(ctrl, 3), -1))
+            ctrl.SetToolTip(wx.ToolTip(tip))
+            ctrl.Bind(wx.EVT_TOGGLEBUTTON, partial(self.OnImageFiducials, n))
+
+            self.btns_set_fiducial[n] = ctrl
  
         # Push buttons for tracker fiducials
         for n, fiducial in enumerate(const.TRACKER_FIDUCIALS):
@@ -796,15 +389,18 @@ class NeuronavigationPanel(wx.Panel):
             label = fiducial['label']
             tip = fiducial['tip']
  
-            self.btns_set_fiducial[n + 3] = wx.Button(self, button_id, label=label, size=wx.Size(45, 23))
-            self.btns_set_fiducial[n + 3].SetToolTip(wx.ToolTip(tip))
-            self.btns_set_fiducial[n + 3].Bind(wx.EVT_BUTTON, partial(self.OnTrackerFiducials, n))
+            ctrl = wx.ToggleButton(self, button_id, label=label)
+            ctrl.SetMinSize((gui_utils.calc_width_needed(ctrl, 3), -1))
+            ctrl.SetToolTip(wx.ToolTip(tip))
+            ctrl.Bind(wx.EVT_TOGGLEBUTTON, partial(self.OnTrackerFiducials, n, ctrl=ctrl))
+
+            self.btns_set_fiducial[n + 3] = ctrl
  
         # TODO: Find a better allignment between FRE, text and navigate button
         txt_fre = wx.StaticText(self, -1, _('FRE:'))
         txt_icp = wx.StaticText(self, -1, _('Refine:'))
  
-        if HAS_PEDAL_CONNECTION and self.pedal_connection.in_use:
+        if pedal_connection is not None and pedal_connection.in_use:
             txt_pedal_pressed = wx.StaticText(self, -1, _('Pedal pressed:'))
         else:
             txt_pedal_pressed = None
@@ -833,14 +429,14 @@ class NeuronavigationPanel(wx.Panel):
         self.checkbox_icp = checkbox_icp
  
         # An indicator for pedal trigger
-        if HAS_PEDAL_CONNECTION and self.pedal_connection.in_use:
+        if pedal_connection is not None and pedal_connection.in_use:
             tooltip = wx.ToolTip(_(u"Is the pedal pressed"))
             checkbox_pedal_pressed = wx.CheckBox(self, -1, _(' '))
             checkbox_pedal_pressed.SetValue(False)
             checkbox_pedal_pressed.Enable(False)
             checkbox_pedal_pressed.SetToolTip(tooltip)
  
-            self.pedal_connection.add_callback('gui', checkbox_pedal_pressed.SetValue)
+            pedal_connection.add_callback('gui', checkbox_pedal_pressed.SetValue)
  
             self.checkbox_pedal_pressed = checkbox_pedal_pressed
         else:
@@ -874,7 +470,7 @@ class NeuronavigationPanel(wx.Panel):
                            (checkbox_icp, 0, wx.ALIGN_CENTER_HORIZONTAL | wx.ALIGN_CENTER_VERTICAL)])
  
         pedal_sizer = wx.FlexGridSizer(rows=1, cols=2, hgap=5, vgap=5)
-        if HAS_PEDAL_CONNECTION and self.pedal_connection.in_use:
+        if HAS_PEDAL_CONNECTION and pedal_connection.in_use:
             pedal_sizer.AddMany([(txt_pedal_pressed, 0, wx.ALIGN_CENTER_HORIZONTAL | wx.ALIGN_CENTER_VERTICAL),
                                 (checkbox_pedal_pressed, 0, wx.ALIGN_CENTER_HORIZONTAL | wx.ALIGN_CENTER_VERTICAL)])
  
@@ -899,7 +495,7 @@ class NeuronavigationPanel(wx.Panel):
         Publisher.subscribe(self.LoadImageFiducials, 'Load image fiducials')
         Publisher.subscribe(self.SetImageFiducial, 'Set image fiducial')
         Publisher.subscribe(self.SetTrackerFiducial, 'Set tracker fiducial')
-        Publisher.subscribe(self.UpdateTriggerState, 'Update trigger state')
+        Publisher.subscribe(self.UpdateSerialPort, 'Update serial port')
         Publisher.subscribe(self.UpdateTrackObjectState, 'Update track object state')
         Publisher.subscribe(self.UpdateImageCoordinates, 'Set cross focal point')
         Publisher.subscribe(self.OnDisconnectTracker, 'Disconnect tracker')
@@ -920,14 +516,14 @@ class NeuronavigationPanel(wx.Panel):
         Publisher.subscribe(self.OnStartNavigation, 'Start navigation')
         Publisher.subscribe(self.OnStopNavigation, 'Stop navigation')
  
-    def LoadImageFiducials(self, marker_id, coord):
-        fiducial = self.GetFiducialByAttribute(const.IMAGE_FIDUCIALS, 'label', marker_id)
+    def LoadImageFiducials(self, label, coord):
+        fiducial = self.GetFiducialByAttribute(const.IMAGE_FIDUCIALS, 'label', label)
  
         fiducial_index = fiducial['fiducial_index']
         fiducial_name = fiducial['fiducial_name']
  
         if self.btns_set_fiducial[fiducial_index].GetValue():
-            print("Fiducial {} already set, not resetting".format(marker_id))
+            print("Fiducial {} already set, not resetting".format(label))
             return
  
         Publisher.sendMessage('Set image fiducial', fiducial_name=fiducial_name, coord=coord[0:3])
@@ -956,7 +552,12 @@ class NeuronavigationPanel(wx.Panel):
         fiducial = self.GetFiducialByAttribute(const.TRACKER_FIDUCIALS, 'fiducial_name', fiducial_name)
         fiducial_index = fiducial['fiducial_index']
  
-        self.tracker.SetTrackerFiducial(fiducial_index)
+        # XXX: The reference mode is fetched from navigation object, however it seems like not quite
+        #      navigation-related attribute here, as the reference mode used during the fiducial registration
+        #      is more concerned with the calibration than the navigation.
+        #
+        ref_mode_id = self.navigation.GetReferenceMode()
+        self.tracker.SetTrackerFiducial(ref_mode_id, fiducial_index)
  
         self.ResetICP()
         self.tracker.UpdateUI(self.select_tracker_elem, self.numctrls_fiducial[3:6], self.txtctrl_fre)
@@ -986,7 +587,7 @@ class NeuronavigationPanel(wx.Panel):
         self.navigation.seed_radius = data
  
     def UpdateSleep(self, data):
-        self.navigation.sleep_nav = data
+        self.navigation.UpdateSleep(data)
  
     def UpdateNumberThreads(self, data):
         self.navigation.n_threads = data
@@ -1005,7 +606,7 @@ class NeuronavigationPanel(wx.Panel):
  
     def UpdateImageCoordinates(self, position):
         # TODO: Change from world coordinates to matrix coordinates. They are better for multi software communication.
-        self.navigation.current_coord = position
+        self.current_coord = position
  
         for m in [0, 1, 2]:
             if not self.btns_set_fiducial[m].GetValue():
@@ -1015,11 +616,11 @@ class NeuronavigationPanel(wx.Panel):
     def UpdateObjectRegistration(self, data=None):
         self.navigation.obj_reg = data
  
-    def UpdateTrackObjectState(self, evt=None, flag=None, obj_name=None, polydata=None):
+    def UpdateTrackObjectState(self, evt=None, flag=None, obj_name=None, polydata=None, use_default_object=True):
         self.navigation.track_obj = flag
  
-    def UpdateTriggerState(self, trigger_state):
-        self.navigation.trigger_state = trigger_state
+    def UpdateSerialPort(self, serial_port):
+        self.navigation.serial_port = serial_port
  
     def ResetICP(self):
         self.icp.ResetICP()
@@ -1032,19 +633,14 @@ class NeuronavigationPanel(wx.Panel):
         self.tracker.UpdateUI(self.select_tracker_elem, self.numctrls_fiducial[3:6], self.txtctrl_fre)
  
     def OnChooseTracker(self, evt, ctrl):
-        #Publisher.sendMessage('Update status text in GUI',
-        #                      label=_("Configuring tracker ..."))
+        Publisher.sendMessage('Update status text in GUI',
+                              label=_("Configuring tracker ..."))
         if hasattr(evt, 'GetSelection'):
             choice = evt.GetSelection()
         else:
             choice = None
  
         self.tracker.SetTracker(choice)
-        #sleep(5)
-        # dco.ReceiveCoordinates(self.tracker.trk_init, self.tracker.tracker_id,
-        #                        self.tracker.TrackerCoordinates, self.navigation.event).start()
-        #sleep(5)
-
         if self.tracker.tracker_id == const.ROBOT:
             self.robot.SetRobotQueues([self.navigation.robottarget_queue,
                                        self.navigation.objattarget_queue])
@@ -1060,7 +656,14 @@ class NeuronavigationPanel(wx.Panel):
         Publisher.sendMessage('Update status text in GUI', label=_("Ready"))
  
     def OnChooseReferenceMode(self, evt, ctrl):
-        self.tracker.SetReferenceMode(evt.GetSelection())
+        self.navigation.SetReferenceMode(evt.GetSelection())
+
+        # When ref mode is changed the tracker coordinates are set to zero
+        self.tracker.ResetTrackerFiducials()
+
+        # Some trackers do not accept restarting within this time window
+        # TODO: Improve the restarting of trackers after changing reference mode
+
         self.ResetICP()
  
         print("Reference mode changed!")
@@ -1069,7 +672,7 @@ class NeuronavigationPanel(wx.Panel):
         fiducial_name = const.IMAGE_FIDUCIALS[n]['fiducial_name']
  
         # XXX: This is still a bit hard to read, could be cleaned up.
-        marker_id = list(const.BTNS_IMG_MARKERS[evt.GetId()].values())[0]
+        label = list(const.BTNS_IMG_MARKERS[evt.GetId()].values())[0]
  
         if self.btns_set_fiducial[n].GetValue():
             coord = self.numctrls_fiducial[n][0].GetValue(),\
@@ -1083,17 +686,40 @@ class NeuronavigationPanel(wx.Panel):
             seed = 3 * [0.]
  
             Publisher.sendMessage('Create marker', coord=coord, colour=colour, size=size,
-                                   marker_id=marker_id, seed=seed)
+                                   label=label, seed=seed)
         else:
             for m in [0, 1, 2]:
                 self.numctrls_fiducial[n][m].SetValue(float(self.current_coord[m]))
  
             Publisher.sendMessage('Set image fiducial', fiducial_name=fiducial_name, coord=np.nan)
-            Publisher.sendMessage('Delete fiducial marker', marker_id=marker_id)
+            Publisher.sendMessage('Delete fiducial marker', label=label)
  
-    def OnTrackerFiducials(self, n, evt):
-        fiducial_name = const.TRACKER_FIDUCIALS[n]['fiducial_name']
-        Publisher.sendMessage('Set tracker fiducial', fiducial_name=fiducial_name)
+    def OnTrackerFiducials(self, n, evt, ctrl):
+
+        # Do not allow several tracker fiducials to be set at the same time.
+        if self.tracker_fiducial_being_set is not None and self.tracker_fiducial_being_set != n:
+            ctrl.SetValue(False)
+            return
+
+        # Called when the button for setting the tracker fiducial is enabled and either pedal is pressed
+        # or the button is pressed again.
+        #
+        def set_fiducial_callback(state):
+            if state:
+                fiducial_name = const.TRACKER_FIDUCIALS[n]['fiducial_name']
+                Publisher.sendMessage('Set tracker fiducial', fiducial_name=fiducial_name)
+                if self.pedal_connection is not None:
+                    self.pedal_connection.remove_callback('fiducial')
+
+                ctrl.SetValue(False)
+                self.tracker_fiducial_being_set = None
+
+        if ctrl.GetValue():
+            self.tracker_fiducial_being_set = n
+            if self.pedal_connection is not None:
+                self.pedal_connection.add_callback('fiducial', set_fiducial_callback)
+        else:
+            set_fiducial_callback(True)
  
     def OnStopNavigation(self):
         select_tracker_elem = self.select_tracker_elem
@@ -1197,14 +823,16 @@ class NeuronavigationPanel(wx.Panel):
         # TODO: Reset camera initial focus
         Publisher.sendMessage('Reset cam clipping range')
         self.navigation.StopNavigation()
-        self.navigation.__init__()
+        self.navigation.__init__(
+            pedal_connection=self.pedal_connection,
+        )
         self.tracker.__init__()
         self.icp.__init__()
         self.robot.__init__()
  
  
 class ObjectRegistrationPanel(wx.Panel):
-    def __init__(self, parent):
+    def __init__(self, parent, tracker, pedal_connection):
         wx.Panel.__init__(self, parent)
         try:
             default_colour = wx.SystemSettings.GetColour(wx.SYS_COLOUR_MENUBAR)
@@ -1214,6 +842,9 @@ class ObjectRegistrationPanel(wx.Panel):
  
         self.coil_list = const.COIL
  
+        self.tracker = tracker
+        self.pedal_connection = pedal_connection
+
         self.nav_prop = None
         self.obj_fiducials = None
         self.obj_orients = None
@@ -1323,14 +954,10 @@ class ObjectRegistrationPanel(wx.Panel):
         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')
         Publisher.subscribe(self.OnRemoveObject, 'Remove object data')
  
-    def UpdateTrackerInit(self, nav_prop):
-        self.nav_prop = nav_prop
-
     def UpdateNavigationStatus(self, nav_status, vis_status):
         if nav_status:
             self.checkrecordcoords.Enable(1)
@@ -1378,11 +1005,11 @@ class ObjectRegistrationPanel(wx.Panel):
  
     def OnLinkCreate(self, event=None):
  
-        if self.nav_prop:
-            dialog = dlg.ObjectCalibrationDialog(self.nav_prop)
+        if self.tracker.IsTrackerInitialized():
+            dialog = dlg.ObjectCalibrationDialog(self.tracker, self.pedal_connection)
             try:
                 if dialog.ShowModal() == wx.ID_OK:
-                    self.obj_fiducials, self.obj_orients, self.obj_ref_mode, self.obj_name, polydata = dialog.GetValue()
+                    self.obj_fiducials, self.obj_orients, self.obj_ref_mode, self.obj_name, polydata, use_default_object = 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',
@@ -1391,7 +1018,13 @@ class ObjectRegistrationPanel(wx.Panel):
                                               label=_("Ready"))
                         # Enable automatically Track object, Show coil and disable Vol. Camera
                         self.checktrack.SetValue(True)
-                        Publisher.sendMessage('Update track object state', flag=True, obj_name=self.obj_name, polydata=polydata)
+                        Publisher.sendMessage(
+                            'Update track object state',
+                            flag=True,
+                            obj_name=self.obj_name,
+                            polydata=polydata,
+                            use_default_object=use_default_object,
+                        )
                         Publisher.sendMessage('Change camera checkbox', status=False)
  
             except wx._core.PyAssertionError:  # TODO FIX: win64
@@ -1468,6 +1101,76 @@ class ObjectRegistrationPanel(wx.Panel):
  
  
 class MarkersPanel(wx.Panel):
+    @dataclasses.dataclass
+    class Marker:
+        """Class for storing markers. @dataclass decorator simplifies
+        setting default values, serialization, etc."""
+        x : float = 0
+        y : float = 0
+        z : float = 0
+        alpha : float = 0
+        beta : float = 0
+        gamma : float = 0
+        r : float = 0
+        g : float = 1
+        b : float = 0
+        size : int = 2
+        label : str = '*'
+        x_seed : float = 0
+        y_seed : float = 0
+        z_seed : float = 0
+        is_target : int = 0 # is_target is int instead of boolean to avoid
+                            # problems with CSV export
+
+        # x, y, z, alpha, beta, gamma can be jointly accessed as coord
+        @property
+        def coord(self):
+            return list((self.x, self.y, self.z, self.alpha, self.beta, self.gamma),)
+
+        @coord.setter
+        def coord(self, new_coord):
+            self.x, self.y, self.z, self.alpha, self.beta, self.gamma = new_coord
+
+        # r, g, b can be jointly accessed as colour
+        @property
+        def colour(self):
+            return list((self.r, self.g, self.b),)
+
+        @colour.setter
+        def colour(self, new_colour):
+            self.r, self.g, self.b = new_colour
+
+        # x_seed, y_seed, z_seed can be jointly accessed as seed
+        @property
+        def seed(self):
+            return list((self.x_seed, self.y_seed, self.z_seed),)
+
+        @seed.setter
+        def seed(self, new_seed):
+            self.x_seed, self.y_seed, self.z_seed = new_seed
+
+        @classmethod
+        def get_headers(cls):
+            """Return the list of field names (headers) for exporting to csv."""
+            res = [field.name for field in dataclasses.fields(cls)]
+            res.extend(['x_world', 'y_world', 'z_world', 'alpha_world', 'beta_world', 'gamma_world'])
+            return res
+
+        def get_values(self):
+            """Return the list of values for exporting to csv."""
+            res = []
+            res.extend(dataclasses.astuple(self))
+
+            # Add world coordinates (in addition to the internal ones).
+            position_world, orientation_world = imagedata_utils.convert_invesalius_to_world(
+                position=[self.x, self.y, self.z],
+                orientation=[self.alpha, self.beta, self.gamma],
+            )
+            res.extend(position_world)
+            res.extend(orientation_world)
+
+            return res
+
     def __init__(self, parent):
         wx.Panel.__init__(self, parent)
         try:
@@ -1483,6 +1186,7 @@ class MarkersPanel(wx.Panel):
         self.current_coord = 0, 0, 0, 0, 0, 0
         self.current_angle = 0, 0, 0
         self.current_seed = 0, 0, 0
+        self.markers = []
         self.current_ref = 0, 0, 0, 0, 0, 0
         self.current_robot = 0, 0, 0, 0, 0, 0
         self.list_coord = []
@@ -1492,12 +1196,12 @@ class MarkersPanel(wx.Panel):
         self.mchange = None
         self.flag_target = False
  
-        # self.timer = wx.Timer(self)
-        # self.Bind(wx.EVT_TIMER, self.OnUpdateSendCoord, self.timer)
-
         self.marker_colour = const.MARKER_COLOUR
         self.marker_size = const.MARKER_SIZE
  
+        # Define CSV dialect for saving/loading markers
+        csv.register_dialect('markers_dialect', delimiter='\t', quoting=csv.QUOTE_NONNUMERIC)
+
         # Change marker size
         spin_size = wx.SpinCtrl(self, -1, "", size=wx.Size(40, 23))
         spin_size.SetRange(1, 99)
@@ -1534,7 +1238,7 @@ class MarkersPanel(wx.Panel):
  
         # Buttons to delete or remove markers
         btn_delete_single = wx.Button(self, -1, label=_('Remove'), size=wx.Size(65, 23))
-        btn_delete_single.Bind(wx.EVT_BUTTON, self.OnDeleteSingleMarker)
+        btn_delete_single.Bind(wx.EVT_BUTTON, self.OnDeleteMultipleMarkers)
  
         btn_delete_all = wx.Button(self, -1, label=_('Delete all'), size=wx.Size(135, 23))
         btn_delete_all.Bind(wx.EVT_BUTTON, self.OnDeleteAllMarkers)
@@ -1550,11 +1254,15 @@ class MarkersPanel(wx.Panel):
         self.lc.InsertColumn(2, 'Y')
         self.lc.InsertColumn(3, 'Z')
         self.lc.InsertColumn(4, 'ID')
+        self.lc.InsertColumn(5, 'Target')
+
         self.lc.SetColumnWidth(0, 28)
         self.lc.SetColumnWidth(1, 50)
         self.lc.SetColumnWidth(2, 50)
         self.lc.SetColumnWidth(3, 50)
         self.lc.SetColumnWidth(4, 60)
+        self.lc.SetColumnWidth(5, 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)
@@ -1573,7 +1281,7 @@ class MarkersPanel(wx.Panel):
     def __bind_events(self):
         # Publisher.subscribe(self.UpdateCurrentCoord, 'Co-registered points')
         Publisher.subscribe(self.UpdateCurrentCoord, 'Set cross focal point')
-        Publisher.subscribe(self.OnDeleteSingleMarker, 'Delete fiducial marker')
+        Publisher.subscribe(self.OnDeleteMultipleMarkers, 'Delete fiducial marker')
         Publisher.subscribe(self.OnDeleteAllMarkers, 'Delete all markers')
         Publisher.subscribe(self.CreateMarker, 'Create marker')
         Publisher.subscribe(self.UpdateNavigationStatus, 'Navigation status')
@@ -1584,6 +1292,70 @@ class MarkersPanel(wx.Panel):
         Publisher.subscribe(self.UpdateObjectMarker2Center, 'Update object marker to center')
         Publisher.subscribe(self.OnObjectTarget, 'Coil at target')
  
+    def __find_target_marker(self):
+        """Return the index of the marker currently selected as target (there
+        should be at most one). If there is no such marker, return -1."""
+        for i in range(len(self.markers)):
+            if self.markers[i].is_target:
+                return i
+
+        return -1
+
+    def __get_selected_items(self):
+        """
+        Returns a (possibly empty) list of the selected items in the list control.
+        """
+        selection = []
+
+        next = self.lc.GetFirstSelected()
+
+        while next != -1:
+            selection.append(next)
+            next = self.lc.GetNextSelected(next)
+
+        return selection
+
+    def __delete_multiple_markers(self, index):
+        """ Delete multiple markers indexed by index. index must be sorted in
+        the ascending order.
+        """
+        for i in reversed(index):
+            del self.markers[i]
+            self.lc.DeleteItem(i)
+            for n in range(0, self.lc.GetItemCount()):
+                self.lc.SetItem(n, 0, str(n+1))
+        Publisher.sendMessage('Remove multiple markers', index=index)
+
+    def __set_marker_as_target(self, idx):
+        """Set marker indexed by idx as the new target. idx must be a valid index."""
+        # Find the previous target
+        prev_idx = self.__find_target_marker()
+
+        # If the new target is same as the previous do nothing.
+        if prev_idx == idx:
+            return
+
+        # Unset the previous target
+        if prev_idx != -1:
+            self.markers[prev_idx].is_target = 0
+            self.lc.SetItemBackgroundColour(prev_idx, 'white')
+            Publisher.sendMessage('Set target transparency', status=False, index=prev_idx)
+            self.lc.SetItem(prev_idx, 5, "")
+
+        # Set the new target
+        self.markers[idx].is_target = 1
+        self.lc.SetItemBackgroundColour(idx, 'RED')
+        self.lc.SetItem(idx, 5, _("Yes"))
+
+        Publisher.sendMessage('Update target', coord=self.markers[idx].coord)
+        Publisher.sendMessage('Set target transparency', status=True, index=idx)
+        wx.MessageBox(_("New target selected."), _("InVesalius 3"))
+
+    @staticmethod
+    def __list_fiducial_labels():
+        """Return the list of marker labels denoting fucials."""
+        return list(itertools.chain(*(const.BTNS_IMG_MARKERS[i].values() for i in const.BTNS_IMG_MARKERS)))
+
     def UpdateCurrentCoord(self, position):
         self.current_coord = position
         #self.current_angle = pubsub_evt.data[1][3:]
@@ -1620,7 +1392,7 @@ class MarkersPanel(wx.Panel):
         # TODO: Enable the "Set as target" only when target is created with registered object
         menu_id = wx.Menu()
         edit_id = menu_id.Append(0, _('Edit ID'))
-        menu_id.Bind(wx.EVT_MENU, self.OnMenuEditMarkerId, edit_id)
+        menu_id.Bind(wx.EVT_MENU, self.OnMenuEditMarkerLabel, edit_id)
         color_id = menu_id.Append(2, _('Edit color'))
         menu_id.Bind(wx.EVT_MENU, self.OnMenuSetColor, color_id)
         menu_id.AppendSeparator()
@@ -1644,54 +1416,29 @@ class MarkersPanel(wx.Panel):
     def OnStopItemBlink(self, evt):
         Publisher.sendMessage('Stop Blink Marker')
  
-    def OnMenuEditMarkerId(self, evt):
+    def OnMenuEditMarkerLabel(self, evt):
         list_index = self.lc.GetFocusedItem()
-        if evt == 'TARGET':
-            id_label = evt
+        if list_index != -1:
+            new_label = dlg.ShowEnterMarkerID(self.lc.GetItemText(list_index, 4))
+            self.markers[list_index].label = str(new_label)
+            self.lc.SetItem(list_index, 4, new_label)
         else:
-            id_label = dlg.ShowEnterMarkerID(self.lc.GetItemText(list_index, 4))
-            if id_label == 'TARGET':
-                id_label = '*'
-                wx.MessageBox(_("Invalid TARGET ID."), _("InVesalius 3"))
-
-        # Add the new ID to exported list
-        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)
-
-        self.lc.SetItem(list_index, 4, id_label)
+            wx.MessageBox(_("No data selected."), _("InVesalius 3"))
  
     def OnMenuSetTarget(self, evt):
-        if isinstance(evt, int):
-            self.lc.Focus(evt)
-
-        if self.tgt_flag:
-            marker_id = '*'
-
-            self.lc.SetItemBackgroundColour(self.tgt_index, 'white')
-            Publisher.sendMessage('Set target transparency', status=False, index=self.tgt_index)
-            self.lc.SetItem(self.tgt_index, 4, marker_id)
-
-            # Add the new ID to exported list
-            if len(self.list_coord[self.tgt_index]) > 8:
-                self.list_coord[self.tgt_index][10] = marker_id
-            else:
-                self.list_coord[self.tgt_index][7] = marker_id
-
-        self.tgt_index = self.lc.GetFocusedItem()
-        self.lc.SetItemBackgroundColour(self.tgt_index, 'RED')
-
-        Publisher.sendMessage('Update target', coord=self.list_coord[self.tgt_index][:6])
-        Publisher.sendMessage('Set target transparency', status=True, index=self.tgt_index)
-        self.OnMenuEditMarkerId('TARGET')
-        self.tgt_flag = True
-        wx.MessageBox(_("New target selected."), _("InVesalius 3"))
+        idx = self.lc.GetFocusedItem()
+        if idx != -1:
+            self.__set_marker_as_target(idx)
+        else:
+            wx.MessageBox(_("No data selected."), _("InVesalius 3"))
  
     def OnMenuSetColor(self, evt):
         index = self.lc.GetFocusedItem()
+        if index == -1:
+            wx.MessageBox(_("No data selected."), _("InVesalius 3"))
+            return
  
-        color_current = [self.list_coord[index][n] * 255 for n in range(6, 9)]
+        color_current = [ch * 255 for ch in self.markers[index].colour]
  
         color_new = dlg.ShowColorDialog(color_current=color_current)
  
@@ -1701,7 +1448,7 @@ class MarkersPanel(wx.Panel):
             # XXX: Seems like a slightly too early point for rounding; better to round only when the value
             #      is printed to the screen or file.
             #
-            self.list_coord[index][6:9] = [round(s/255.0, 3) for s in color_new]
+            self.markers[index].colour = [round(s/255.0, 3) for s in color_new]
  
             Publisher.sendMessage('Set new color', index=index, color=color_new)
  
@@ -1738,151 +1485,95 @@ class MarkersPanel(wx.Panel):
  
  
     def OnDeleteAllMarkers(self, evt=None):
-        if self.list_coord:
-            if evt is None:
-                result = wx.ID_OK
-            else:
-                # result = dlg.DeleteAllMarkers()
-                result = dlg.ShowConfirmationDialog(msg=_("Remove all markers? Cannot be undone."))
-
-            if result == wx.ID_OK:
-                self.list_coord = []
-                self.marker_ind = 0
-                Publisher.sendMessage('Remove all markers', indexes=self.lc.GetItemCount())
-                self.lc.DeleteAllItems()
-                Publisher.sendMessage('Stop Blink Marker', index='DeleteAll')
-
-                if self.tgt_flag:
-                    self.tgt_flag = self.tgt_index = None
-                    Publisher.sendMessage('Disable or enable coil tracker', status=False)
-                    if not hasattr(evt, 'data'):
-                        wx.MessageBox(_("Target deleted."), _("InVesalius 3"))
-
-    def OnDeleteSingleMarker(self, evt=None, marker_id=None):
-        # OnDeleteSingleMarker is used for both pubsub and button click events
+        if evt is None:
+            result = wx.ID_OK
+        else:
+            result = dlg.ShowConfirmationDialog(msg=_("Remove all markers? Cannot be undone."))
+
+        if result != wx.ID_OK:
+            return
+
+        if self.__find_target_marker() != -1:
+            Publisher.sendMessage('Disable or enable coil tracker', status=False)
+            if evt is not None:
+                wx.MessageBox(_("Target deleted."), _("InVesalius 3"))
+
+        self.markers = []
+        Publisher.sendMessage('Remove all markers', indexes=self.lc.GetItemCount())
+        self.lc.DeleteAllItems()
+        Publisher.sendMessage('Stop Blink Marker', index='DeleteAll')
+
+    def OnDeleteMultipleMarkers(self, evt=None, label=None):
+        # OnDeleteMultipleMarkers is used for both pubsub and button click events
         # Pubsub is used for fiducial handle and button click for all others
  
-        if not evt:
-            if self.lc.GetItemCount():
+        if not evt: # called through pubsub
+            index = []
+
+            if label and (label in self.__list_fiducial_labels()):
                 for id_n in range(self.lc.GetItemCount()):
                     item = self.lc.GetItem(id_n, 4)
-                    if item.GetText() == marker_id:
-                        for i in const.BTNS_IMG_MARKERS:
-                            if marker_id in list(const.BTNS_IMG_MARKERS[i].values())[0]:
-                                self.lc.Focus(item.GetId())
-                index = [self.lc.GetFocusedItem()]
-        else:
-            if self.lc.GetFirstSelected() != -1:
-                index = self.GetSelectedItems()
-            else:
-                index = None
+                    if item.GetText() == label:
+                        self.lc.Focus(item.GetId())
+                        index = [self.lc.GetFocusedItem()]
+
+        else:       # called from button click
+            index = self.__get_selected_items()
  
-        #TODO: There are bugs when no marker is selected, test and improve
         if index:
-            if self.tgt_flag and self.tgt_index == index[0]:
-                self.tgt_flag = self.tgt_index = None
+            if self.__find_target_marker() in index:
                 Publisher.sendMessage('Disable or enable coil tracker', status=False)
+                wx.MessageBox(_("Target deleted."), _("InVesalius 3"))
+
+            self.__delete_multiple_markers(index)
+        else:
+            if evt: # Don't show the warning if called through pubsub
                 #TODO: reset robot target. (target should be the same target as invesalius?)
                 Publisher.sendMessage('Robot target matrix', robot_tracker_flag=False,
                                       m_change_robot2ref=None)
                 wx.MessageBox(_("No data selected."), _("InVesalius 3"))
  
-            self.DeleteMarker(index)
-        else:
-            wx.MessageBox(_("Target deleted."), _("InVesalius 3"))
-
-    def DeleteMarker(self, index):
-        for i in reversed(index):
-            del self.list_coord[i]
-            self.lc.DeleteItem(i)
-            for n in range(0, self.lc.GetItemCount()):
-                self.lc.SetItem(n, 0, str(n+1))
-            self.marker_ind -= 1
-        Publisher.sendMessage('Remove marker', index=index)
-
-    def OnCreateMarker(self, evt=None, coord=None, marker_id=None, colour=None):
+    def OnCreateMarker(self, evt):
         self.CreateMarker()
  
     def OnLoadMarkers(self, evt):
+        """Loads markers from file and appends them to the current marker list.
+        The file should contain no more than a single target marker. Also the
+        file should not contain any fiducials already in the list."""
         filename = dlg.ShowLoadSaveDialog(message=_(u"Load markers"),
                                           wildcard=const.WILDCARD_MARKER_FILES)
-        # data_dir = os.environ.get('OneDrive') + r'\data\dti_navigation\baran\anat_reg_improve_20200609'
-        # marker_path = 'markers.mks'
-        # filename = os.path.join(data_dir, marker_path)
  
-        if filename:
-            try:
-                count_line = self.lc.GetItemCount()
-                # content = [s.rstrip() for s in open(filename)]
-                with open(filename, 'r') as file:
-                    reader = csv.reader(file, delimiter='\t')
-
-                    # skip the header
-                    if filename.lower().endswith('.mkss'):
-                        next(reader)
-
-                    content = [row for row in reader]
-
-                for line in content:
-                    target = None
-                    if len(line) > 8:
-                        coord = [float(s) for s in line[:6]]
-                        colour = [float(s) for s in line[6:9]]
-                        size = float(line[9])
-                        marker_id = line[10]
-
-                        if len(line) > 11:
-                            seed = [float(s) for s in line[11:14]]
-                        else:
-                            seed = 3 * [0.]
-                        if len(line) > 12:
-                            robot = [float(s) for s in line[14:20]]
-                            ref = [float(s) for s in line[20:26]]
-                        else:
-                            robot = 0., 0., 0., 0., 0., 0.
-                            ref = 0., 0., 0., 0., 0., 0.
-
-                        if len(line) >= 11:
-                            for i in const.BTNS_IMG_MARKERS:
-                                if marker_id in list(const.BTNS_IMG_MARKERS[i].values())[0]:
-                                    Publisher.sendMessage('Load image fiducials', marker_id=marker_id, coord=coord)
-                                elif marker_id == 'TARGET':
-                                    target = count_line
-                        else:
-                            marker_id = '*'
-
-                        if len(line) == 15:
-                            target_id = line[14]
-                        else:
-                            target_id = '*'
-                    else:
-                        # for compatibility with previous version without the extra seed and target columns
-                        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])
-
-                        seed = 3 * [0]
-                        target_id = '*'
-
-                        if len(line) == 8:
-                            marker_id = line[7]
-                            for i in const.BTNS_IMG_MARKERS:
-                                if marker_id in list(const.BTNS_IMG_MARKERS[i].values())[0]:
-                                    Publisher.sendMessage('Load image fiducials', marker_id=marker_id, coord=coord)
-                        else:
-                            marker_id = '*'
-
-                    self.CreateMarker(coord=coord, colour=colour, size=size,
-                                      marker_id=marker_id, target_id=target_id, seed=seed,
-                                      robot=self.current_robot, ref=self.current_ref)
-
-                    # if there are multiple TARGETS will set the last one
-                    if target:
-                        self.OnMenuSetTarget(target)
-
-                    count_line += 1
-            except:
-                wx.MessageBox(_("Invalid markers file."), _("InVesalius 3"))
+        if not filename:
+            return
+
+        try:
+            with open(filename, 'r') as file:
+                magick_line = file.readline()
+                assert magick_line.startswith(const.MARKER_FILE_MAGICK_STRING)
+                ver = int(magick_line.split('_')[-1])
+                if ver != 0:
+                    wx.MessageBox(_("Unknown version of the markers file."), _("InVesalius 3"))
+                    return
+
+                reader = csv.reader(file, dialect='markers_dialect')
+                next(reader) # skip the header line
+
+                # Read the data lines and create markers
+                for line in reader:
+                    marker = self.Marker(*line[:-6]) # Discard the last 6 fields (the world coordinates)
+                    self.CreateMarker(coord=marker.coord, colour=marker.colour, size=marker.size,
+                                      label=marker.label, is_target=0, seed=marker.seed)
+
+                    if marker.label in self.__list_fiducial_labels():
+                        Publisher.sendMessage('Load image fiducials', label=marker.label, coord=marker.coord)
+
+                    # If the new marker has is_target=1 (True), we first create
+                    # a marker with is_target=0 (False), and then call __set_marker_as_target
+                    if marker.is_target:
+                        self.__set_marker_as_target(len(self.markers)-1)
+
+        except:
+            wx.MessageBox(_("Invalid markers file."), _("InVesalius 3"))
  
     def OnMarkersVisibility(self, evt, ctrl):
  
@@ -1905,79 +1596,55 @@ class MarkersPanel(wx.Panel):
         filename = dlg.ShowLoadSaveDialog(message=_(u"Save markers as..."),
                                           wildcard=const.WILDCARD_MARKER_FILES,
                                           style=wx.FD_SAVE | wx.FD_OVERWRITE_PROMPT,
-                                          default_filename="markers.mks", save_ext="mks")
-
-        header_titles = ['x', 'y', 'z', 'alpha', 'beta', 'gamma', 'r', 'g', 'b',
-                         'size', 'marker_id', 'x_seed', 'y_seed', 'z_seed', 'target_id']
+                                          default_filename=default_filename)
  
-        if filename:
-            if self.list_coord:
-                with open(filename, 'w', newline='') as file:
-                    writer = csv.writer(file, delimiter='\t')
-
-                    if filename.lower().endswith('.mkss'):
-                        writer.writerow(header_titles)
+        if not filename:
+            return
  
-                    writer.writerows(self.list_coord)
+        try:
+            with open(filename, 'w', newline='') as file:
+                file.writelines(['%s%i\n' % (const.MARKER_FILE_MAGICK_STRING, const.CURRENT_MARKER_FILE_VERSION)])
+                writer = csv.writer(file, dialect='markers_dialect')
+                writer.writerow(self.Marker.get_headers())
+                writer.writerows(marker.get_values() for marker in self.markers)
+                file.close()
+        except:
+            wx.MessageBox(_("Error writing markers file."), _("InVesalius 3"))
  
     def OnSelectColour(self, evt, ctrl):
-        self.marker_colour = [colour/255.0 for colour in ctrl.GetValue()]
+        #TODO: Make sure GetValue returns 3 numbers (without alpha)
+        self.marker_colour = [colour/255.0 for colour in ctrl.GetValue()][:3]
  
     def OnSelectSize(self, evt, ctrl):
         self.marker_size = ctrl.GetValue()
  
-    def CreateMarker(self, coord=None, colour=None, size=None, marker_id='*', target_id='*', seed=None, robot=None, ref=None):
-        coord = coord or self.current_coord
-        colour = colour or self.marker_colour
-        size = size or self.marker_size
-        seed = seed or self.current_seed
-        robot = robot or self.current_robot
-        ref = ref or self.current_ref
-        # TODO: Use matrix coordinates and not world coordinates as current method.
-        # This makes easier for inter-software comprehension.
-
-        Publisher.sendMessage('Add marker', ball_id=self.marker_ind, size=size, colour=colour,  coord=coord[0:3])
-
-        self.marker_ind += 1
-
-        # List of lists with coordinates and properties of a marker
-        line = []
-        line.extend(coord)
-        line.extend(colour)
-        line.append(size)
-        line.append(marker_id)
-        line.extend(seed)
-        line.extend(robot)
-        line.extend(ref)
-        line.append(target_id)
-
-        # Adding current line to a list of all markers already created
-        if not self.list_coord:
-            self.list_coord = [line]
-        else:
-            self.list_coord.append(line)
+    def CreateMarker(self, coord=None, colour=None, size=None, label='*', is_target=0, seed=None, robot=None, ref=None):
+        new_marker = self.Marker()
+        new_marker.coord = coord or self.current_coord
+        new_marker.colour = colour or self.marker_colour
+        new_marker.size = size or self.marker_size
+        new_marker.label = label
+        new_marker.is_target = is_target
+        new_marker.seed = seed or self.current_seed
+        new_marker.robot = robot or self.current_robot
+        new_marker.ref = ref or self.current_ref
+
+        # Note that ball_id is zero-based, so we assign it len(self.markers) before the new marker is added
+        Publisher.sendMessage('Add marker', ball_id=len(self.markers),
+                                            size=new_marker.size,
+                                            colour=new_marker.colour,
+                                            coord=new_marker.coord[:3])
+        self.markers.append(new_marker)
  
         # Add item to list control in panel
         num_items = self.lc.GetItemCount()
         self.lc.InsertItem(num_items, str(num_items + 1))
-        self.lc.SetItem(num_items, 1, str(round(coord[0], 2)))
-        self.lc.SetItem(num_items, 2, str(round(coord[1], 2)))
-        self.lc.SetItem(num_items, 3, str(round(coord[2], 2)))
-        self.lc.SetItem(num_items, 4, str(marker_id))
+        self.lc.SetItem(num_items, 1, str(round(new_marker.x, 2)))
+        self.lc.SetItem(num_items, 2, str(round(new_marker.y, 2)))
+        self.lc.SetItem(num_items, 3, str(round(new_marker.z, 2)))
+        self.lc.SetItem(num_items, 4, str(new_marker.label))
         self.lc.EnsureVisible(num_items)
  
-    def GetSelectedItems(self):
-        """    
-        Returns a list of the selected items in the list control.
-        """
-        selection = []
-        index = self.lc.GetFirstSelected()
-        selection.append(index)
-        while len(selection) != self.lc.GetSelectedItemCount():
-            index = self.lc.GetNextSelected(index)
-            selection.append(index)
-        return selection
-
 class DbsPanel(wx.Panel):
     def __init__(self, parent):
         wx.Panel.__init__(self, parent)
@@ -2429,101 +2096,6 @@ class TractographyPanel(wx.Panel):
         Publisher.sendMessage('Remove tracts')
  
  
-class QueueCustom(queue.Queue):
-    """
-    A custom queue subclass that provides a :meth:`clear` method.
-    https://stackoverflow.com/questions/6517953/clear-all-items-from-the-queue
-    Modified to a LIFO Queue type (Last-in-first-out). Seems to make sense for the navigation
-    threads, as the last added coordinate should be the first to be processed.
-    In the first tests in a short run, seems to increase the coord queue size considerably,
-    possibly limiting the queue size is good.
-    """
-
-    def clear(self):
-        """
-        Clears all items from the queue.
-        """
-
-        with self.mutex:
-            unfinished = self.unfinished_tasks - len(self.queue)
-            if unfinished <= 0:
-                if unfinished < 0:
-                    raise ValueError('task_done() called too many times')
-                self.all_tasks_done.notify_all()
-            self.unfinished_tasks = unfinished
-            self.queue.clear()
-            self.not_full.notify_all()
-
-
-class UpdateNavigationScene(threading.Thread):
-
-    def __init__(self, vis_queues, vis_components, event, sle):
-        """Class (threading) to update the navigation scene with all graphical elements.
-
-        Sleep function in run method is used to avoid blocking GUI and more fluent, real-time navigation
-
-        :param affine_vtk: Affine matrix in vtkMatrix4x4 instance to update objects position in 3D scene
-        :type affine_vtk: vtkMatrix4x4
-        :param visualization_queue: Queue instance that manage coordinates to be visualized
-        :type visualization_queue: queue.Queue
-        :param event: Threading event to coordinate when tasks as done and allow UI release
-        :type event: threading.Event
-        :param sle: Sleep pause in seconds
-        :type sle: float
-        """
-
-        threading.Thread.__init__(self, name='UpdateScene')
-        self.trigger_state, self.view_tracts, self.peel_loaded = vis_components
-        self.coord_queue, self.trigger_queue, self.tracts_queue, self.icp_queue, self.robottarget_queue = vis_queues
-        self.sle = sle
-        self.event = event
-
-    def run(self):
-        # count = 0
-        while not self.event.is_set():
-            got_coords = False
-            try:
-                coord, [coord_raw, markers_flag], m_img, view_obj = self.coord_queue.get_nowait()
-                got_coords = True
-
-                # print('UpdateScene: get {}'.format(count))
-
-                # use of CallAfter is mandatory otherwise crashes the wx interface
-                if self.view_tracts:
-                    bundle, affine_vtk, coord_offset = self.tracts_queue.get_nowait()
-                    #TODO: Check if possible to combine the Remove tracts with Update tracts in a single command
-                    wx.CallAfter(Publisher.sendMessage, 'Remove tracts')
-                    wx.CallAfter(Publisher.sendMessage, 'Update tracts', root=bundle,
-                                 affine_vtk=affine_vtk, coord_offset=coord_offset)
-                    # wx.CallAfter(Publisher.sendMessage, 'Update marker offset', coord_offset=coord_offset)
-                    self.tracts_queue.task_done()
-
-                if self.trigger_state:
-                    trigger_on = self.trigger_queue.get_nowait()
-                    if trigger_on:
-                        wx.CallAfter(Publisher.sendMessage, 'Create marker')
-                    self.trigger_queue.task_done()
-
-                #TODO: If using the view_tracts substitute the raw coord from the offset coordinate, so the user
-                # see the red cross in the position of the offset marker
-                wx.CallAfter(Publisher.sendMessage, 'Update slices position', position=coord[:3])
-                wx.CallAfter(Publisher.sendMessage, 'Set cross focal point', position=coord)
-                wx.CallAfter(Publisher.sendMessage, 'Update raw coord', coord_raw=coord_raw,  markers_flag=markers_flag)
-                wx.CallAfter(Publisher.sendMessage, 'Update slice viewer')
-
-                if view_obj:
-                    wx.CallAfter(Publisher.sendMessage, 'Update object matrix', m_img=m_img, coord=coord)
-                    wx.CallAfter(Publisher.sendMessage, 'Update object arrow matrix',m_img=m_img, coord=coord, flag= self.peel_loaded)
-                self.coord_queue.task_done()
-                # print('UpdateScene: done {}'.format(count))
-                # count += 1
-
-                sleep(self.sle)
-            except queue.Empty:
-                if got_coords:
-                    self.coord_queue.task_done()
-
-
 class InputAttributes(object):
     # taken from https://stackoverflow.com/questions/2466191/set-attributes-from-dictionary-in-python
     def __init__(self, *initial_data, **kwargs):
@@ -0,0 +1,80 @@
+#--------------------------------------------------------------------------
+# 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 wx
+
+import invesalius.data.bases as db
+import invesalius.gui.dialogs as dlg
+from invesalius.pubsub import pub as Publisher
+
+
+class ICP():
+    def __init__(self):
+        self.use_icp = False
+        self.m_icp = None
+        self.icp_fre = None
+
+    def StartICP(self, navigation, tracker):
+        ref_mode_id = navigation.GetReferenceMode()
+
+        if not self.use_icp:
+            if dlg.ICPcorregistration(navigation.fre):
+                Publisher.sendMessage('Stop navigation')
+                use_icp, self.m_icp = self.OnICP(navigation, tracker, navigation.m_change)
+                if use_icp:
+                    self.icp_fre = db.calculate_fre(tracker.tracker_fiducials_raw, navigation.all_fiducials,
+                                                    ref_mode_id, navigation.m_change, self.m_icp)
+                    self.SetICP(navigation, use_icp)
+                else:
+                    print("ICP canceled")
+                Publisher.sendMessage('Start navigation')
+
+    def OnICP(self, navigation, tracker, m_change):
+        ref_mode_id = navigation.GetReferenceMode()
+
+        dialog = dlg.ICPCorregistrationDialog(nav_prop=(m_change, tracker, ref_mode_id))
+
+        if dialog.ShowModal() == wx.ID_OK:
+            m_icp, point_coord, transformed_points, prev_error, final_error = dialog.GetValue()
+            # TODO: checkbox in the dialog to transfer the icp points to 3D viewer
+            #create markers
+            # for i in range(len(point_coord)):
+            #     img_coord = point_coord[i][0],-point_coord[i][1],point_coord[i][2], 0, 0, 0
+            #     transf_coord = transformed_points[i][0],-transformed_points[i][1],transformed_points[i][2], 0, 0, 0
+            #     Publisher.sendMessage('Create marker', coord=img_coord, marker_id=None, colour=(1,0,0))
+            #     Publisher.sendMessage('Create marker', coord=transf_coord, marker_id=None, colour=(0,0,1))
+            if m_icp is not None:
+                dlg.ReportICPerror(prev_error, final_error)
+                use_icp = True
+            else:
+                use_icp = False
+
+            return use_icp, m_icp
+
+        else:
+            return self.use_icp, self.m_icp
+
+    def SetICP(self, navigation, use_icp):
+        self.use_icp = use_icp
+        navigation.icp_queue.put_nowait([self.use_icp, self.m_icp])
+
+    def ResetICP(self):
+        self.use_icp = False
+        self.m_icp = None
+        self.icp_fre = None
@@ -0,0 +1,341 @@
+#--------------------------------------------------------------------------
+# 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 queue
+from time import sleep
+
+import wx
+import numpy as np
+
+import invesalius.constants as const
+import invesalius.project as prj
+import invesalius.data.bases as db
+import invesalius.data.coordinates as dco
+import invesalius.data.coregistration as dcr
+import invesalius.data.serial_port_connection as spc
+import invesalius.data.slice_ as sl
+import invesalius.data.tractography as dti
+import invesalius.data.transformations as tr
+import invesalius.data.vtk_utils as vtk_utils
+from invesalius.pubsub import pub as Publisher
+
+
+class QueueCustom(queue.Queue):
+    """
+    A custom queue subclass that provides a :meth:`clear` method.
+    https://stackoverflow.com/questions/6517953/clear-all-items-from-the-queue
+    Modified to a LIFO Queue type (Last-in-first-out). Seems to make sense for the navigation
+    threads, as the last added coordinate should be the first to be processed.
+    In the first tests in a short run, seems to increase the coord queue size considerably,
+    possibly limiting the queue size is good.
+    """
+
+    def clear(self):
+        """
+        Clears all items from the queue.
+        """
+
+        with self.mutex:
+            unfinished = self.unfinished_tasks - len(self.queue)
+            if unfinished <= 0:
+                if unfinished < 0:
+                    raise ValueError('task_done() called too many times')
+                self.all_tasks_done.notify_all()
+            self.unfinished_tasks = unfinished
+            self.queue.clear()
+            self.not_full.notify_all()
+
+
+class UpdateNavigationScene(threading.Thread):
+
+    def __init__(self, vis_queues, vis_components, event, sle):
+        """Class (threading) to update the navigation scene with all graphical elements.
+
+        Sleep function in run method is used to avoid blocking GUI and more fluent, real-time navigation
+
+        :param affine_vtk: Affine matrix in vtkMatrix4x4 instance to update objects position in 3D scene
+        :type affine_vtk: vtkMatrix4x4
+        :param visualization_queue: Queue instance that manage coordinates to be visualized
+        :type visualization_queue: queue.Queue
+        :param event: Threading event to coordinate when tasks as done and allow UI release
+        :type event: threading.Event
+        :param sle: Sleep pause in seconds
+        :type sle: float
+        """
+
+        threading.Thread.__init__(self, name='UpdateScene')
+        self.serial_port_enabled, self.view_tracts, self.peel_loaded = vis_components
+        self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue = vis_queues
+        self.sle = sle
+        self.event = event
+
+    def run(self):
+        # count = 0
+        while not self.event.is_set():
+            got_coords = False
+            try:
+                coord, m_img, view_obj = self.coord_queue.get_nowait()
+                got_coords = True
+
+                # print('UpdateScene: get {}'.format(count))
+
+                # use of CallAfter is mandatory otherwise crashes the wx interface
+                if self.view_tracts:
+                    bundle, affine_vtk, coord_offset = self.tracts_queue.get_nowait()
+                    #TODO: Check if possible to combine the Remove tracts with Update tracts in a single command
+                    wx.CallAfter(Publisher.sendMessage, 'Remove tracts')
+                    wx.CallAfter(Publisher.sendMessage, 'Update tracts', root=bundle,
+                                 affine_vtk=affine_vtk, coord_offset=coord_offset)
+                    # wx.CallAfter(Publisher.sendMessage, 'Update marker offset', coord_offset=coord_offset)
+                    self.tracts_queue.task_done()
+
+                if self.serial_port_enabled:
+                    trigger_on = self.serial_port_queue.get_nowait()
+                    if trigger_on:
+                        wx.CallAfter(Publisher.sendMessage, 'Create marker')
+                    self.serial_port_queue.task_done()
+
+                #TODO: If using the view_tracts substitute the raw coord from the offset coordinate, so the user
+                # see the red cross in the position of the offset marker
+                wx.CallAfter(Publisher.sendMessage, 'Update slices position', position=coord[:3])
+                wx.CallAfter(Publisher.sendMessage, 'Set cross focal point', position=coord)
+                wx.CallAfter(Publisher.sendMessage, 'Update slice viewer')
+
+                if view_obj:
+                    wx.CallAfter(Publisher.sendMessage, 'Update object matrix', m_img=m_img, coord=coord)
+                    wx.CallAfter(Publisher.sendMessage, 'Update object arrow matrix',m_img=m_img, coord=coord, flag= self.peel_loaded)
+                self.coord_queue.task_done()
+                # print('UpdateScene: done {}'.format(count))
+                # count += 1
+
+                sleep(self.sle)
+            except queue.Empty:
+                if got_coords:
+                    self.coord_queue.task_done()
+
+
+class Navigation():
+    def __init__(self, pedal_connection):
+        self.pedal_connection = pedal_connection
+
+        self.image_fiducials = np.full([3, 3], np.nan)
+        self.correg = None
+        self.target = None
+        self.obj_reg = None
+        self.track_obj = False
+        self.m_change = None
+        self.all_fiducials = np.zeros((6, 6))
+
+        self.event = threading.Event()
+        self.coord_queue = QueueCustom(maxsize=1)
+        self.icp_queue = QueueCustom(maxsize=1)
+        # self.visualization_queue = QueueCustom(maxsize=1)
+        self.serial_port_queue = QueueCustom(maxsize=1)
+        self.coord_tracts_queue = QueueCustom(maxsize=1)
+        self.tracts_queue = QueueCustom(maxsize=1)
+
+        # Tracker parameters
+        self.ref_mode_id = const.DEFAULT_REF_MODE
+
+        # Tractography parameters
+        self.trk_inp = None
+        self.trekker = None
+        self.n_threads = None
+        self.view_tracts = False
+        self.peel_loaded = False
+        self.enable_act = False
+        self.act_data = None
+        self.n_tracts = const.N_TRACTS
+        self.seed_offset = const.SEED_OFFSET
+        self.seed_radius = const.SEED_RADIUS
+        self.sleep_nav = const.SLEEP_NAVIGATION
+
+        # Serial port
+        self.serial_port = None
+        self.serial_port_connection = None
+
+        # During navigation
+        self.coil_at_target = False
+
+        self.__bind_events()
+
+    def __bind_events(self):
+        Publisher.subscribe(self.CoilAtTarget, 'Coil at target')
+
+    def CoilAtTarget(self, state):
+        self.coil_at_target = state
+
+    def UpdateSleep(self, sleep):
+        self.sleep_nav = sleep
+        self.serial_port_connection.sleep_nav = sleep
+
+    def SerialPortEnabled(self):
+        return self.serial_port is not None
+
+    def SetReferenceMode(self, value):
+        self.ref_mode_id = value
+
+    def GetReferenceMode(self):
+        return self.ref_mode_id
+
+    def SetImageFiducial(self, fiducial_index, coord):
+        self.image_fiducials[fiducial_index, :] = coord
+
+        print("Set image fiducial {} to coordinates {}".format(fiducial_index, coord))
+
+    def AreImageFiducialsSet(self):
+        return not np.isnan(self.image_fiducials).any()
+
+    def UpdateFiducialRegistrationError(self, tracker):
+        tracker_fiducials, tracker_fiducials_raw = tracker.GetTrackerFiducials()
+
+        self.all_fiducials = np.vstack([self.image_fiducials, tracker_fiducials])
+
+        self.fre = db.calculate_fre(tracker_fiducials_raw, self.all_fiducials, self.ref_mode_id, self.m_change)
+
+    def GetFiducialRegistrationError(self, icp):
+        fre = icp.icp_fre if icp.use_icp else self.fre
+        return fre, fre <= const.FIDUCIAL_REGISTRATION_ERROR_THRESHOLD
+
+    def PedalStateChanged(self, state):
+        if state is True and self.coil_at_target and self.SerialPortEnabled():
+            self.serial_port_connection.SendPulse()
+
+    def StartNavigation(self, tracker):
+        tracker_fiducials, tracker_fiducials_raw = tracker.GetTrackerFiducials()
+
+        # initialize jobs list
+        jobs_list = []
+
+        if self.event.is_set():
+            self.event.clear()
+
+        vis_components = [self.SerialPortEnabled(), self.view_tracts, self.peel_loaded]
+        vis_queues = [self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue]
+
+        Publisher.sendMessage("Navigation status", nav_status=True, vis_status=vis_components)
+
+        self.all_fiducials = np.vstack([self.image_fiducials, tracker_fiducials])
+
+        # fiducials matrix
+        m_change = tr.affine_matrix_from_points(self.all_fiducials[3:, :].T, self.all_fiducials[:3, :].T,
+                                                shear=False, scale=False)
+        self.m_change = m_change
+
+        errors = False
+
+        if self.track_obj:
+            # if object tracking is selected
+            if self.obj_reg is None:
+                # check if object registration was performed
+                wx.MessageBox(_("Perform coil registration before navigation."), _("InVesalius 3"))
+                errors = True
+            else:
+                # if object registration was correctly performed continue with navigation
+                # 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
+
+                coreg_data = [m_change, obj_ref_mode]
+
+                if self.ref_mode_id:
+                    coord_raw, markers_flag = tracker.TrackerCoordinates.GetCoordinates()
+                else:
+                    coord_raw = np.array([None])
+
+                obj_data = db.object_registration(obj_fiducials, obj_orients, coord_raw, m_change)
+                coreg_data.extend(obj_data)
+
+                queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue]
+                jobs_list.append(dcr.CoordinateCorregistrate(self.ref_mode_id, tracker, coreg_data,
+                                                                self.view_tracts, queues,
+                                                                self.event, self.sleep_nav, tracker.tracker_id,
+                                                                self.target))
+        else:
+            coreg_data = (m_change, 0)
+            queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue]
+            jobs_list.append(dcr.CoordinateCorregistrateNoObject(self.ref_mode_id, tracker, coreg_data,
+                                                                    self.view_tracts, queues,
+                                                                    self.event, self.sleep_nav))
+
+        if not errors:
+            #TODO: Test the serial port thread
+            if self.SerialPortEnabled():
+                self.serial_port_connection = spc.SerialPortConnection(
+                    self.serial_port,
+                    self.serial_port_queue,
+                    self.event,
+                    self.sleep_nav,
+                )
+                self.serial_port_connection.Connect()
+                jobs_list.append(self.serial_port_connection)
+
+            if self.view_tracts:
+                # initialize Trekker parameters
+                slic = sl.Slice()
+                prj_data = prj.Project()
+                matrix_shape = tuple(prj_data.matrix_shape)
+                affine = slic.affine.copy()
+                affine[1, -1] -= matrix_shape[1]
+                affine_vtk = vtk_utils.numpy_to_vtkMatrix4x4(affine)
+                Publisher.sendMessage("Update marker offset state", create=True)
+                self.trk_inp = self.trekker, affine, self.seed_offset, self.n_tracts, self.seed_radius,\
+                                self.n_threads, self.act_data, affine_vtk, matrix_shape[1]
+                # print("Appending the tract computation thread!")
+                queues = [self.coord_tracts_queue, self.tracts_queue]
+                if self.enable_act:
+                    jobs_list.append(dti.ComputeTractsACTThread(self.trk_inp, queues, self.event, self.sleep_nav))
+                else:
+                    jobs_list.append(dti.ComputeTractsThread(self.trk_inp, queues, self.event, self.sleep_nav))
+
+            jobs_list.append(UpdateNavigationScene(vis_queues, vis_components,
+                                                    self.event, self.sleep_nav))
+
+            for jobs in jobs_list:
+                # jobs.daemon = True
+                jobs.start()
+                # del jobs
+
+            if self.pedal_connection is not None:
+                self.pedal_connection.add_callback('navigation', self.PedalStateChanged)
+
+    def StopNavigation(self):
+        self.event.set()
+
+        if self.pedal_connection is not None:
+            self.pedal_connection.remove_callback('navigation')
+
+        self.coord_queue.clear()
+        self.coord_queue.join()
+
+        if self.SerialPortEnabled():
+            self.serial_port_connection.join()
+
+            self.serial_port_queue.clear()
+            self.serial_port_queue.join()
+
+        if self.view_tracts:
+            self.coord_tracts_queue.clear()
+            self.coord_tracts_queue.join()
+
+            self.tracts_queue.clear()
+            self.tracts_queue.join()
+
+        vis_components = [self.SerialPortEnabled(), self.view_tracts,  self.peel_loaded]
+        Publisher.sendMessage("Navigation status", nav_status=False, vis_status=vis_components)
@@ -0,0 +1,158 @@
+#--------------------------------------------------------------------------
+# 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 numpy as np
+import threading
+
+import invesalius.constants as const
+import invesalius.data.coordinates as dco
+import invesalius.data.trackers as dt
+import invesalius.gui.dialogs as dlg
+from invesalius.pubsub import pub as Publisher
+
+
+class Tracker():
+    def __init__(self):
+        self.trk_init = None
+        self.tracker_id = const.DEFAULT_TRACKER
+
+        self.tracker_fiducials = np.full([3, 3], np.nan)
+        self.tracker_fiducials_raw = np.zeros((6, 6))
+
+        self.tracker_connected = False
+
+        self.thread_coord = None
+
+        self.event_coord = threading.Event()
+
+        self.TrackerCoordinates = dco.TrackerCoordinates()
+
+    def SetTracker(self, new_tracker):
+        if new_tracker:
+            self.DisconnectTracker()
+
+            self.trk_init = dt.TrackerConnection(new_tracker, None, 'connect')
+            if not self.trk_init[0]:
+                dlg.ShowNavigationTrackerWarning(self.tracker_id, self.trk_init[1])
+
+                self.tracker_id = 0
+                self.tracker_connected = False
+            else:
+                self.tracker_id = new_tracker
+                self.tracker_connected = True
+                self.thread_coord = dco.ReceiveCoordinates(self.trk_init, self.tracker_id, self.TrackerCoordinates,
+                                       self.event_coord)
+                self.thread_coord.start()
+
+    def DisconnectTracker(self):
+        if self.tracker_connected:
+            self.ResetTrackerFiducials()
+            Publisher.sendMessage('Update status text in GUI',
+                                    label=_("Disconnecting tracker ..."))
+            Publisher.sendMessage('Remove sensors ID')
+            Publisher.sendMessage('Remove object data')
+            self.trk_init = dt.TrackerConnection(self.tracker_id, self.trk_init[0], 'disconnect')
+            if not self.trk_init[0]:
+                self.tracker_connected = False
+                self.tracker_id = 0
+
+                if self.thread_coord:
+                    self.event_coord.set()
+                    self.thread_coord.join()
+                    self.event_coord.clear()
+
+                Publisher.sendMessage('Update status text in GUI',
+                                        label=_("Tracker disconnected"))
+                print("Tracker disconnected!")
+            else:
+                Publisher.sendMessage('Update status text in GUI',
+                                        label=_("Tracker still connected"))
+                print("Tracker still connected!")
+
+    def IsTrackerInitialized(self):
+        return self.trk_init and self.tracker_id and self.tracker_connected
+
+    def AreTrackerFiducialsSet(self):
+        return not np.isnan(self.tracker_fiducials).any()
+
+    def GetTrackerCoordinates(self, ref_mode_id, n_samples=1):
+        coord_raw_samples = {}
+        coord_samples = {}
+
+        for i in range(n_samples):
+            coord_raw, markers_flag = self.TrackerCoordinates.GetCoordinates()
+
+            if ref_mode_id == const.DYNAMIC_REF:
+                coord = dco.dynamic_reference_m(coord_raw[0, :], coord_raw[1, :])
+            else:
+                coord = coord_raw[0, :]
+                coord[2] = -coord[2]
+
+            coord_raw_samples[i] = coord_raw
+            coord_samples[i] = coord
+
+        coord_raw_avg = np.median(list(coord_raw_samples.values()), axis=0)
+        coord_avg = np.median(list(coord_samples.values()), axis=0)
+
+        return coord_avg, coord_raw_avg
+
+    def SetTrackerFiducial(self, ref_mode_id, fiducial_index):
+        coord, coord_raw = self.GetTrackerCoordinates(
+            ref_mode_id=ref_mode_id,
+            n_samples=const.CALIBRATION_TRACKER_SAMPLES,
+        )
+
+        # Update tracker fiducial with tracker coordinates
+        self.tracker_fiducials[fiducial_index, :] = coord[0:3]
+
+        assert 0 <= fiducial_index <= 2, "Fiducial index out of range (0-2): {}".format(fiducial_index)
+
+        self.tracker_fiducials_raw[2 * fiducial_index, :] = coord_raw[0, :]
+        self.tracker_fiducials_raw[2 * fiducial_index + 1, :] = coord_raw[1, :]
+
+        print("Set tracker fiducial {} to coordinates {}.".format(fiducial_index, coord[0:3]))
+
+    def ResetTrackerFiducials(self):
+        for m in range(3):
+            self.tracker_fiducials[m, :] = [np.nan, np.nan, np.nan]
+
+    def GetTrackerFiducials(self):
+        return self.tracker_fiducials, self.tracker_fiducials_raw
+
+    def GetTrackerInfo(self):
+        return self.trk_init, self.tracker_id
+
+    def UpdateUI(self, selection_ctrl, numctrls_fiducial, txtctrl_fre):
+        if self.tracker_connected:
+            selection_ctrl.SetSelection(self.tracker_id)
+        else:
+            selection_ctrl.SetSelection(0)
+
+        # Update tracker location in the UI.
+        for m in range(3):
+            coord = self.tracker_fiducials[m, :]
+            for n in range(0, 3):
+                value = 0.0 if np.isnan(coord[n]) else float(coord[n])
+                numctrls_fiducial[m][n].SetValue(value)
+
+        txtctrl_fre.SetValue('')
+        txtctrl_fre.SetBackgroundColour('WHITE')
+
+    def get_trackers(self):
+        return const.TRACKERS
@@ -22,6 +22,7 @@ from threading import Thread
  
 import mido
  
+from invesalius.pubsub import pub as Publisher
 from invesalius.utils import Singleton
  
 class PedalConnection(Thread, metaclass=Singleton):
@@ -50,6 +51,8 @@ class PedalConnection(Thread, metaclass=Singleton):
             if not self._callbacks:
                 print("Pedal pressed, no callbacks registered")
             else:
+                Publisher.sendMessage('Pedal state changed', state=True)
+
                 for callback in self._callbacks.values():
                     callback(True)
  
@@ -57,9 +60,10 @@ class PedalConnection(Thread, metaclass=Singleton):
             if not self._callbacks:
                 print("Pedal released, no callbacks registered")
             else:
+                Publisher.sendMessage('Pedal state changed', state=False)
+
                 for callback in self._callbacks.values():
                     callback(False)
-
         else:
             print("Unknown message type received from MIDI device")
  
@@ -70,6 +74,8 @@ class PedalConnection(Thread, metaclass=Singleton):
             self._midi_in._rt.ignore_types(False, False, False)
             self._midi_in.callback = self._midi_to_pedal
  
+            Publisher.sendMessage('Pedal connection', state=True)
+
             print("Connected to MIDI device")
  
     def _check_disconnected(self):
@@ -77,6 +83,8 @@ class PedalConnection(Thread, metaclass=Singleton):
             if self._active_input not in self._midi_inputs:
                 self._midi_in = None
  
+                Publisher.sendMessage('Pedal connection', state=False)
+
                 print("Disconnected from MIDI device")
  
     def _update_midi_inputs(self):
-Cython==0.29.22
-Pillow==8.2.0
+Cython==0.29.24
+Pillow==8.3.2
 Pypubsub==4.0.3
 configparser==5.0.1
 h5py==2.10.0
 imageio==2.9.0
 nibabel==3.2.1
-numpy==1.20.1
+numpy==1.21.2
 plaidml-keras==0.7.0
 psutil==5.8.0
 pyserial==3.5
-python-gdcm==3.0.8.1
-scikit-image==0.18.1
-scipy==1.6.1
-vtk==9.0.1
-wxPython==4.1.0
+python-gdcm==3.0.9.1
+scikit-image==0.18.3
+scipy==1.7.1
+vtk==9.0.3
+wxPython==4.1.1
 Theano==1.0.5
1		-invesalius/*.classpath
2		-invesalius/*.hg
3		-invesalius/*.log
4		-invesalius/*.project
5		-invesalius/*.pyc
6		-invesalius/*.swn
7		-invesalius/*.swo
8		-invesalius/*.swp
9		-invesalius/data/*.log
10		-invesalius/data/*.pyc
11		-invesalius/data/*.pyd
12		-invesalius/gui/*.log
13		-invesalius/gui/*.pyc
14		-invesalius/gui/widgets/*.log
15		-invesalius/gui/widgets/*.pyc
16		-invesalius/reader/*.log
17		-invesalius/reader/*.pyc
18		-
19		-.idea
20		-
21		-*.pyc
22		-*.swp
	1	+# Created by https://www.toptal.com/developers/gitignore/api/vim,python,intellij,visualstudiocode,direnv
	2	+# Edit at https://www.toptal.com/developers/gitignore?templates=vim,python,intellij,visualstudiocode,direnv
	3	+
	4	+### direnv ###
	5	+.direnv
	6	+.envrc
	7	+
	8	+### Intellij ###
	9	+# Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio, WebStorm and Rider
	10	+# Reference: https://intellij-support.jetbrains.com/hc/en-us/articles/206544839
	11	+.idea/
	12	+
	13	+# CMake
	14	+cmake-build-*/
	15	+
	16	+# File-based project format
	17	+*.iws
	18	+
	19	+# IntelliJ
	20	+out/
	21	+
	22	+# mpeltonen/sbt-idea plugin
	23	+.idea_modules/
	24	+
	25	+# JIRA plugin
	26	+atlassian-ide-plugin.xml
	27	+
	28	+# Crashlytics plugin (for Android Studio and IntelliJ)
	29	+com_crashlytics_export_strings.xml
	30	+crashlytics.properties
	31	+crashlytics-build.properties
	32	+fabric.properties
	33	+
	34	+### Python ###
	35	+# Byte-compiled / optimized / DLL files
	36	+__pycache__/
	37	+*.py[cod]
	38	+*$py.class
	39	+
	40	+# C extensions
23	41	*.so
24		-tags
25		-*.c
26	42
27		-.idea
28		-build
29		-*.patch
30		-*.tgz
	43	+# Distribution / packaging
	44	+.Python
	45	+build/
	46	+develop-eggs/
	47	+dist/
	48	+downloads/
	49	+eggs/
	50	+.eggs/
	51	+lib/
	52	+lib64/
	53	+parts/
	54	+sdist/
	55	+var/
	56	+wheels/
	57	+share/python-wheels/
	58	+*.egg-info/
	59	+.installed.cfg
	60	+*.egg
	61	+MANIFEST
31	62
32		-*.pyd
33		-*.cpp
34		-*.diff
	63	+# PyInstaller
	64	+# Usually these files are written by a python script from a template
	65	+# before PyInstaller builds the exe, so as to inject date/other infos into it.
	66	+*.manifest
	67	+*.spec
	68	+
	69	+# Installer logs
	70	+pip-log.txt
	71	+pip-delete-this-directory.txt
	72	+
	73	+# Unit test / coverage reports
	74	+htmlcov/
	75	+.tox/
	76	+.nox/
	77	+.coverage
	78	+.coverage.*
	79	+.cache
	80	+nosetests.xml
	81	+coverage.xml
	82	+*.cover
	83	+*.py,cover
	84	+.hypothesis/
	85	+.pytest_cache/
	86	+cover/
	87	+
	88	+# Translations
	89	+*.mo
	90	+*.pot
	91	+
	92	+# Django stuff:
	93	+*.log
	94	+local_settings.py
	95	+db.sqlite3
	96	+db.sqlite3-journal
	97	+
	98	+# Flask stuff:
	99	+instance/
	100	+.webassets-cache
	101	+
	102	+# Scrapy stuff:
	103	+.scrapy
	104	+
	105	+# Sphinx documentation
	106	+docs/_build/
	107	+
	108	+# PyBuilder
	109	+.pybuilder/
	110	+target/
	111	+
	112	+# Jupyter Notebook
	113	+.ipynb_checkpoints
	114	+
	115	+# IPython
	116	+profile_default/
	117	+ipython_config.py
	118	+
	119	+# pyenv
	120	+# For a library or package, you might want to ignore these files since the code is
	121	+# intended to run in multiple environments; otherwise, check them in:
	122	+# .python-version
	123	+
	124	+# pipenv
	125	+# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
	126	+# However, in case of collaboration, if having platform-specific dependencies or dependencies
	127	+# having no cross-platform support, pipenv may install dependencies that don't work, or not
	128	+# install all needed dependencies.
	129	+#Pipfile.lock
	130	+
	131	+# PEP 582; used by e.g. github.com/David-OConnor/pyflow
	132	+__pypackages__/
	133	+
	134	+# Celery stuff
	135	+celerybeat-schedule
	136	+celerybeat.pid
	137	+
	138	+# SageMath parsed files
	139	+*.sage.py
35	140
36		-*.directory
	141	+# Environments
	142	+.env
	143	+.venv
	144	+env/
	145	+venv/
	146	+ENV/
	147	+env.bak/
	148	+venv.bak/
	149	+myenv/
37	150
	151	+# Spyder project settings
	152	+.spyderproject
	153	+.spyproject
38	154
39		-# latex
	155	+# Rope project settings
	156	+.ropeproject
	157	+
	158	+# mkdocs documentation
	159	+/site
	160	+
	161	+# mypy
	162	+.mypy_cache/
	163	+.dmypy.json
	164	+dmypy.json
	165	+
	166	+# Pyre type checker
	167	+.pyre/
	168	+
	169	+# pytype static type analyzer
	170	+.pytype/
	171	+
	172	+# Cython debug symbols
	173	+cython_debug/
	174	+
	175	+### Vim ###
	176	+# Swap
	177	+[._]*.s[a-v][a-z]
	178	+!*.svg # comment out if you don't need vector files
	179	+[._]*.sw[a-p]
	180	+[._]s[a-rt-v][a-z]
	181	+[._]ss[a-gi-z]
	182	+[._]sw[a-p]
	183	+
	184	+# Session
	185	+Session.vim
	186	+Sessionx.vim
	187	+
	188	+# Temporary
	189	+.netrwhist
	190	+*~
	191	+# Auto-generated tag files
	192	+tags
	193	+# Persistent undo
	194	+[._]*.un~
	195	+
	196	+### VisualStudioCode ###
	197	+.vscode/
	198	+*.code-workspace
	199	+
	200	+# Local History for Visual Studio Code
	201	+.history/
	202	+
	203	+### VisualStudioCode Patch ###
	204	+# Ignore all local history of files
	205	+.history
	206	+.ionide
	207	+
	208	+### LaTeX ###
	209	+## Core latex/pdflatex auxiliary files:
40	210	*.aux
	211	+*.lof
	212	+*.log
	213	+*.lot
	214	+*.fls
	215	+*.out
41	216	*.toc
	217	+*.fmt
	218	+*.fot
	219	+*.cb
	220	+*.cb2
	221	+.*.lb
	222	+
	223	+## Intermediate documents:
	224	+*.dvi
	225	+*.xdv
	226	+-converted-to.
	227	+# these rules might exclude image files for figures etc.
	228	+# *.ps
	229	+# *.eps
	230	+# *.pdf
	231	+
	232	+## Generated if empty string is given at "Please type another file name for output:"
	233	+.pdf
	234	+
	235	+## Bibliography auxiliary files (bibtex/biblatex/biber):
42	236	*.bbl
	237	+*.bcf
43	238	*.blg
44		-*.fls
	239	+*-blx.aux
	240	+*-blx.bib
	241	+*.run.xml
	242	+
	243	+## Build tool auxiliary files:
45	244	*.fdb_latexmk
	245	+*.synctex
	246	+*.synctex(busy)
46	247	*.synctex.gz
47		-*.out
48		-*.log
	248	+*.synctex.gz(busy)
	249	+*.pdfsync
	250	+
	251	+# End of https://www.toptal.com/developers/gitignore/api/vim,python,intellij,visualstudiocode,direnv
	252	+
	253	+### Cython generated files ###
	254	+*.c
	255	+*.cpp
	256	+
	257	+### Patches and diffs ###
	258	+*.patch
	259	+*.diff
...	...
...	...	@@ -209,9 +209,9 @@ class Inv3SplashScreen(SplashScreen):
209	209
210	210	else:
211	211
212		- path = os.path.join(".","icons", icon_file)
	212	+ path = os.path.join(inv_paths.ICON_DIR, icon_file)
213	213	if not os.path.exists(path):
214		- path = os.path.join(".", "icons", "splash_en.png")
	214	+ path = os.path.join(inv_paths.ICON_DIR, "splash_en.png")
215	215
216	216	bmp = wx.Image(path).ConvertToBitmap()
217	217
...	...
...	...	@@ -3,25 +3,23 @@ channels:
3	3	- bioconda
4	4	dependencies:
5	5	- python=3.7
6		- - cython==0.29.22
7		- - pillow==8.1.1
8		- - wxpython==4.1.0
	6	+ - cython==0.29.24
	7	+ - pillow==8.3.2
9	8	- pypubsub==4.0.3
10	9	- configparser==5.0.1
11	10	- h5py==2.10.0
12	11	- imageio==2.9.0
13	12	- nibabel==3.2.1
14		- - numpy==1.20.1
15		- - pooch==1.4.0
	13	+ - numpy==1.21.2
16	14	- psutil==5.8.0
17	15	- pyserial==3.5
18		- - scikit-image==0.18.1
19		- - scipy==1.6.1
20		- - vtk==9.0.1
21		- - wxpython==4.1.0
	16	+ - scikit-image==0.18.3
	17	+ - scipy==1.7.1
	18	+ - vtk==9.0.3
	19	+ - wxpython==4.1.1
22	20	- pip
23	21	- pip:
24		- - python-gdcm==3.0.8.1
	22	+ - python-gdcm==3.0.9.1
25	23	- plaidml-keras==0.7.0
26	24	- theano==1.0.5
27	25	- pyacvd==0.2.3
...	...
...	...	@@ -659,6 +659,8 @@ BOOLEAN_XOR = 4
659	659
660	660	MARKER_COLOUR = (1.0, 1.0, 0.)
661	661	MARKER_SIZE = 2
	662	+
	663	+CALIBRATION_TRACKER_SAMPLES = 10
662	664	FIDUCIAL_REGISTRATION_ERROR_THRESHOLD = 3.0
663	665
664	666	SELECT = 0
...	...	@@ -760,21 +762,42 @@ OBJA = wx.NewId()
760	762	OBJC = wx.NewId()
761	763	OBJF = wx.NewId()
762	764
763		-BTNS_OBJ = {OBJL: {0: _('Left')},
764		- OBJR: {1: _('Right')},
765		- OBJA: {2: _('Anterior')},
766		- OBJC: {3: _('Center')},
767		- OBJF: {4: _('Fixed')}}
768		-
769		-TIPS_OBJ = [_("Select left object fiducial"),
770		- _("Select right object fiducial"),
771		- _("Select anterior object fiducial"),
772		- _("Select object center"),
773		- _("Attach sensor to object")]
	765	+OBJECT_FIDUCIALS = [
	766	+ {
	767	+ 'fiducial_index': 0,
	768	+ 'button_id': OBJL,
	769	+ 'label': _('Left'),
	770	+ 'tip': _("Select left object fiducial"),
	771	+ },
	772	+ {
	773	+ 'fiducial_index': 1,
	774	+ 'button_id': OBJR,
	775	+ 'label': _('Right'),
	776	+ 'tip': _("Select right object fiducial"),
	777	+ },
	778	+ {
	779	+ 'fiducial_index': 2,
	780	+ 'button_id': OBJA,
	781	+ 'label': _('Anterior'),
	782	+ 'tip': _("Select anterior object fiducial"),
	783	+ },
	784	+ {
	785	+ 'fiducial_index': 3,
	786	+ 'button_id': OBJC,
	787	+ 'label': _('Center'),
	788	+ 'tip': _("Select object center"),
	789	+ },
	790	+ {
	791	+ 'fiducial_index': 4,
	792	+ 'button_id': OBJF,
	793	+ 'label': _('Fixed'),
	794	+ 'tip': _("Attach sensor to object"),
	795	+ },
	796	+]
774	797
775	798	MTC_PROBE_NAME = "1Probe"
776	799	MTC_REF_NAME = "2Ref"
777		-MTC_OBJ_NAME = "3bigcoil"
	800	+MTC_OBJ_NAME = "3Coil"
778	801
779	802	# Object tracking
780	803	ARROW_SCALE = 6
...	...	@@ -805,8 +828,9 @@ TREKKER_CONFIG = {'seed_max': 1, 'step_size': 0.1, 'min_fod': 0.1, 'probe_qualit
805	828	'write_interval': 50, 'numb_threads': '', 'max_lenth': 200,
806	829	'min_lenth': 20, 'max_sampling_step': 100}
807	830
808		-WILDCARD_MARKER_FILES = _("Marker scanner coord files (.mkss)\|.mkss") + "\|" +\
809		- _("Marker files (.mks)\|.mks")
	831	+MARKER_FILE_MAGICK_STRING = "INVESALIUS3_MARKER_FILE_"
	832	+CURRENT_MARKER_FILE_VERSION = 0
	833	+WILDCARD_MARKER_FILES = _("Marker scanner coord files (.mkss)\|.mkss")
810	834
811	835	ROBOT_ElFIN_IP = ['Select robot IP:', '143.107.220.251', '169.254.153.251', '127.0.0.1']
812	836	ROBOT_ElFIN_PORT = 10003
...	...
...	...	@@ -915,6 +915,16 @@ class Controller():
915	915	matrix, matrix_filename = self.OpenOtherFiles(group)
916	916	self.CreateOtherProject(name, matrix, matrix_filename)
917	917	self.LoadProject()
	918	+ if group.affine.any():
	919	+ # TODO: replace the inverse of the affine by the actual affine in the whole code
	920	+ # remove scaling factor for non-unitary voxel dimensions
	921	+ # self.affine = image_utils.world2invspace(affine=group.affine)
	922	+ scale, shear, angs, trans, persp = tr.decompose_matrix(group.affine)
	923	+ self.affine = np.linalg.inv(tr.compose_matrix(scale=None, shear=shear,
	924	+ angles=angs, translate=trans, perspective=persp))
	925	+ # print("repos_img: {}".format(repos_img))
	926	+ self.Slice.affine = self.affine
	927	+ Publisher.sendMessage('Update affine matrix', affine=self.affine)
918	928	Publisher.sendMessage("Enable state project", state=True)
919	929	else:
920	930	dialog.ImportInvalidFiles(ftype="Others")
...	...	@@ -1043,17 +1053,6 @@ class Controller():
1043	1053	self.Slice.window_level = wl
1044	1054	self.Slice.window_width = ww
1045	1055
1046		- if group.affine.any():
1047		- # TODO: replace the inverse of the affine by the actual affine in the whole code
1048		- # remove scaling factor for non-unitary voxel dimensions
1049		- # self.affine = image_utils.world2invspace(affine=group.affine)
1050		- scale, shear, angs, trans, persp = tr.decompose_matrix(group.affine)
1051		- self.affine = np.linalg.inv(tr.compose_matrix(scale=None, shear=shear,
1052		- angles=angs, translate=trans, perspective=persp))
1053		- # print("repos_img: {}".format(repos_img))
1054		- self.Slice.affine = self.affine
1055		- Publisher.sendMessage('Update affine matrix', affine=self.affine)
1056		-
1057	1056	scalar_range = int(scalar_range[0]), int(scalar_range[1])
1058	1057	Publisher.sendMessage('Update threshold limits list',
1059	1058	threshold_range=scalar_range)
...	...
...	...	@@ -188,10 +188,18 @@ def object_registration(fiducials, orients, coord_raw, m_change):
188	188	fids_raw[ic, :] = dco.dynamic_reference_m2(coords[ic, :], coords[3, :])[:3]
189	189
190	190	# compute initial alignment of probe fixed in the object in source frame
191		- t_s0_raw = tr.translation_matrix(coords[3, :3])
192		- r_s0_raw = tr.euler_matrix(np.radians(coords[3, 3]), np.radians(coords[3, 4]),
193		- np.radians(coords[3, 5]), 'rzyx')
194		- s0_raw = tr.concatenate_matrices(t_s0_raw, r_s0_raw)
	191	+
	192	+ # XXX: Some duplicate processing is done here: the Euler angles are calculated once by
	193	+ # the lines below, and then again in dco.coordinates_to_transformation_matrix.
	194	+ #
	195	+ a, b, g = np.radians(coords[3, 3:])
	196	+ r_s0_raw = tr.euler_matrix(a, b, g, axes='rzyx')
	197	+
	198	+ s0_raw = dco.coordinates_to_transformation_matrix(
	199	+ position=coords[3, :3],
	200	+ orientation=coords[3, 3:],
	201	+ axes='rzyx',
	202	+ )
195	203
196	204	# compute change of basis for object fiducials in source frame
197	205	base_obj_raw, q_obj_raw = base_creation(fids_raw[:3, :3])
...	...	@@ -210,10 +218,11 @@ def object_registration(fiducials, orients, coord_raw, m_change):
210	218	fids_dyn[ic, 2] = -fids_dyn[ic, 2]
211	219
212	220	# compute object fiducials in vtk head frame
213		- a, b, g = np.radians(fids_dyn[ic, 3:])
214		- T_p = tr.translation_matrix(fids_dyn[ic, :3])
215		- R_p = tr.euler_matrix(a, b, g, 'rzyx')
216		- M_p = tr.concatenate_matrices(T_p, R_p)
	221	+ M_p = dco.coordinates_to_transformation_matrix(
	222	+ position=fids_dyn[ic, :3],
	223	+ orientation=fids_dyn[ic, 3:],
	224	+ axes='rzyx',
	225	+ )
217	226	M_img = m_change @ M_p
218	227
219	228	angles_img = np.degrees(np.asarray(tr.euler_from_matrix(M_img, 'rzyx')))
...	...	@@ -228,10 +237,11 @@ def object_registration(fiducials, orients, coord_raw, m_change):
228	237	r_obj_img[:3, :3] = base_obj_img[:3, :3]
229	238
230	239	# compute initial alignment of probe fixed in the object in reference (or static) frame
231		- s0_trans_dyn = tr.translation_matrix(fids_dyn[3, :3])
232		- s0_rot_dyn = tr.euler_matrix(np.radians(fids_dyn[3, 3]), np.radians(fids_dyn[3, 4]),
233		- np.radians(fids_dyn[3, 5]), 'rzyx')
234		- s0_dyn = tr.concatenate_matrices(s0_trans_dyn, s0_rot_dyn)
	240	+ s0_dyn = dco.coordinates_to_transformation_matrix(
	241	+ position=fids_dyn[3, :3],
	242	+ orientation=fids_dyn[3, 3:],
	243	+ axes='rzyx',
	244	+ )
235	245
236	246	return t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img
237	247
...	...
...	...	@@ -149,6 +149,7 @@ def np_rgba_to_vtk(n_array, spacing=(1.0, 1.0, 1.0)):
149	149	# Based on http://gdcm.sourceforge.net/html/ConvertNumpy_8py-example.html
150	150	def gdcm_to_numpy(image, apply_intercep_scale=True):
151	151	map_gdcm_np = {
	152	+ gdcm.PixelFormat.SINGLEBIT: np.uint8,
152	153	gdcm.PixelFormat.UINT8: np.uint8,
153	154	gdcm.PixelFormat.INT8: np.int8,
154	155	gdcm.PixelFormat.UINT12: np.uint16,
...	...	@@ -177,6 +178,8 @@ def gdcm_to_numpy(image, apply_intercep_scale=True):
177	178	np_array = np.frombuffer(
178	179	gdcm_array.encode("utf-8", errors="surrogateescape"), dtype=dtype
179	180	)
	181	+ if pf.GetScalarType() == gdcm.PixelFormat.SINGLEBIT:
	182	+ np_array = np.unpackbits(np_array)
180	183	np_array.shape = shape
181	184	np_array = np_array.squeeze()
182	185
...	...
...	...	@@ -39,9 +39,9 @@ class TrackerCoordinates():
39	39	def SetCoordinates(self, coord, markers_flag):
40	40	self.coord = coord
41	41	self.markers_flag = markers_flag
	42	+ wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag)
42	43
43	44	def GetCoordinates(self):
44		- wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag)
45	45	return self.coord, self.markers_flag
46	46
47	47
...	...	@@ -179,7 +179,7 @@ def PolarisCoord(trck_init, trck_id, ref_mode):
179	179
180	180	coord = np.vstack([coord1, coord2, coord3])
181	181
182		- return coord, [trck.probeID, trck.refID, trck.coilID]
	182	+ return coord, [trck.probeID, trck.refID, trck.objID]
183	183
184	184	def ElfinCoord(trck_init):
185	185	if len(trck_init) > 2:
...	...	@@ -384,11 +384,45 @@ def DebugCoordRandom(trk_init, trck_id, ref_mode):
384	384	# coord4 = np.array([uniform(1, 200), uniform(1, 200), uniform(1, 200),
385	385	# uniform(-180.0, 180.0), uniform(-180.0, 180.0), uniform(-180.0, 180.0)])
386	386
387		- #Publisher.sendMessage('Sensors ID', probe_id=int(uniform(0, 5)), ref_id=int(uniform(0, 5)), obj_id=int(uniform(0, 5)))
388		-
389	387	return np.vstack([coord1, coord2, coord3, coord4]), [int(uniform(0, 5)), int(uniform(0, 5)), int(uniform(0, 5))]
390	388
391	389
	390	+def coordinates_to_transformation_matrix(position, orientation, axes='sxyz'):
	391	+ """
	392	+ Transform vectors consisting of position and orientation (in Euler angles) in 3d-space into a 4x4
	393	+ transformation matrix that combines the rotation and translation.
	394	+ :param position: A vector of three coordinates.
	395	+ :param orientation: A vector of three Euler angles in degrees.
	396	+ :param axes: The order in which the rotations are done for the axes. See transformations.py for details. Defaults to 'sxyz'.
	397	+ :return: The transformation matrix (4x4).
	398	+ """
	399	+ a, b, g = np.radians(orientation)
	400	+
	401	+ r_ref = tr.euler_matrix(a, b, g, axes=axes)
	402	+ t_ref = tr.translation_matrix(position)
	403	+
	404	+ m_img = tr.concatenate_matrices(t_ref, r_ref)
	405	+
	406	+ return m_img
	407	+
	408	+
	409	+def transformation_matrix_to_coordinates(matrix, axes='sxyz'):
	410	+ """
	411	+ Given a matrix that combines the rotation and translation, return the position and the orientation
	412	+ determined by the matrix. The orientation is given as three Euler angles.
	413	+ The inverse of coordinates_of_transformation_matrix when the parameter 'axes' matches.
	414	+ :param matrix: A 4x4 transformation matrix.
	415	+ :param axes: The order in which the rotations are done for the axes. See transformations.py for details. Defaults to 'sxyz'.
	416	+ :return: The position (a vector of length 3) and Euler angles for the orientation in degrees (a vector of length 3).
	417	+ """
	418	+ angles = tr.euler_from_matrix(matrix, axes=axes)
	419	+ angles_as_deg = np.degrees(angles)
	420	+
	421	+ translation = tr.translation_from_matrix(matrix)
	422	+
	423	+ return translation, angles_as_deg
	424	+
	425	+
392	426	def dynamic_reference(probe, reference):
393	427	"""
394	428	Apply dynamic reference correction to probe coordinates. Uses the alpha, beta and gama
...	...	@@ -435,11 +469,11 @@ def dynamic_reference_m(probe, reference):
435	469	:param reference: sensor two defined as reference
436	470	:return: rotated and translated coordinates
437	471	"""
438		- a, b, g = np.radians(reference[3:6])
439		-
440		- trans = tr.translation_matrix(reference[:3])
441		- rot = tr.euler_matrix(a, b, g, 'rzyx')
442		- affine = tr.concatenate_matrices(trans, rot)
	472	+ affine = coordinates_to_transformation_matrix(
	473	+ position=reference[:3],
	474	+ orientation=reference[3:],
	475	+ axes='rzyx',
	476	+ )
443	477	probe_4 = np.vstack((probe[:3].reshape([3, 1]), 1.))
444	478	coord_rot = np.linalg.inv(affine) @ probe_4
445	479	# minus sign to the z coordinate
...	...	@@ -479,15 +513,16 @@ def dynamic_reference_m2(probe, reference):
479	513	:return: rotated and translated coordinates
480	514	"""
481	515
482		- a, b, g = np.radians(reference[3:6])
483		- a_p, b_p, g_p = np.radians(probe[3:6])
484		-
485		- T = tr.translation_matrix(reference[:3])
486		- T_p = tr.translation_matrix(probe[:3])
487		- R = tr.euler_matrix(a, b, g, 'rzyx')
488		- R_p = tr.euler_matrix(a_p, b_p, g_p, 'rzyx')
489		- M = tr.concatenate_matrices(T, R)
490		- M_p = tr.concatenate_matrices(T_p, R_p)
	516	+ M = coordinates_to_transformation_matrix(
	517	+ position=reference[:3],
	518	+ orientation=reference[3:],
	519	+ axes='rzyx',
	520	+ )
	521	+ M_p = coordinates_to_transformation_matrix(
	522	+ position=probe[:3],
	523	+ orientation=probe[3:],
	524	+ axes='rzyx',
	525	+ )
491	526
492	527	M_dyn = np.linalg.inv(M) @ M_p
493	528
...	...
...	...	@@ -69,12 +69,11 @@ def object_to_reference(coord_raw, m_probe):
69	69	:return: 4 x 4 numpy double array
70	70	:rtype: numpy.ndarray
71	71	"""
72		-
73		- a, b, g = np.radians(coord_raw[1, 3:])
74		- r_ref = tr.euler_matrix(a, b, g, 'rzyx')
75		- t_ref = tr.translation_matrix(coord_raw[1, :3])
76		- m_ref = tr.concatenate_matrices(t_ref, r_ref)
77		-
	72	+ m_ref = dco.coordinates_to_transformation_matrix(
	73	+ position=coord_raw[1, :3],
	74	+ orientation=coord_raw[1, 3:],
	75	+ axes='rzyx',
	76	+ )
78	77	m_dyn = np.linalg.inv(m_ref) @ m_probe
79	78	return m_dyn
80	79
...	...	@@ -108,20 +107,25 @@ def corregistrate_object_dynamic(inp, coord_raw, ref_mode_id, icp):
108	107
109	108	# transform raw marker coordinate to object center
110	109	m_probe = object_marker_to_center(coord_raw, obj_ref_mode, t_obj_raw, s0_raw, r_s0_raw)
	110	+
111	111	# transform object center to reference marker if specified as dynamic reference
112	112	if ref_mode_id:
113	113	m_probe_ref = object_to_reference(coord_raw, m_probe)
114	114	else:
115	115	m_probe_ref = m_probe
	116	+
116	117	# invert y coordinate
117	118	m_probe_ref[2, -1] = -m_probe_ref[2, -1]
	119	+
118	120	# corregistrate from tracker to image space
119	121	m_img = tracker_to_image(m_change, m_probe_ref, r_obj_img, m_obj_raw, s0_dyn)
120	122	if icp[0]:
121	123	m_img = bases.transform_icp(m_img, icp[1])
	124	+
122	125	# compute rotation angles
123		- _, _, angles, _, _ = tr.decompose_matrix(m_img)
124		- # create output coordiante list
	126	+ angles = tr.euler_from_matrix(m_img, axes='sxyz')
	127	+
	128	+ # create output coordinate list
125	129	coord = m_img[0, -1], m_img[1, -1], m_img[2, -1], \
126	130	np.degrees(angles[0]), np.degrees(angles[1]), np.degrees(angles[2])
127	131
...	...	@@ -132,10 +136,11 @@ def UpdateICP(self, m_icp, flag):
132	136	self.icp = flag
133	137
134	138	def compute_marker_transformation(coord_raw, obj_ref_mode):
135		- psi, theta, phi = np.radians(coord_raw[obj_ref_mode, 3:])
136		- r_probe = tr.euler_matrix(psi, theta, phi, 'rzyx')
137		- t_probe = tr.translation_matrix(coord_raw[obj_ref_mode, :3])
138		- m_probe = tr.concatenate_matrices(t_probe, r_probe)
	139	+ m_probe = dco.coordinates_to_transformation_matrix(
	140	+ position=coord_raw[obj_ref_mode, :3],
	141	+ orientation=coord_raw[obj_ref_mode, 3:],
	142	+ axes='rzyx',
	143	+ )
139	144	return m_probe
140	145
141	146
...	...	@@ -145,6 +150,7 @@ def corregistrate_dynamic(inp, coord_raw, ref_mode_id, icp):
145	150
146	151	# transform raw marker coordinate to object center
147	152	m_probe = compute_marker_transformation(coord_raw, obj_ref_mode)
	153	+
148	154	# transform object center to reference marker if specified as dynamic reference
149	155	if ref_mode_id:
150	156	m_ref = compute_marker_transformation(coord_raw, 1)
...	...	@@ -154,6 +160,7 @@ def corregistrate_dynamic(inp, coord_raw, ref_mode_id, icp):
154	160
155	161	# invert y coordinate
156	162	m_probe_ref[2, -1] = -m_probe_ref[2, -1]
	163	+
157	164	# corregistrate from tracker to image space
158	165	m_img = m_change @ m_probe_ref
159	166
...	...	@@ -161,8 +168,9 @@ def corregistrate_dynamic(inp, coord_raw, ref_mode_id, icp):
161	168	m_img = bases.transform_icp(m_img, icp[1])
162	169
163	170	# compute rotation angles
164		- _, _, angles, _, _ = tr.decompose_matrix(m_img)
165		- # create output coordiante list
	171	+ angles = tr.euler_from_matrix(m_img, axes='sxyz')
	172	+
	173	+ # create output coordinate list
166	174	coord = m_img[0, -1], m_img[1, -1], m_img[2, -1],\
167	175	np.degrees(angles[0]), np.degrees(angles[1]), np.degrees(angles[2])
168	176
...	...	@@ -204,7 +212,7 @@ class CoordinateCorregistrate(threading.Thread):
204	212	coreg_data = self.coreg_data
205	213	view_obj = 1
206	214
207		- trck_init, trck_id, trck_mode = self.tracker.GetTrackerInfo()
	215	+ trck_init, trck_id = self.tracker.GetTrackerInfo()
208	216
209	217	# print('CoordCoreg: event {}'.format(self.event.is_set()))
210	218	while not self.event.is_set():
...	...	@@ -284,7 +292,7 @@ class CoordinateCorregistrateNoObject(threading.Thread):
284	292	coreg_data = self.coreg_data
285	293	view_obj = 0
286	294
287		- trck_init, trck_id, trck_mode = self.tracker.GetTrackerInfo()
	295	+ trck_init, trck_id = self.tracker.GetTrackerInfo()
288	296	# print('CoordCoreg: event {}'.format(self.event.is_set()))
289	297	while not self.event.is_set():
290	298	try:
...	...
...	...	@@ -34,6 +34,7 @@ from vtk.util import numpy_support
34	34
35	35	import invesalius.constants as const
36	36	import invesalius.data.converters as converters
	37	+import invesalius.data.coordinates as dco
37	38	import invesalius.data.slice_ as sl
38	39	import invesalius.data.transformations as tr
39	40	import invesalius.reader.bitmap_reader as bitmap_reader
...	...	@@ -555,18 +556,23 @@ def image_normalize(image, min_=0.0, max_=1.0, output_dtype=np.int16):
555	556	return output
556	557
557	558
	559	+# TODO: Add a description of different coordinate systems, namely:
	560	+# - the world coordinate system,
	561	+# - the voxel coordinate system.
	562	+# - InVesalius's internal coordinate system,
	563	+#
558	564	def convert_world_to_voxel(xyz, affine):
559	565	"""
560	566	Convert a coordinate from the world space ((x, y, z); scanner space; millimeters) to the
561	567	voxel space ((i, j, k)). This is achieved by multiplying a coordinate by the inverse
562	568	of the affine transformation.
	569	+
563	570	More information: https://nipy.org/nibabel/coordinate_systems.html
	571	+
564	572	:param xyz: a list or array of 3 coordinates (x, y, z) in the world coordinates
565	573	:param affine: a 4x4 array containing the image affine transformation in homogeneous coordinates
566	574	:return: a 1x3 array with the point coordinates in image space (i, j, k)
567	575	"""
568		-
569		- # print("xyz: ", xyz, "\naffine", affine)
570	576	# convert xyz coordinate to 1x4 homogeneous coordinates array
571	577	xyz_homo = np.hstack((xyz, 1.0)).reshape([4, 1])
572	578	ijk_homo = np.linalg.inv(affine) @ xyz_homo
...	...	@@ -575,6 +581,65 @@ def convert_world_to_voxel(xyz, affine):
575	581	return ijk
576	582
577	583
	584	+def convert_invesalius_to_voxel(position):
	585	+ """
	586	+ Convert position from InVesalius space to the voxel space.
	587	+
	588	+ The two spaces are otherwise identical, but InVesalius space has a reverted y-axis
	589	+ (increasing y-coordinate moves posterior in InVesalius space, but anterior in the voxel space).
	590	+
	591	+ For instance, if the size of the voxel image is 256 x 256 x 160, the y-coordinate 0 in
	592	+ InVesalius space corresponds to the y-coordinate 255 in the voxel space.
	593	+
	594	+ :param position: a vector of 3 coordinates (x, y, z) in InVesalius space.
	595	+ :return: a vector of 3 coordinates in the voxel space
	596	+ """
	597	+ slice = sl.Slice()
	598	+ return np.array((position[0], slice.matrix.shape[1] - position[1] - 1, position[2]))
	599	+
	600	+
	601	+def convert_invesalius_to_world(position, orientation):
	602	+ """
	603	+ Convert position and orientation from InVesalius space to the world space.
	604	+
	605	+ The axis definition for the Euler angles returned is 'sxyz', see transformations.py for more
	606	+ information.
	607	+
	608	+ Uses 'affine' matrix defined in the project created or opened by the user. If it is
	609	+ undefined, return Nones as the coordinates for both position and orientation.
	610	+
	611	+ More information: https://nipy.org/nibabel/coordinate_systems.html
	612	+
	613	+ :param position: a vector of 3 coordinates in InVesalius space.
	614	+ :param orientation: a vector of 3 Euler angles in InVesalius space.
	615	+ :return: a pair consisting of 3 coordinates and 3 Euler angles in the world space, or Nones if
	616	+ 'affine' matrix is not defined in the project.
	617	+ """
	618	+ slice = sl.Slice()
	619	+
	620	+ if slice.affine is None:
	621	+ position_world = (None, None, None)
	622	+ orientation_world = (None, None, None)
	623	+
	624	+ return position_world, orientation_world
	625	+
	626	+ position_voxel = convert_invesalius_to_voxel(position)
	627	+
	628	+ M_invesalius = dco.coordinates_to_transformation_matrix(
	629	+ position=position_voxel,
	630	+ orientation=orientation,
	631	+ axes='sxyz',
	632	+ )
	633	+ M_world = np.linalg.inv(slice.affine) @ M_invesalius
	634	+
	635	+ position_world, orientation_world = dco.transformation_matrix_to_coordinates(
	636	+ M_world,
	637	+ axes='sxyz',
	638	+ )
	639	+
	640	+ return position_world, orientation_world
	641	+
	642	+
578	643	def create_grid(xy_range, z_range, z_offset, spacing):
579	644	x = np.arange(xy_range[0], xy_range[1] + 1, spacing)
580	645	y = np.arange(xy_range[0], xy_range[1] + 1, spacing)
...	...