FRE testing...

Renan Matsuda
1 parent 37a8fcce
Showing 4 changed files with 46 additions and 1 deletions Show diff stats
invesalius/data/bases.py
invesalius/data/coregistration.py
invesalius/gui/dialogs.py
invesalius/gui/task_navigator.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):
 #     """
@@ -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
@@ -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()
@@ -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
...	...	@@ -175,6 +175,47 @@ def calculate_fre_m(fiducials):
175	175
176	176	return float(np.sqrt(np.sum(dist ** 2) / 3))
177	177
	178	+def calculate_fre_matrix(fiducials, m_change):
	179	+
	180	+ dist = np.zeros([3, 1])
	181	+
	182	+ p1 = np.hstack((fiducials[3, :], 1)).reshape(4, 1)
	183	+ p2 = np.hstack((fiducials[4, :], 1)).reshape(4, 1)
	184	+ p3 = np.hstack((fiducials[5, :], 1)).reshape(4, 1)
	185	+
	186	+ print(p1)
	187	+
	188	+ p1 = flip_x_m([p1[0,0],p1[1,0],p1[2,0]])
	189	+ p2 = flip_x_m([p2[0,0],p2[1,0],p2[2,0]])
	190	+ p3 = flip_x_m([p3[0,0],p3[1,0],p3[2,0]])
	191	+ print(p1)
	192	+
	193	+ p1 = np.hstack([np.identity(3), p1[:3]])
	194	+ p2 = np.hstack([np.identity(3), p2[:3]])
	195	+ p3 = np.hstack([np.identity(3), p3[:3]])
	196	+
	197	+ p1 = np.vstack([p1,[0,0,0,1]])
	198	+ p2 = np.vstack([p2,[0,0,0,1]])
	199	+ p3 = np.vstack([p3,[0,0,0,1]])
	200	+ print(p1)
	201	+
	202	+ p1_m = m_change @ p1
	203	+ p2_m = m_change @ p2
	204	+ p3_m = m_change @ p3
	205	+
	206	+ #p1_m = flip_x_m([p1_m[0,0],p1_m[1,0],p1_m[2,0]])
	207	+ #p2_m = flip_x_m([p2_m[0,0],p2_m[1,0],p2_m[2,0]])
	208	+ #p3_m = flip_x_m([p3_m[0,0],p3_m[1,0],p3_m[2,0]])
	209	+
	210	+ print(p1_m)
	211	+ print(fiducials[0, :])
	212	+
	213	+ dist[0] = np.sqrt(np.sum(np.power((p1_m[:3] - fiducials[0, :]), 2)))
	214	+ dist[1] = np.sqrt(np.sum(np.power((p2_m[:3] - fiducials[1, :]), 2)))
	215	+ dist[2] = np.sqrt(np.sum(np.power((p3_m[:3] - fiducials[2, :]), 2)))
	216	+
	217	+ return float(np.sqrt(np.sum(dist ** 2) / 3))
	218	+
178	219
179	220	# def flip_x(point):
180	221	# """
...	...
...	...	@@ -155,10 +155,13 @@ def corregistrate_dynamic(inp, coord_raw, ref_mode_id, icp):
155	155	else:
156	156	m_probe_ref = m_probe
157	157
	158	+ print(m_probe_ref)
158	159	# invert y coordinate
159	160	m_probe_ref[2, -1] = -m_probe_ref[2, -1]
160	161	# corregistrate from tracker to image space
161	162	m_img = m_change @ m_probe_ref
	163	+ print(m_img)
	164	+
162	165	if icp[0]:
163	166	m_img = bases.transform_icp(m_img, icp[1])
164	167	# compute rotation angles
...	...
...	...	@@ -3606,7 +3606,7 @@ class ICPCorregistrationDialog(wx.Dialog):
3606	3606	def LoadActor(self):
3607	3607	mapper = vtk.vtkPolyDataMapper()
3608	3608	mapper.SetInputData(self.surface)
3609		- #mapper.ScalarVisibilityOff()
	3609	+ mapper.ScalarVisibilityOff()
3610	3610	#mapper.ImmediateModeRenderingOn()
3611	3611
3612	3612	obj_actor = vtk.vtkActor()
...	...
...	...	@@ -779,6 +779,7 @@ class NeuronavigationPanel(wx.Panel):
779	779	# fre = db.calculate_fre(self.fiducials, minv, n, q1, q2)
780	780	fre = db.calculate_fre_m(self.fiducials)
781	781	self.UpdateFRE(fre)
	782	+ print(db.calculate_fre_matrix(self.fiducials, m_change))
782	783
783	784	if self.track_obj:
784	785	# if object tracking is selected
...	...