diff --git a/invesalius/data/bases.py b/invesalius/data/bases.py
index 054d026..2e0c720 100644
--- a/invesalius/data/bases.py
+++ b/invesalius/data/bases.py
@@ -1,7 +1,7 @@
 import numpy as np
 import invesalius.data.coordinates as dco
 import invesalius.data.transformations as tr
-
+import invesalius.data.coregistration as dcr
 
 def angle_calculation(ap_axis, coil_axis):
     """
@@ -175,37 +175,18 @@ def calculate_fre_m(fiducials):
 
     return float(np.sqrt(np.sum(dist ** 2) / 3))
 
-def calculate_fre_matrix(fiducials, m_change):
+def calculate_fre(fiducials_raw, fiducials, ref_mode_id, 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 = np.vstack([fiducials_raw[0, :], fiducials_raw[1, :]])
+    p2 = np.vstack([fiducials_raw[2, :], fiducials_raw[3, :]])
+    p3 = np.vstack([fiducials_raw[4, :], fiducials_raw[5, :]])
 
-    #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]])
+    coreg_data = (m_change, 0)
+    p1_m, m_img = dcr.corregistrate_dynamic(coreg_data, p1, ref_mode_id, [None, None])
+    p2_m, m_img = dcr.corregistrate_dynamic(coreg_data, p2, ref_mode_id, [None, None])
+    p3_m, m_img = dcr.corregistrate_dynamic(coreg_data, p3, ref_mode_id, [None, None])
 
     print(p1_m)
     print(fiducials[0, :])
diff --git a/invesalius/gui/task_navigator.py b/invesalius/gui/task_navigator.py
index 8ed0adf..d8b118e 100644
--- a/invesalius/gui/task_navigator.py
+++ b/invesalius/gui/task_navigator.py
@@ -299,6 +299,7 @@ class NeuronavigationPanel(wx.Panel):
 
         # Initialize global variables
         self.fiducials = np.full([6, 3], np.nan)
+        self.fiducials_raw = np.zeros((6, 6))
         self.correg = None
         self.current_coord = 0, 0, 0
         self.trk_init = None
@@ -661,6 +662,16 @@ class NeuronavigationPanel(wx.Panel):
         # Update number controls with tracker coordinates
         if coord is not None:
             self.fiducials[btn_id, :] = coord[0:3]
+            if btn_id == 3:
+                self.fiducials_raw[0, :] = coord_raw[0, :]
+                self.fiducials_raw[1, :] = coord_raw[1, :]
+            elif btn_id == 4:
+                self.fiducials_raw[2, :] = coord_raw[0, :]
+                self.fiducials_raw[3, :] = coord_raw[1, :]
+            else:
+                self.fiducials_raw[4, :] = coord_raw[0, :]
+                self.fiducials_raw[5, :] = coord_raw[1, :]
+
             for n in [0, 1, 2]:
                 self.numctrls_coord[btn_id][n].SetValue(float(coord[n]))
 
@@ -779,7 +790,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))
+                print(db.calculate_fre(self.fiducials_raw, self.fiducials, self.ref_mode_id, m_change))
 
                 if self.track_obj:
                     # if object tracking is selected
--
libgit2 0.21.2