.dockerignore

+.git*
+.python-version
+Dockerfile

Dockerfile

+FROM ubuntu:16.04
+
+RUN apt-get update
+RUN apt-get install -y \
+    cython \
+    locales \
+    python-concurrent.futures \
+    python-gdcm \
+    python-matplotlib \
+    python-nibabel \
+    python-numpy \
+    python-pil \
+    python-psutil \
+    python-scipy \
+    python-serial \
+    python-skimage \
+    python-vtk6 \
+    python-vtkgdcm \
+    python-wxgtk3.0
+
+RUN locale-gen en_US.UTF-8
+ENV LANG en_US.UTF-8
+ENV LANGUAGE en_US:en
+ENV LC_ALL en_US.UTF-8
+
+WORKDIR /usr/local/app
+
+COPY . .
+
+RUN python setup.py build_ext --inplace

app.py

@@ -18,31 +18,42 @@
 #    detalhes.
 #-------------------------------------------------------------------------
 
+from __future__ import print_function
 
 import multiprocessing
 import optparse as op
 import os
 import sys
 import shutil
+import traceback
+
+import re
 
 if sys.platform == 'win32':
-    import _winreg
-else:
-    if sys.platform != 'darwin':
-        import wxversion
-        #wxversion.ensureMinimal('2.8-unicode', optionsRequired=True)
-        #wxversion.select('2.8-unicode', optionsRequired=True)
-        wxversion.ensureMinimal('3.0')
+    try:
+        import winreg
+    except ImportError:
+        import _winreg as winreg
+#  else:
+    #  if sys.platform != 'darwin':
+        #  import wxversion
+        #  #wxversion.ensureMinimal('2.8-unicode', optionsRequired=True)
+        #  #wxversion.select('2.8-unicode', optionsRequired=True)
+        #  #  wxversion.ensureMinimal('4.0')
         
 import wx
+try:
+    from wx.adv import SplashScreen
+except ImportError:
+    from wx import SplashScreen
 #from wx.lib.pubsub import setupv1 #new wx
-from wx.lib.pubsub import setuparg1# as psv1
+#  from wx.lib.pubsub import setuparg1# as psv1
 #from wx.lib.pubsub import Publisher 
 #import wx.lib.pubsub as ps
 from wx.lib.pubsub import pub as Publisher
 
 #import wx.lib.agw.advancedsplash as agw
-#if sys.platform == 'linux2':
+#if sys.platform.startswith('linux'):
 #    _SplashScreen = agw.AdvancedSplash
 #else:
 #    if sys.platform != 'darwin':
@@ -61,9 +72,9 @@ if sys.platform == 'win32':
     try:
         USER_DIR = expand_user()
     except:
-        USER_DIR = os.path.expanduser('~').decode(FS_ENCODE)
+        USER_DIR = utils.decode(os.path.expanduser('~'), FS_ENCODE)
 else:
-    USER_DIR = os.path.expanduser('~').decode(FS_ENCODE)
+    USER_DIR = utils.decode(os.path.expanduser('~'),FS_ENCODE)
 
 USER_INV_DIR = os.path.join(USER_DIR, u'.invesalius')
 USER_PRESET_DIR = os.path.join(USER_INV_DIR, u'presets')
@@ -72,6 +83,15 @@ USER_LOG_DIR = os.path.join(USER_INV_DIR, u'logs')
 
 # ------------------------------------------------------------------
 
+if sys.platform in ('linux2', 'linux', 'win32'):
+    try:
+        tmp_var = wx.GetXDisplay
+    except AttributeError:
+        # A workaround to make InVesalius run with wxPython4 from Ubuntu 18.04
+        wx.GetXDisplay = lambda: None
+    else:
+        del tmp_var
+
 
 class InVesalius(wx.App):
     """
@@ -86,7 +106,7 @@ class InVesalius(wx.App):
         freeze_support()
 
         self.SetAppName("InVesalius 3")
-        self.splash = SplashScreen()
+        self.splash = Inv3SplashScreen()
         self.splash.Show()
         wx.CallLater(1000,self.Startup2)
 
@@ -97,7 +117,7 @@ class InVesalius(wx.App):
         Open drag & drop files under darwin
         """
         path = os.path.abspath(filename)
-        Publisher.sendMessage('Open project', path)
+        Publisher.sendMessage('Open project', filepath=path)
 
     def Startup2(self):
         self.control = self.splash.control
@@ -108,7 +128,7 @@ class InVesalius(wx.App):
 
 # ------------------------------------------------------------------
 
-class SplashScreen(wx.SplashScreen):
+class Inv3SplashScreen(SplashScreen):
     """
     Splash screen to be shown in InVesalius initialization.
     """
@@ -175,7 +195,10 @@ class SplashScreen(wx.SplashScreen):
         if lang:
             #  print "LANG", lang, _, wx.Locale(), wx.GetLocale()
             import locale
+            try:
                 locale.setlocale(locale.LC_ALL, '')
+            except locale.Error:
+                pass
             # For pt_BR, splash_pt.png should be used
             if (lang.startswith('pt')):
                 icon_file = "splash_pt.png"
@@ -196,8 +219,12 @@ class SplashScreen(wx.SplashScreen):
 				
             bmp = wx.Image(path).ConvertToBitmap()
 
+            try:
+                style = wx.adv.SPLASH_TIMEOUT | wx.adv.SPLASH_CENTRE_ON_SCREEN
+            except AttributeError:
                 style = wx.SPLASH_TIMEOUT | wx.SPLASH_CENTRE_ON_SCREEN
-            wx.SplashScreen.__init__(self,
+
+            SplashScreen.__init__(self,
                                   bitmap=bmp,
                                   splashStyle=style,
                                   milliseconds=1500,
@@ -218,7 +245,8 @@ class SplashScreen(wx.SplashScreen):
         self.control = Controller(self.main)
         
         self.fc = wx.FutureCall(1, self.ShowMain)
-        wx.FutureCall(1, parse_comand_line)
+        options, args = parse_comand_line()
+        wx.FutureCall(1, use_cmd_optargs, options, args)
 
         # Check for updates
         from threading import Thread
@@ -244,6 +272,24 @@ class SplashScreen(wx.SplashScreen):
         if self.fc.IsRunning():
             self.Raise()
 
+
+def non_gui_startup(options, args):
+    lang = 'en'
+    _ = i18n.InstallLanguage(lang)
+
+    from invesalius.control import Controller
+    from invesalius.project import Project
+
+    session = ses.Session()
+    if not session.ReadSession():
+        session.CreateItens()
+        session.SetLanguage(lang)
+        session.WriteSessionFile()
+
+    control = Controller(None)
+
+    use_cmd_optargs(options, args)
+
 # ------------------------------------------------------------------
 
 
@@ -262,49 +308,168 @@ def parse_comand_line():
                       action="store_true",
                       dest="debug")
 
+    parser.add_option('--no-gui',
+                      action='store_true',
+                      dest='no_gui')
+
     # -i or --import: import DICOM directory
     # chooses largest series
     parser.add_option("-i", "--import",
                       action="store",
                       dest="dicom_dir")
+
+    parser.add_option("--import-all",
+                      action="store")
+
+    parser.add_option("-s", "--save",
+                      help="Save the project after an import.")
+
+    parser.add_option("-t", "--threshold",
+                      help="Define the threshold for the export (e.g. 100-780).")
+
+    parser.add_option("-e", "--export",
+                      help="Export to STL.")
+
+    parser.add_option("-a", "--export-to-all",
+                      help="Export to STL for all mask presets.")
+
     options, args = parser.parse_args()
+    return options, args
+
 
+def use_cmd_optargs(options, args):
     # If debug argument...
     if options.debug:
         Publisher.subscribe(print_events, Publisher.ALL_TOPICS)
+        session = ses.Session()
         session.debug = 1
 
     # If import DICOM argument...
     if options.dicom_dir:
         import_dir = options.dicom_dir
-        Publisher.sendMessage('Import directory', import_dir)
+        Publisher.sendMessage('Import directory', directory=import_dir, use_gui=not options.no_gui)
+
+        if options.save:
+            Publisher.sendMessage('Save project', filepath=os.path.abspath(options.save))
+            exit(0)
+
+        check_for_export(options)
+
+        return True
+    elif options.import_all:
+        import invesalius.reader.dicom_reader as dcm
+        for patient in dcm.GetDicomGroups(options.import_all):
+            for group in patient.GetGroups():
+                Publisher.sendMessage('Import group',
+                                      group=group,
+                                      use_gui=not options.no_gui)
+                check_for_export(options, suffix=group.title, remove_surfaces=False)
+                Publisher.sendMessage('Remove masks', mask_indexes=(0,))
         return True
 
     # Check if there is a file path somewhere in what the user wrote
     # In case there is, try opening as it was a inv3
     else:
-        i = len(args)
-        while i:
-            i -= 1
-            file = args[i].decode(sys.stdin.encoding)
+        for arg in reversed(args):
+
+            file = utils.decode(arg, FS_ENCODE)
             if os.path.isfile(file):
                 path = os.path.abspath(file)
-                Publisher.sendMessage('Open project', path)
-                i = 0
+                Publisher.sendMessage('Open project', filepath=path)
+                check_for_export(options)
                 return True
+
+            file = utils.decode(arg, sys.stdin.encoding)
+            if os.path.isfile(file):
+                path = os.path.abspath(file)
+                Publisher.sendMessage('Open project', filepath=path)
+                check_for_export(options)
+                return True
+
     return False
 
 
-def print_events(data):
+def sanitize(text):
+    text = str(text).strip().replace(' ', '_')
+    return re.sub(r'(?u)[^-\w.]', '', text)
+
+
+def check_for_export(options, suffix='', remove_surfaces=False):
+    suffix = sanitize(suffix)
+
+    if options.export:
+        if not options.threshold:
+            print("Need option --threshold when using --export.")
+            exit(1)
+        threshold_range = tuple([int(n) for n in options.threshold.split(',')])
+
+        if suffix:
+            if options.export.endswith('.stl'):
+                path_ = '{}-{}.stl'.format(options.export[:-4], suffix)
+            else:
+                path_ = '{}-{}.stl'.format(options.export, suffix)
+        else:
+            path_ = options.export
+
+        export(path_, threshold_range, remove_surface=remove_surfaces)
+    elif options.export_to_all:
+        # noinspection PyBroadException
+        try:
+            from invesalius.project import Project
+
+            for threshold_name, threshold_range in Project().presets.thresh_ct.iteritems():
+                if isinstance(threshold_range[0], int):
+                    path_ = u'{}-{}-{}.stl'.format(options.export_to_all, suffix, threshold_name)
+                    export(path_, threshold_range, remove_surface=True)
+        except:
+            traceback.print_exc()
+        finally:
+            exit(0)
+
+
+def export(path_, threshold_range, remove_surface=False):
+    import invesalius.constants as const
+
+    Publisher.sendMessage('Set threshold values',
+                          threshold_range=threshold_range)
+
+    surface_options = {
+        'method': {
+            'algorithm': 'Default',
+            'options': {},
+        }, 'options': {
+            'index': 0,
+            'name': '',
+            'quality': _('Optimal *'),
+            'fill': False,
+            'keep_largest': False,
+            'overwrite': False,
+        }
+    }
+    Publisher.sendMessage('Create surface from index',
+                          surface_parameters=surface_options)
+    Publisher.sendMessage('Export surface to file',
+                          filename=path_, filetype=const.FILETYPE_STL)
+    if remove_surface:
+        Publisher.sendMessage('Remove surfaces',
+                              surface_indexes=(0,))
+
+
+def print_events(topic=Publisher.AUTO_TOPIC, **msg_data):
     """
     Print pubsub messages
     """
-    utils.debug(data.topic)
+    utils.debug("%s\n\tParameters: %s" % (topic, msg_data))
 
 def main():
     """
     Initialize InVesalius GUI
     """
+    options, args = parse_comand_line()
+
+    if options.no_gui:
+        non_gui_startup(options, args)
+    else:
         application = InVesalius(0)
         application.MainLoop()
 
@@ -316,10 +481,10 @@ if __name__ == '__main__':
     if hasattr(sys,"frozen") and sys.platform.startswith('win'):
 
         #Click in the .inv3 file support
-        root = _winreg.HKEY_CLASSES_ROOT
+        root = winreg.HKEY_CLASSES_ROOT
         key = "InVesalius 3.1\InstallationDir"
-        hKey = _winreg.OpenKey (root, key, 0, _winreg.KEY_READ)
-        value, type_ = _winreg.QueryValueEx (hKey, "")
+        hKey = winreg.OpenKey (root, key, 0, winreg.KEY_READ)
+        value, type_ = winreg.QueryValueEx (hKey, "")
         path = os.path.join(value,'dist')
 
         os.chdir(path)

debian/changelog

+invesalius (3.1.99991-1) unstable; urgency=medium
+
+  * New upstream version
+    Closes: #901562
+
+ -- Thiago Franco de Moraes <tfmoraes@cti.gov.br>  Fri, 17 Aug 2018 14:07:48 +0200
+
 invesalius (3.1.1-3) unstable; urgency=medium
 
   * Further changes concerning #779655

debian/control

@@ -11,8 +11,8 @@ Build-Depends: debhelper (>= 10),
                python-numpy
 Build-Depends-Indep: python
 Standards-Version: 4.1.0
-Vcs-Browser: https://anonscm.debian.org/cgit/debian-med/invesalius.git
-Vcs-Git: https://anonscm.debian.org/git/debian-med/invesalius.git
+Vcs-Browser: https://salsa.debian.org/med-team/invesalius
+Vcs-Git: https://salsa.debian.org/med-team/invesalius.git
 Homepage: http://www.cti.gov.br/invesalius/
 X-Python-Version: 2.7
 
@@ -23,7 +23,7 @@ Depends: ${python:Depends},
          python-numpy,
          python-scipy,
          python-skimage,
-         python-wxgtk3.0,
+         python-wxgtk4.0,
          python-pil,
          python-gdcm,
          python-vtkgdcm,

debian/patches/10_change_casmoothing_import.patch

-Author: Thiago Franco de Moraes
-Date: Mon, 10 Dec 2012 14:22:46 -0200
-Description: Upstream has ca_smoothing integrated and Debian have it separated
-in another package (python-casmoothing), so the import must be changed.
---- a/invesalius/data/surface.py
-+++ b/invesalius/data/surface.py
-@@ -37,7 +37,7 @@ import surface_process
- import utils as utl
- import vtk_utils as vu
- try:
--    import data.ca_smoothing as ca_smoothing
-+    import ca_smoothing
- except:
-     pass
- 

debian/patches/10_import_cython_modules.patch

@@ -52,7 +52,7 @@ Description: Import cython modules
 --- a/setup.py
 +++ b/setup.py
 @@ -11,26 +11,26 @@
- if sys.platform == 'linux2':
+ if sys.platform.startswith('linux'):
      setup(
          cmdclass = {'build_ext': build_ext},
 -        ext_modules = cythonize([ Extension("invesalius.data.mips", ["invesalius/data/mips.pyx"],

debian/patches/10_import_mips.patch

-Author: Thiago Franco de Moraes
-Date: Mon, 28 Apr 2014 11:08:46 -0200
-Description: Upstream has the mips module inside the invesalius source code and
-Debian has it separated inside /usr/lib/invesaliu so it was necessary to change
-the import
-diff --git a/invesalius/data/slice_.py b/invesalius/data/slice_.py
-index 4816b96..8ecc872 100644
---- a/invesalius/data/slice_.py
-+++ b/invesalius/data/slice_.py
-@@ -33,7 +33,7 @@ import utils
- 
- from mask import Mask
- from project import Project
--from data import mips
-+import mips
- 
- OTHER=0
- PLIST=1

debian/patches/20_fix_division_by_zero.patch

-Description: Avoid division by zero in progress display 
-Author: Gert Wollny <gewo@debian.org>
-Debian-Bug: https://bugs.debian.org/779655
-
---- a/invesalius/data/vtk_utils.py
-+++ b/invesalius/data/vtk_utils.py
-@@ -49,7 +49,10 @@
- 
-     # when the pipeline is larger than 1, we have to consider this object
-     # percentage
--    ratio = (100.0 / number_of_filters)
-+    if number_of_filters > 0: 
-+        ratio = (100.0 / number_of_filters)
-+    else:
-+        ratio = 100.0
- 
-     def UpdateProgress(obj, label=""):
-         """

debian/patches/20_fix_one_slice_import.patch

-Description: Fix import of dicom serie with one unique slice.
-Author: Thiago Franco de Moraes <tfmoraes@cti.gov.br>
-Debian-Bug: https://bugs.debian.org/779655
-
---- a/invesalius/control.py
-+++ b/invesalius/control.py
-@@ -408,6 +408,10 @@ class Controller():
- 
-     def OnLoadImportPanel(self, evt):
-         patient_series = evt.data
-+        if self.progress_dialog:
-+            self.progress_dialog.Close()
-+            self.progress_dialog = None
-+
-         ok = self.LoadImportPanel(patient_series)
-         if ok:
-             Publisher.sendMessage('Show import panel')
---- a/invesalius/data/imagedata_utils.py
-+++ b/invesalius/data/imagedata_utils.py
-@@ -523,7 +523,8 @@ def dcm2memmap(files, slice_size, orientation, resolution_percentage):
-     returns it and its related filename.
-     """
-     message = _("Generating multiplanar visualization...")
--    update_progress= vtk_utils.ShowProgress(len(files) - 1, dialog_type = "ProgressDialog")
-+    if len(files) > 1:
-+        update_progress= vtk_utils.ShowProgress(len(files) - 1, dialog_type = "ProgressDialog")
- 
-     temp_file = tempfile.mktemp()
- 
-@@ -584,7 +585,8 @@ def dcm2memmap(files, slice_size, orientation, resolution_percentage):
-         else:
-             array.shape = matrix.shape[1], matrix.shape[2]
-             matrix[n] = array
--        update_progress(cont,message)
-+        if len(files) > 1:
-+            update_progress(cont,message)
-         cont += 1
- 
-     matrix.flush()
---- a/invesalius/data/slice_.py
-+++ b/invesalius/data/slice_.py
-@@ -633,7 +633,7 @@ class Slice(object):
-                 if np.any(self.q_orientation[1::]):
-                     transforms.apply_view_matrix_transform(self.matrix, self.spacing, M, slice_number, orientation, self.interp_method, self.matrix.min(), tmp_array)
-                 if self._type_projection == const.PROJECTION_NORMAL:
--                    n_image = tmp_array.squeeze()
-+                    n_image = tmp_array[0]
-                 else:
-                     if inverted:
-                         tmp_array = tmp_array[::-1]
-@@ -681,7 +681,7 @@ class Slice(object):
-                     transforms.apply_view_matrix_transform(self.matrix, self.spacing, M, slice_number, orientation, self.interp_method, self.matrix.min(), tmp_array)
- 
-                 if self._type_projection == const.PROJECTION_NORMAL:
--                    n_image = tmp_array.squeeze()
-+                    n_image = tmp_array[:, 0, :]
-                 else:
-                     #if slice_number == 0:
-                         #slice_number = 1
-@@ -731,7 +731,7 @@ class Slice(object):
-                     transforms.apply_view_matrix_transform(self.matrix, self.spacing, M, slice_number, orientation, self.interp_method, self.matrix.min(), tmp_array)
- 
-                 if self._type_projection == const.PROJECTION_NORMAL:
--                    n_image = tmp_array.squeeze()
-+                    n_image = tmp_array[:, :, 0]
-                 else:
-                     if inverted:
-                         tmp_array = tmp_array[:, :, ::-1]
---- a/invesalius/gui/import_panel.py
-+++ b/invesalius/gui/import_panel.py
-@@ -186,11 +186,8 @@ class InnerPanel(wx.Panel):
-                 slice_amont = group.nslices
- 
-         nslices_result = slice_amont / (interval + 1)
--        if (nslices_result > 1):
--            Publisher.sendMessage('Open DICOM group', (group, interval, 
--                                    [self.first_image_selection, self.last_image_selection]))
--        else:
--            dlg.MissingFilesForReconstruction()
-+        Publisher.sendMessage('Open DICOM group', (group, interval, 
-+                                [self.first_image_selection, self.last_image_selection]))
- 
- class TextPanel(wx.Panel):
-     def __init__(self, parent):

debian/patches/series

 10_sample_path.patch
 10_import_cython_modules.patch
-20_fix_one_slice_import.patch

debian/rules

@@ -40,6 +40,7 @@ override_dh_clean:
 create-launcher:
 	echo '#!/bin/sh' > invesalius3
 	echo 'export PYTHONPATH=$$PYTHONPATH:"/usr/lib/invesalius"' >> invesalius3
+	echo 'export PYTHONPATH=$$PYTHONPATH:"/usr/lib/python2.7/dist-packages/wxPython-4.0.1-py2.7-linux-amd64.egg"' >> invesalius3
 	echo 'export INVESALIUS_LIBRARY_PATH="/usr/share/invesalius/"' >> invesalius3
 	echo 'cd $$INVESALIUS_LIBRARY_PATH' >> invesalius3
 	echo 'python app.py $$@' >> invesalius3

docs/devel/example_singleton_pubsub.py

@@ -52,7 +52,7 @@ p3 = Person("Andre ")
 people = [p1, p2, p3]
 
 print "Everyone eats 2 pieces:"
-for i in xrange(2):
+for i in range(2):
     for person in people:
         person.EatPieceOfPizza()
     

invesalius/constants.py

@@ -23,9 +23,13 @@ import sys
 import wx
 import itertools
 
+from invesalius import utils
+
 #from invesalius.project import Project
 INVESALIUS_VERSION = "3.1.1"
 
+INVESALIUS_ACTUAL_FORMAT_VERSION = 1.1
+
 #---------------
 
 # Measurements
@@ -56,6 +60,7 @@ STEREO_ANAGLYPH = _("Anaglyph")
 TEXT_SIZE_SMALL = 11
 TEXT_SIZE = 12
 TEXT_SIZE_LARGE = 16
+TEXT_SIZE_EXTRA_LARGE = 20
 TEXT_COLOUR = (1,1,1)
 
 (X,Y) = (0.03, 0.97)
@@ -329,15 +334,15 @@ if sys.platform == 'win32':
     try:
         USER_DIR = expand_user()
     except:
-        USER_DIR = os.path.expanduser('~').decode(FS_ENCODE)
+        USER_DIR = utils.decode(os.path.expanduser('~'), FS_ENCODE)
 else:
-    USER_DIR = os.path.expanduser('~').decode(FS_ENCODE)
+    USER_DIR = utils.decode(os.path.expanduser('~'), FS_ENCODE)
 
 USER_INV_DIR = os.path.join(USER_DIR, u'.invesalius')
 USER_PRESET_DIR = os.path.join(USER_INV_DIR, u'presets')
 USER_LOG_DIR = os.path.join(USER_INV_DIR, u'logs')
 
-FILE_PATH = os.path.split(__file__)[0].decode(FS_ENCODE)
+FILE_PATH = utils.decode(os.path.split(__file__)[0], FS_ENCODE)
 
 if hasattr(sys,"frozen") and (sys.frozen == "windows_exe"\
                             or sys.frozen == "console_exe"):
@@ -541,6 +546,8 @@ ID_WATERSHED_SEGMENTATION = wx.NewId()
 ID_THRESHOLD_SEGMENTATION = wx.NewId()
 ID_FLOODFILL_SEGMENTATION = wx.NewId()
 ID_CROP_MASK = wx.NewId()
+ID_CREATE_SURFACE = wx.NewId()
+ID_CREATE_MASK = wx.NewId()
 
 #---------------------------------------------------------
 STATE_DEFAULT = 1000
@@ -678,6 +685,10 @@ DYNAMIC_REF = 1
 DEFAULT_REF_MODE = DYNAMIC_REF
 REF_MODE = [_("Static ref."), _("Dynamic ref.")]
 
+DEFAULT_COIL = SELECT
+COIL = [_("Select coil:"), _("Neurosoft Figure-8"),
+           _("Magstim 70 mm"), _("Nexstim")]
+
 IR1 = wx.NewId()
 IR2 = wx.NewId()
 IR3 = wx.NewId()
@@ -694,19 +705,48 @@ BTNS_IMG_MKS = {IR1: {0: 'LEI'},
             IR2: {1: 'REI'},
             IR3: {2: 'NAI'}}
 
-TIPS_IMG = [wx.ToolTip(_("Select left ear in image")),
-            wx.ToolTip(_("Select right ear in image")),
-            wx.ToolTip(_("Select nasion in image"))]
+TIPS_IMG = [_("Select left ear in image"),
+            _("Select right ear in image"),
+            _("Select nasion in image")]
 
 BTNS_TRK = {TR1: {3: _('LET')},
             TR2: {4: _('RET')},
             TR3: {5: _('NAT')},
             SET: {6: _('SET')}}
 
-TIPS_TRK = [wx.ToolTip(_("Select left ear with spatial tracker")),
-            wx.ToolTip(_("Select right ear with spatial tracker")),
-            wx.ToolTip(_("Select nasion with spatial tracker")),
-            wx.ToolTip(_("Show set coordinates in image"))]
+TIPS_TRK = [_("Select left ear with spatial tracker"),
+            _("Select right ear with spatial tracker"),
+            _("Select nasion with spatial tracker"),
+            _("Show set coordinates in image")]
+
+OBJL = wx.NewId()
+OBJR = wx.NewId()
+OBJA = wx.NewId()
+OBJC = wx.NewId()
+OBJF = wx.NewId()
+
+BTNS_OBJ = {OBJL: {0: _('Left')},
+            OBJR: {1: _('Right')},
+            OBJA: {2: _('Anterior')},
+            OBJC: {3: _('Center')},
+            OBJF: {4: _('Fixed')}}
+
+TIPS_OBJ = [_("Select left object fiducial"),
+            _("Select right object fiducial"),
+            _("Select anterior object fiducial"),
+            _("Select object center"),
+            _("Attach sensor to object")]
 
 CAL_DIR = os.path.abspath(os.path.join(FILE_PATH, '..', 'navigation', 'mtc_files', 'CalibrationFiles'))
 MAR_DIR = os.path.abspath(os.path.join(FILE_PATH, '..', 'navigation', 'mtc_files', 'Markers'))
+
+#OBJECT TRACKING
+OBJ_DIR = os.path.abspath(os.path.join(FILE_PATH, '..', 'navigation', 'objects'))
+ARROW_SCALE = 3
+ARROW_UPPER_LIMIT = 30
+#COIL_ANGLES_THRESHOLD = 3 * ARROW_SCALE
+COIL_ANGLES_THRESHOLD = 3
+COIL_COORD_THRESHOLD = 3
+TIMESTAMP = 2.0
+
+CAM_MODE = True

invesalius/data/bases.py

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

invesalius/data/coordinates.py

@@ -20,10 +20,13 @@
 from math import sin, cos
 import numpy as np
 
+import invesalius.data.transformations as tr
+
 from time import sleep
 from random import uniform
 from wx.lib.pubsub import pub as Publisher
 
+
 def GetCoordinates(trck_init, trck_id, ref_mode):
 
     """
@@ -44,7 +47,7 @@ def GetCoordinates(trck_init, trck_id, ref_mode):
                     5: DebugCoord}
         coord = getcoord[trck_id](trck_init, trck_id, ref_mode)
     else:
-        print "Select Tracker"
+        print("Select Tracker")
 
     return coord
 
@@ -54,34 +57,36 @@ def ClaronCoord(trck_init, trck_id, ref_mode):
     scale = np.array([1.0, 1.0, -1.0])
     coord = None
     k = 0
-    # TODO: try to replace while and use some Claron internal computation
+    # TODO: try to replace 'while' and use some Claron internal computation
 
     if ref_mode:
         while k < 20:
             try:
                 trck.Run()
-                probe = np.array([trck.PositionTooltipX1 * scale[0], trck.PositionTooltipY1 * scale[1],
-                                  trck.PositionTooltipZ1 * scale[2], trck.AngleX1, trck.AngleY1, trck.AngleZ1])
-                reference = np.array([trck.PositionTooltipX2 * scale[0], trck.PositionTooltipY2 * scale[1],
-                                      trck.PositionTooltipZ2 * scale[2], trck.AngleX2, trck.AngleY2, trck.AngleZ2])
+                probe = np.array([trck.PositionTooltipX1, trck.PositionTooltipY1,
+                                  trck.PositionTooltipZ1, trck.AngleX1, trck.AngleY1, trck.AngleZ1])
+                reference = np.array([trck.PositionTooltipX2, trck.PositionTooltipY2,
+                                      trck.PositionTooltipZ2, trck.AngleZ2,  trck.AngleY2, trck.AngleX2])
                 k = 30
             except AttributeError:
                 k += 1
-                print "wait, collecting coordinates ..."
+                print("wait, collecting coordinates ...")
         if k == 30:
             coord = dynamic_reference(probe, reference)
+            coord = (coord[0] * scale[0], coord[1] * scale[1], coord[2] * scale[2], coord[3], coord[4], coord[5])
     else:
         while k < 20:
             try:
                 trck.Run()
                 coord = np.array([trck.PositionTooltipX1 * scale[0], trck.PositionTooltipY1 * scale[1],
                                   trck.PositionTooltipZ1 * scale[2], trck.AngleX1, trck.AngleY1, trck.AngleZ1])
+
                 k = 30
             except AttributeError:
                 k += 1
-                print "wait, collecting coordinates ..."
+                print("wait, collecting coordinates ...")
 
-    Publisher.sendMessage('Sensors ID', [trck.probeID, trck.refID])
+    Publisher.sendMessage('Sensors ID', probe_id=trck.probeID, ref_id=trck.refID)
 
     return coord
 
@@ -103,26 +108,23 @@ def PolhemusCoord(trck, trck_id, ref_mode):
 
 def PolhemusWrapperCoord(trck, trck_id, ref_mode):
 
-    scale = 25.4 * np.array([1., 1.0, -1.0])
-    coord = None
-
-    if ref_mode:
     trck.Run()
-        probe = np.array([float(trck.PositionTooltipX1) * scale[0], float(trck.PositionTooltipY1) * scale[1],
-                          float(trck.PositionTooltipZ1) * scale[2], float(trck.AngleX1), float(trck.AngleY1),
-                          float(trck.AngleZ1)])
-        reference = np.array([float(trck.PositionTooltipX2) * scale[0], float(trck.PositionTooltipY2) * scale[1],
-                          float(trck.PositionTooltipZ2) * scale[2], float(trck.AngleX2), float(trck.AngleY2),
-                          float(trck.AngleZ2)])
+    scale = 10.0 * np.array([1., 1., 1.])
 
-        if probe.all() and reference.all():
-            coord = dynamic_reference(probe, reference)
+    coord1 = np.array([float(trck.PositionTooltipX1)*scale[0], float(trck.PositionTooltipY1)*scale[1],
+                      float(trck.PositionTooltipZ1)*scale[2],
+                      float(trck.AngleX1), float(trck.AngleY1), float(trck.AngleZ1)])
 
-    else:
-        trck.Run()
-        coord = np.array([float(trck.PositionTooltipX1) * scale[0], float(trck.PositionTooltipY1) * scale[1],
-                          float(trck.PositionTooltipZ1) * scale[2], float(trck.AngleX1), float(trck.AngleY1),
-                          float(trck.AngleZ1)])
+    coord2 = np.array([float(trck.PositionTooltipX2)*scale[0], float(trck.PositionTooltipY2)*scale[1],
+                       float(trck.PositionTooltipZ2)*scale[2],
+                       float(trck.AngleX2), float(trck.AngleY2), float(trck.AngleZ2)])
+    coord = np.vstack([coord1, coord2])
+
+    if trck_id == 2:
+        coord3 = np.array([float(trck.PositionTooltipX3) * scale[0], float(trck.PositionTooltipY3) * scale[1],
+                           float(trck.PositionTooltipZ3) * scale[2],
+                           float(trck.AngleX3), float(trck.AngleY3), float(trck.AngleZ3)])
+        coord = np.vstack([coord, coord3])
 
     if trck.StylusButton:
         Publisher.sendMessage('PLH Stylus Button On')
@@ -148,13 +150,12 @@ def PolhemusUSBCoord(trck, trck_id, ref_mode):
 
         # six coordinates of first and second sensor: x, y, z and alfa, beta and gama
         # jump one element for reference to avoid the sensor ID returned by Polhemus
-        probe = data[0] * scale[0], data[1] * scale[1], data[2] * scale[2], \
-                data[3], data[4], data[5], data[6]
-        reference = data[7] * scale[0], data[8] * scale[1], data[9] * scale[2], data[10], \
-                    data[11], data[12], data[13]
+        probe = data[0], data[1], data[2], data[3], data[4], data[5], data[6]
+        reference = data[7], data[8], data[9], data[10], data[11], data[12], data[13]
 
         if probe.all() and reference.all():
             coord = dynamic_reference(probe, reference)
+            coord = (coord[0] * scale[0], coord[1] * scale[1], coord[2] * scale[2], coord[3], coord[4], coord[5])
 
         return coord
 
@@ -179,7 +180,7 @@ def PolhemusSerialCoord(trck_init, trck_id, ref_mode):
     coord = None
 
     if lines[0][0] != '0':
-        print "The Polhemus is not connected!"
+        print("The Polhemus is not connected!")
     else:
         for s in lines:
             if s[1] == '1':
@@ -197,7 +198,7 @@ def PolhemusSerialCoord(trck_init, trck_id, ref_mode):
                     plh1 = [float(s) for s in data[1:len(data)]]
                     j = 1
                 except:
-                    print "error!!"
+                    print("error!!")
 
             coord = data[0:6]
     return coord
@@ -212,22 +213,21 @@ def DebugCoord(trk_init, trck_id, ref_mode):
     :param trck_id: id of tracking device
     :return: six coordinates x, y, z, alfa, beta and gama
     """
-    sleep(0.2)
-    if ref_mode:
-        probe = np.array([uniform(1, 200), uniform(1, 200), uniform(1, 200),
-                          uniform(1, 200), uniform(1, 200), uniform(1, 200)])
-        reference = np.array([uniform(1, 200), uniform(1, 200), uniform(1, 200),
-                              uniform(1, 200), uniform(1, 200), uniform(1, 200)])
 
-        coord = dynamic_reference(probe, reference)
+    sleep(0.05)
 
-    else:
-        coord = np.array([uniform(1, 200), uniform(1, 200), uniform(1, 200),
-                          uniform(1, 200), uniform(1, 200), uniform(1, 200)])
+    coord1 = np.array([uniform(1, 200), uniform(1, 200), uniform(1, 200),
+                      uniform(-180.0, 180.0), uniform(-180.0, 180.0), uniform(-180.0, 180.0)])
 
-    Publisher.sendMessage('Sensors ID', [int(uniform(0, 5)), int(uniform(0, 5))])
+    coord2 = np.array([uniform(1, 200), uniform(1, 200), uniform(1, 200),
+                       uniform(-180.0, 180.0), uniform(-180.0, 180.0), uniform(-180.0, 180.0)])
 
-    return coord
+    coord3 = np.array([uniform(1, 200), uniform(1, 200), uniform(1, 200),
+                       uniform(-180.0, 180.0), uniform(-180.0, 180.0), uniform(-180.0, 180.0)])
+
+    Publisher.sendMessage('Sensors ID', probe_id=int(uniform(0, 5)), ref_id=int(uniform(0, 5)))
+
+    return np.vstack([coord1, coord2, coord3])
 
 
 def dynamic_reference(probe, reference):
@@ -244,28 +244,143 @@ def dynamic_reference(probe, reference):
     """
     a, b, g = np.radians(reference[3:6])
 
-    vet = probe[0:3] - reference[0:3]
-    vet = np.mat(vet.reshape(3, 1))
+    vet = np.asmatrix(probe[0:3] - reference[0:3])
+    # vet = np.mat(vet.reshape(3, 1))
 
-    # Attitude Matrix given by Patriot Manual
-    Mrot = np.mat([[cos(a) * cos(b), sin(b) * sin(g) * cos(a) - cos(g) * sin(a),
+    # Attitude matrix given by Patriot manual
+    # a: rotation of plane (X, Y) around Z axis (azimuth)
+    # b: rotation of plane (X', Z) around Y' axis (elevation)
+    # a: rotation of plane (Y', Z') around X'' axis (roll)
+    m_rot = np.mat([[cos(a) * cos(b), sin(b) * sin(g) * cos(a) - cos(g) * sin(a),
                     cos(a) * sin(b) * cos(g) + sin(a) * sin(g)],
                    [cos(b) * sin(a), sin(b) * sin(g) * sin(a) + cos(g) * cos(a),
                     cos(g) * sin(b) * sin(a) - sin(g) * cos(a)],
                    [-sin(b), sin(g) * cos(b), cos(b) * cos(g)]])
 
-    coord_rot = Mrot.T * vet
+    # coord_rot = m_rot.T * vet
+    coord_rot = vet*m_rot
+    coord_rot = np.squeeze(np.asarray(coord_rot))
+
+    return coord_rot[0], coord_rot[1], -coord_rot[2], probe[3], probe[4], probe[5]
+
+
+def dynamic_reference_m(probe, reference):
+    """
+    Apply dynamic reference correction to probe coordinates. Uses the alpha, beta and gama
+    rotation angles of reference to rotate the probe coordinate and returns the x, y, z
+    difference between probe and reference. Angles sequences and equation was extracted from
+    Polhemus manual and Attitude matrix in Wikipedia.
+    General equation is:
+    coord = Mrot * (probe - reference)
+    :param probe: sensor one defined as probe
+    :param reference: sensor two defined as reference
+    :return: rotated and translated coordinates
+    """
+
+    a, b, g = np.radians(reference[3:6])
+
+    T = tr.translation_matrix(reference[:3])
+    R = tr.euler_matrix(a, b, g, 'rzyx')
+    M = np.asmatrix(tr.concatenate_matrices(T, R))
+    # M = tr.compose_matrix(angles=np.radians(reference[3:6]), translate=reference[:3])
+    # print M
+    probe_4 = np.vstack((np.asmatrix(probe[:3]).reshape([3, 1]), 1.))
+    coord_rot = M.I * probe_4
     coord_rot = np.squeeze(np.asarray(coord_rot))
 
-    return coord_rot[0], coord_rot[1], coord_rot[2], probe[3], probe[4], probe[5]
+    return coord_rot[0], coord_rot[1], -coord_rot[2], probe[3], probe[4], probe[5]
+
+def dynamic_reference_m2(probe, reference):
+    """
+    Apply dynamic reference correction to probe coordinates. Uses the alpha, beta and gama
+    rotation angles of reference to rotate the probe coordinate and returns the x, y, z
+    difference between probe and reference. Angles sequences and equation was extracted from
+    Polhemus manual and Attitude matrix in Wikipedia.
+    General equation is:
+    coord = Mrot * (probe - reference)
+    :param probe: sensor one defined as probe
+    :param reference: sensor two defined as reference
+    :return: rotated and translated coordinates
+    """
+
+    a, b, g = np.radians(reference[3:6])
+    a_p, b_p, g_p = np.radians(probe[3:6])
+
+    T = tr.translation_matrix(reference[:3])
+    T_p = tr.translation_matrix(probe[:3])
+    R = tr.euler_matrix(a, b, g, 'rzyx')
+    R_p = tr.euler_matrix(a_p, b_p, g_p, 'rzyx')
+    M = np.asmatrix(tr.concatenate_matrices(T, R))
+    M_p = np.asmatrix(tr.concatenate_matrices(T_p, R_p))
+    # M = tr.compose_matrix(angles=np.radians(reference[3:6]), translate=reference[:3])
+    # print M
+
+    M_dyn = M.I * M_p
+
+    al, be, ga = tr.euler_from_matrix(M_dyn, 'rzyx')
+    coord_rot = tr.translation_from_matrix(M_dyn)
+
+    coord_rot = np.squeeze(coord_rot)
+
+    # probe_4 = np.vstack((np.asmatrix(probe[:3]).reshape([3, 1]), 1.))
+    # coord_rot_test = M.I * probe_4
+    # coord_rot_test = np.squeeze(np.asarray(coord_rot_test))
+    #
+    # print "coord_rot: ", coord_rot
+    # print "coord_rot_test: ", coord_rot_test
+    # print "test: ", np.allclose(coord_rot, coord_rot_test[:3])
+
+    return coord_rot[0], coord_rot[1], coord_rot[2], np.degrees(al), np.degrees(be), np.degrees(ga)
+
+# def dynamic_reference_m3(probe, reference):
+#     """
+#     Apply dynamic reference correction to probe coordinates. Uses the alpha, beta and gama
+#     rotation angles of reference to rotate the probe coordinate and returns the x, y, z
+#     difference between probe and reference. Angles sequences and equation was extracted from
+#     Polhemus manual and Attitude matrix in Wikipedia.
+#     General equation is:
+#     coord = Mrot * (probe - reference)
+#     :param probe: sensor one defined as probe
+#     :param reference: sensor two defined as reference
+#     :return: rotated and translated coordinates
+#     """
+#
+#     a, b, g = np.radians(reference[3:6])
+#     a_p, b_p, g_p = np.radians(probe[3:6])
+#
+#     T = tr.translation_matrix(reference[:3])
+#     T_p = tr.translation_matrix(probe[:3])
+#     R = tr.euler_matrix(a, b, g, 'rzyx')
+#     R_p = tr.euler_matrix(a_p, b_p, g_p, 'rzyx')
+#     M = np.asmatrix(tr.concatenate_matrices(T, R))
+#     M_p = np.asmatrix(tr.concatenate_matrices(T_p, R_p))
+#     # M = tr.compose_matrix(angles=np.radians(reference[3:6]), translate=reference[:3])
+#     # print M
+#
+#     M_dyn = M.I * M_p
+#
+#     # al, be, ga = tr.euler_from_matrix(M_dyn, 'rzyx')
+#     # coord_rot = tr.translation_from_matrix(M_dyn)
+#     #
+#     # coord_rot = np.squeeze(coord_rot)
+#
+#     # probe_4 = np.vstack((np.asmatrix(probe[:3]).reshape([3, 1]), 1.))
+#     # coord_rot_test = M.I * probe_4
+#     # coord_rot_test = np.squeeze(np.asarray(coord_rot_test))
+#     #
+#     # print "coord_rot: ", coord_rot
+#     # print "coord_rot_test: ", coord_rot_test
+#     # print "test: ", np.allclose(coord_rot, coord_rot_test[:3])
+#
+#     return M_dyn
 
 
 def str2float(data):
     """
-    Converts string detected wth Polhemus device to float array of coordinates. THis method applies
+    Converts string detected wth Polhemus device to float array of coordinates. This method applies
     a correction for the minus sign in string that raises error while splitting the string into coordinates.
     :param data: string of coordinates read with Polhemus
-    :return: six float coordinates x, y, z, alfa, beta and gama
+    :return: six float coordinates x, y, z, alpha, beta and gamma
     """
 
     count = 0

invesalius/data/coregistration.py

@@ -20,16 +20,123 @@
 import threading
 from time import sleep
 
-from numpy import mat
+from numpy import asmatrix, mat, degrees, radians, identity
 import wx
 from wx.lib.pubsub import pub as Publisher
 
 import invesalius.data.coordinates as dco
+import invesalius.data.transformations as tr
 
 # TODO: Optimize navigation thread. Remove the infinite loop and optimize sleep.
 
 
-class Coregistration(threading.Thread):
+class CoregistrationStatic(threading.Thread):
+    """
+    Thread to update the coordinates with the fiducial points
+    co-registration method while the Navigation Button is pressed.
+    Sleep function in run method is used to avoid blocking GUI and
+    for better real-time navigation
+    """
+
+    def __init__(self, coreg_data, nav_id, trck_info):
+        threading.Thread.__init__(self)
+        self.coreg_data = coreg_data
+        self.nav_id = nav_id
+        self.trck_info = trck_info
+        self._pause_ = False
+        self.start()
+
+    def stop(self):
+        self._pause_ = True
+
+    def run(self):
+        # m_change = self.coreg_data[0]
+        # obj_ref_mode = self.coreg_data[2]
+        #
+        # trck_init = self.trck_info[0]
+        # trck_id = self.trck_info[1]
+        # trck_mode = self.trck_info[2]
+
+        m_change, obj_ref_mode = self.coreg_data
+        trck_init, trck_id, trck_mode = self.trck_info
+
+        while self.nav_id:
+            coord_raw = dco.GetCoordinates(trck_init, trck_id, trck_mode)
+
+            psi, theta, phi = coord_raw[obj_ref_mode, 3:]
+            t_probe_raw = asmatrix(tr.translation_matrix(coord_raw[obj_ref_mode, :3]))
+
+            t_probe_raw[2, -1] = -t_probe_raw[2, -1]
+
+            m_img = m_change * t_probe_raw
+
+            coord = m_img[0, -1], m_img[1, -1], m_img[2, -1], psi, theta, phi
+
+            wx.CallAfter(Publisher.sendMessage, 'Co-registered points', arg=m_img, position=coord)
+
+            # TODO: Optimize the value of sleep for each tracking device.
+            sleep(0.175)
+
+            if self._pause_:
+                return
+
+
+class CoregistrationDynamic(threading.Thread):
+    """
+    Thread to update the coordinates with the fiducial points
+    co-registration method while the Navigation Button is pressed.
+    Sleep function in run method is used to avoid blocking GUI and
+    for better real-time navigation
+    """
+
+    def __init__(self, coreg_data, nav_id, trck_info):
+        threading.Thread.__init__(self)
+        self.coreg_data = coreg_data
+        self.nav_id = nav_id
+        self.trck_info = trck_info
+        self._pause_ = False
+        self.start()
+
+    def stop(self):
+        self._pause_ = True
+
+    def run(self):
+        m_change, obj_ref_mode = self.coreg_data
+        trck_init, trck_id, trck_mode = self.trck_info
+
+        while self.nav_id:
+            coord_raw = dco.GetCoordinates(trck_init, trck_id, trck_mode)
+
+            psi, theta, phi = radians(coord_raw[obj_ref_mode, 3:])
+            r_probe = tr.euler_matrix(psi, theta, phi, 'rzyx')
+            t_probe = tr.translation_matrix(coord_raw[obj_ref_mode, :3])
+            m_probe = asmatrix(tr.concatenate_matrices(t_probe, r_probe))
+
+            psi_ref, theta_ref, phi_ref = radians(coord_raw[1, 3:])
+            r_ref = tr.euler_matrix(psi_ref, theta_ref, phi_ref, 'rzyx')
+            t_ref = tr.translation_matrix(coord_raw[1, :3])
+            m_ref = asmatrix(tr.concatenate_matrices(t_ref, r_ref))
+
+            m_dyn = m_ref.I * m_probe
+            m_dyn[2, -1] = -m_dyn[2, -1]
+
+            m_img = m_change * m_dyn
+
+            scale, shear, angles, trans, persp = tr.decompose_matrix(m_img)
+
+            coord = m_img[0, -1], m_img[1, -1], m_img[2, -1], \
+                    degrees(angles[0]), degrees(angles[1]), degrees(angles[2])
+
+            wx.CallAfter(Publisher.sendMessage, 'Co-registered points', arg=m_img, position=coord)
+
+            # TODO: Optimize the value of sleep for each tracking device.
+            sleep(0.175)
+
+            if self._pause_:
+                return
+
+
+class CoregistrationDynamic_old(threading.Thread):
     """
     Thread to update the coordinates with the fiducial points
     co-registration method while the Navigation Button is pressed.
@@ -58,18 +165,23 @@ class Coregistration(threading.Thread):
         trck_mode = self.trck_info[2]
 
         while self.nav_id:
-            trck_coord = dco.GetCoordinates(trck_init, trck_id, trck_mode)
-            trck_xyz = mat([[trck_coord[0]], [trck_coord[1]], [trck_coord[2]]])
+            # trck_coord, probe, reference = dco.GetCoordinates(trck_init, trck_id, trck_mode)
+            coord_raw = dco.GetCoordinates(trck_init, trck_id, trck_mode)
 
+            trck_coord = dco.dynamic_reference(coord_raw[0, :], coord_raw[1, :])
+
+            trck_xyz = mat([[trck_coord[0]], [trck_coord[1]], [trck_coord[2]]])
             img = q1 + (m_inv * n) * (trck_xyz - q2)
 
             coord = (float(img[0]), float(img[1]), float(img[2]), trck_coord[3],
                      trck_coord[4], trck_coord[5])
+            angles = coord_raw[0, 3:6]
 
             # Tried several combinations and different locations to send the messages,
             # however only this one does not block the GUI during navigation.
-            wx.CallAfter(Publisher.sendMessage, 'Co-registered points', coord[0:3])
-            wx.CallAfter(Publisher.sendMessage, 'Set camera in volume', coord[0:3])
+            wx.CallAfter(Publisher.sendMessage, 'Co-registered points', arg=None, position=coord)
+            wx.CallAfter(Publisher.sendMessage, 'Set camera in volume', coord)
+            wx.CallAfter(Publisher.sendMessage, 'Update tracker angles', angles)
 
             # TODO: Optimize the value of sleep for each tracking device.
             # Debug tracker is not working with 0.175 so changed to 0.2
@@ -79,3 +191,173 @@ class Coregistration(threading.Thread):
 
             if self._pause_:
                 return
+
+
+class CoregistrationObjectStatic(threading.Thread):
+    """
+    Thread to update the coordinates with the fiducial points
+    co-registration method while the Navigation Button is pressed.
+    Sleep function in run method is used to avoid blocking GUI and
+    for better real-time navigation
+    """
+
+    def __init__(self, coreg_data, nav_id, trck_info):
+        threading.Thread.__init__(self)
+        self.coreg_data = coreg_data
+        self.nav_id = nav_id
+        self.trck_info = trck_info
+        self._pause_ = False
+        self.start()
+
+    def stop(self):
+        self._pause_ = True
+
+    def run(self):
+        # m_change = self.coreg_data[0]
+        # t_obj_raw = self.coreg_data[1]
+        # s0_raw = self.coreg_data[2]
+        # r_s0_raw = self.coreg_data[3]
+        # s0_dyn = self.coreg_data[4]
+        # m_obj_raw = self.coreg_data[5]
+        # r_obj_img = self.coreg_data[6]
+        # obj_ref_mode = self.coreg_data[7]
+        #
+        # trck_init = self.trck_info[0]
+        # trck_id = self.trck_info[1]
+        # trck_mode = self.trck_info[2]
+
+        m_change, obj_ref_mode, t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img = self.coreg_data
+        trck_init, trck_id, trck_mode = self.trck_info
+
+        while self.nav_id:
+            coord_raw = dco.GetCoordinates(trck_init, trck_id, trck_mode)
+
+            as1, bs1, gs1 = radians(coord_raw[obj_ref_mode, 3:])
+            r_probe = asmatrix(tr.euler_matrix(as1, bs1, gs1, 'rzyx'))
+            t_probe_raw = asmatrix(tr.translation_matrix(coord_raw[obj_ref_mode, :3]))
+            t_offset_aux = r_s0_raw.I * r_probe * t_obj_raw
+            t_offset = asmatrix(identity(4))
+            t_offset[:, -1] = t_offset_aux[:, -1]
+            t_probe = s0_raw * t_offset * s0_raw.I * t_probe_raw
+            m_probe = asmatrix(tr.concatenate_matrices(t_probe, r_probe))
+
+            m_probe[2, -1] = -m_probe[2, -1]
+
+            m_img = m_change * m_probe
+            r_obj = r_obj_img * m_obj_raw.I * s0_dyn.I * m_probe * m_obj_raw
+
+            m_img[:3, :3] = r_obj[:3, :3]
+
+            scale, shear, angles, trans, persp = tr.decompose_matrix(m_img)
+
+            coord = m_img[0, -1], m_img[1, -1], m_img[2, -1], \
+                    degrees(angles[0]), degrees(angles[1]), degrees(angles[2])
+
+            wx.CallAfter(Publisher.sendMessage, 'Co-registered points', arg=m_img, position=coord)
+            wx.CallAfter(Publisher.sendMessage, 'Update object matrix', m_img=m_img, coord=coord)
+
+            # TODO: Optimize the value of sleep for each tracking device.
+            sleep(0.175)
+
+            # Debug tracker is not working with 0.175 so changed to 0.2
+            # However, 0.2 is too low update frequency ~5 Hz. Need optimization URGENTLY.
+            # sleep(.3)
+
+            # partially working for translate and offset,
+            # but offset is kept always in same axis, have to fix for rotation
+            # M_dyn = M_reference.I * T_stylus
+            # M_dyn[2, -1] = -M_dyn[2, -1]
+            # M_dyn_ch = M_change * M_dyn
+            # ddd = M_dyn_ch[0, -1], M_dyn_ch[1, -1], M_dyn_ch[2, -1]
+            # M_dyn_ch[:3, -1] = asmatrix(db.flip_x_m(ddd)).reshape([3, 1])
+            # M_final = S0 * M_obj_trans_0 * S0.I * M_dyn_ch
+
+            # this works for static reference object rotation
+            # R_dyn = M_vtk * M_obj_rot_raw.I * S0_rot_raw.I * R_stylus * M_obj_rot_raw
+            # this works for dynamic reference in rotation but not in translation
+            # R_dyn = M_vtk * M_obj_rot_raw.I * S0_rot_dyn.I * R_reference.I * R_stylus * M_obj_rot_raw
+
+            if self._pause_:
+                return
+
+
+class CoregistrationObjectDynamic(threading.Thread):
+    """
+    Thread to update the coordinates with the fiducial points
+    co-registration method while the Navigation Button is pressed.
+    Sleep function in run method is used to avoid blocking GUI and
+    for better real-time navigation
+    """
+
+    def __init__(self, coreg_data, nav_id, trck_info):
+        threading.Thread.__init__(self)
+        self.coreg_data = coreg_data
+        self.nav_id = nav_id
+        self.trck_info = trck_info
+        self._pause_ = False
+        self.start()
+
+    def stop(self):
+        self._pause_ = True
+
+    def run(self):
+
+        m_change, obj_ref_mode, t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img = self.coreg_data
+        trck_init, trck_id, trck_mode = self.trck_info
+
+        while self.nav_id:
+            coord_raw = dco.GetCoordinates(trck_init, trck_id, trck_mode)
+
+            as1, bs1, gs1 = radians(coord_raw[obj_ref_mode, 3:])
+            r_probe = asmatrix(tr.euler_matrix(as1, bs1, gs1, 'rzyx'))
+            t_probe_raw = asmatrix(tr.translation_matrix(coord_raw[obj_ref_mode, :3]))
+            t_offset_aux = r_s0_raw.I * r_probe * t_obj_raw
+            t_offset = asmatrix(identity(4))
+            t_offset[:, -1] = t_offset_aux[:, -1]
+            t_probe = s0_raw * t_offset * s0_raw.I * t_probe_raw
+            m_probe = asmatrix(tr.concatenate_matrices(t_probe, r_probe))
+
+            a, b, g = radians(coord_raw[1, 3:])
+            r_ref = tr.euler_matrix(a, b, g, 'rzyx')
+            t_ref = tr.translation_matrix(coord_raw[1, :3])
+            m_ref = asmatrix(tr.concatenate_matrices(t_ref, r_ref))
+
+            m_dyn = m_ref.I * m_probe
+            m_dyn[2, -1] = -m_dyn[2, -1]
+
+            m_img = m_change * m_dyn
+            r_obj = r_obj_img * m_obj_raw.I * s0_dyn.I * m_dyn * m_obj_raw
+
+            m_img[:3, :3] = r_obj[:3, :3]
+
+            scale, shear, angles, trans, persp = tr.decompose_matrix(m_img)
+
+            coord = m_img[0, -1], m_img[1, -1], m_img[2, -1],\
+                    degrees(angles[0]), degrees(angles[1]), degrees(angles[2])
+
+            wx.CallAfter(Publisher.sendMessage, 'Co-registered points', arg=m_img, position=coord)
+            wx.CallAfter(Publisher.sendMessage, 'Update object matrix', m_img=m_img, coord=coord)
+
+            # TODO: Optimize the value of sleep for each tracking device.
+            sleep(0.175)
+
+            # Debug tracker is not working with 0.175 so changed to 0.2
+            # However, 0.2 is too low update frequency ~5 Hz. Need optimization URGENTLY.
+            # sleep(.3)
+
+            # partially working for translate and offset,
+            # but offset is kept always in same axis, have to fix for rotation
+            # M_dyn = M_reference.I * T_stylus
+            # M_dyn[2, -1] = -M_dyn[2, -1]
+            # M_dyn_ch = M_change * M_dyn
+            # ddd = M_dyn_ch[0, -1], M_dyn_ch[1, -1], M_dyn_ch[2, -1]
+            # M_dyn_ch[:3, -1] = asmatrix(db.flip_x_m(ddd)).reshape([3, 1])
+            # M_final = S0 * M_obj_trans_0 * S0.I * M_dyn_ch
+
+            # this works for static reference object rotation
+            # R_dyn = M_vtk * M_obj_rot_raw.I * S0_rot_raw.I * R_stylus * M_obj_rot_raw
+            # this works for dynamic reference in rotation but not in translation
+            # R_dyn = M_vtk * M_obj_rot_raw.I * S0_rot_dyn.I * R_reference.I * R_stylus * M_obj_rot_raw
+
+            if self._pause_:
+                return