From 2f872421456312f58288358ea81d0e5677199078 Mon Sep 17 00:00:00 2001
From: Renan Matsuda <renan_hiroshi@hotmail.com>
Date: Tue, 27 Oct 2020 17:56:36 -0300
Subject: [PATCH] FRE testing...

---
 invesalius/data/bases.py          | 41 +++++++++++++++++++++++++++++++++++++++++
 invesalius/data/coregistration.py |  3 +++
 invesalius/gui/dialogs.py         |  2 +-
 invesalius/gui/task_navigator.py  |  1 +
 4 files changed, 46 insertions(+), 1 deletion(-)

diff --git a/invesalius/data/bases.py b/invesalius/data/bases.py
index 2f50feb..054d026 100644
--- a/invesalius/data/bases.py
+++ b/invesalius/data/bases.py
@@ -175,6 +175,47 @@ def calculate_fre_m(fiducials):
 
     return float(np.sqrt(np.sum(dist ** 2) / 3))
 
+def calculate_fre_matrix(fiducials, m_change):
+
+    dist = np.zeros([3, 1])
+
+    p1 = np.hstack((fiducials[3, :], 1)).reshape(4, 1)
+    p2 = np.hstack((fiducials[4, :], 1)).reshape(4, 1)
+    p3 = np.hstack((fiducials[5, :], 1)).reshape(4, 1)
+
+    print(p1)
+
+    p1 = flip_x_m([p1[0,0],p1[1,0],p1[2,0]])
+    p2 = flip_x_m([p2[0,0],p2[1,0],p2[2,0]])
+    p3 = flip_x_m([p3[0,0],p3[1,0],p3[2,0]])
+    print(p1)
+
+    p1 = np.hstack([np.identity(3), p1[:3]])
+    p2 = np.hstack([np.identity(3), p2[:3]])
+    p3 = np.hstack([np.identity(3), p3[:3]])
+
+    p1 = np.vstack([p1,[0,0,0,1]])
+    p2 = np.vstack([p2,[0,0,0,1]])
+    p3 = np.vstack([p3,[0,0,0,1]])
+    print(p1)
+
+    p1_m = m_change @ p1
+    p2_m = m_change @ p2
+    p3_m = m_change @ p3
+
+    #p1_m = flip_x_m([p1_m[0,0],p1_m[1,0],p1_m[2,0]])
+    #p2_m = flip_x_m([p2_m[0,0],p2_m[1,0],p2_m[2,0]])
+    #p3_m = flip_x_m([p3_m[0,0],p3_m[1,0],p3_m[2,0]])
+
+    print(p1_m)
+    print(fiducials[0, :])
+
+    dist[0] = np.sqrt(np.sum(np.power((p1_m[:3] - fiducials[0, :]), 2)))
+    dist[1] = np.sqrt(np.sum(np.power((p2_m[:3] - fiducials[1, :]), 2)))
+    dist[2] = np.sqrt(np.sum(np.power((p3_m[:3] - fiducials[2, :]), 2)))
+
+    return float(np.sqrt(np.sum(dist ** 2) / 3))
+
 
 # def flip_x(point):
 #     """
diff --git a/invesalius/data/coregistration.py b/invesalius/data/coregistration.py
index 879edab..6bcf950 100644
--- a/invesalius/data/coregistration.py
+++ b/invesalius/data/coregistration.py
@@ -155,10 +155,13 @@ def corregistrate_dynamic(inp, coord_raw, ref_mode_id, icp):
     else:
         m_probe_ref = m_probe
 
+    print(m_probe_ref)
     # 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
+    print(m_img)
+
     if icp[0]:
         m_img = bases.transform_icp(m_img, icp[1])
     # compute rotation angles
diff --git a/invesalius/gui/dialogs.py b/invesalius/gui/dialogs.py
index 468e7df..2df06ae 100644
--- a/invesalius/gui/dialogs.py
+++ b/invesalius/gui/dialogs.py
@@ -3606,7 +3606,7 @@ class ICPCorregistrationDialog(wx.Dialog):
     def LoadActor(self):
         mapper = vtk.vtkPolyDataMapper()
         mapper.SetInputData(self.surface)
-        #mapper.ScalarVisibilityOff()
+        mapper.ScalarVisibilityOff()
         #mapper.ImmediateModeRenderingOn()
 
         obj_actor = vtk.vtkActor()
diff --git a/invesalius/gui/task_navigator.py b/invesalius/gui/task_navigator.py
index 76a6427..8ed0adf 100644
--- a/invesalius/gui/task_navigator.py
+++ b/invesalius/gui/task_navigator.py
@@ -779,6 +779,7 @@ class NeuronavigationPanel(wx.Panel):
                 # fre = db.calculate_fre(self.fiducials, minv, n, q1, q2)
                 fre = db.calculate_fre_m(self.fiducials)
                 self.UpdateFRE(fre)
+                print(db.calculate_fre_matrix(self.fiducials, m_change))
 
                 if self.track_obj:
                     # if object tracking is selected
--
libgit2 0.21.2