Download as
Browse Code »

Commit c21b81c51dc00c7bbb874dfe0ae0641d27b4f7e4

Authored by Renan
1 parent 06dd9de7
Exists in master

ENH: more variable name refractor 

-added opencv to optional-requirements
Showing 5 changed files with 91 additions and 83 deletions   Show diff stats
invesalius/data/bases.py
  Diff comments   View file @c21b81c
... ... @@ -279,7 +279,7 @@ class transform_tracker_to_robot(object):
279 279  
280 280 M_tracker = dco.coordinates_to_transformation_matrix(
281 281 position=tracker_coord[:3],
282   - orientation= tracker_coord[3:6],
  282 + orientation=tracker_coord[3:6],
283 283 axes='rzyx',
284 284 )
285 285 M_tracker_in_robot = transform_tracker_to_robot.M_tracker_to_robot @ M_tracker
... ...
invesalius/data/elfin_processing.py
  Diff comments   View file @c21b81c
... ... @@ -20,16 +20,21 @@ import numpy as np
20 20 import cv2
21 21 from time import time
22 22  
23   -import invesalius.data.transformations as tr
24 23 import invesalius.data.coregistration as dcr
  24 +import invesalius.data.coordinates as dco
25 25 import invesalius.constants as const
26 26  
27 27  
28 28 class KalmanTracker:
  29 + """
  30 + Kalman filter to avoid sudden fluctuation from tracker device.
  31 + The filter strength can be set by the cov_process, and cov_measure parameter
  32 + It is required to create one instance for each variable (x, y, z, a, b, g)
  33 + """
29 34 def __init__(self,
30 35 state_num=2,
31   - cov_process=0.001,
32   - cov_measure=0.1):
  36 + covariance_process=0.001,
  37 + covariance_measure=0.1):
33 38  
34 39 self.state_num = state_num
35 40 measure_num = 1
... ... @@ -46,8 +51,8 @@ class KalmanTracker:
46 51 [0, 1]], np.float32)
47 52 self.filter.measurementMatrix = np.array([[1, 1]], np.float32)
48 53 self.filter.processNoiseCov = np.array([[1, 0],
49   - [0, 1]], np.float32) * cov_process
50   - self.filter.measurementNoiseCov = np.array( [[1]], np.float32) * cov_measure
  54 + [0, 1]], np.float32) * covariance_process
  55 + self.filter.measurementNoiseCov = np.array([[1]], np.float32) * covariance_measure
51 56  
52 57 def update_kalman(self, measurement):
53 58 self.prediction = self.filter.predict()
... ... @@ -67,8 +72,8 @@ class TrackerProcessing:
67 72  
68 73 self.tracker_stabilizers = [KalmanTracker(
69 74 state_num=2,
70   - cov_process=0.001,
71   - cov_measure=0.1) for _ in range(6)]
  75 + covariance_process=0.001,
  76 + covariance_measure=0.1) for _ in range(6)]
72 77  
73 78 def kalman_filter(self, coord_tracker):
74 79 kalman_array = []
... ... @@ -100,41 +105,39 @@ class TrackerProcessing:
100 105 init_point = np.array(init_point)
101 106 final_point = np.array(final_point)
102 107 norm = (sum((final_point - init_point) ** 2)) ** 0.5
103   - versorfactor = (((final_point-init_point) / norm) * const.ROBOT_VERSOR_SCALE_FACTOR).tolist()
  108 + versor_factor = (((final_point-init_point) / norm) * const.ROBOT_VERSOR_SCALE_FACTOR).tolist()
104 109  
105   - return versorfactor
  110 + return versor_factor
106 111  
107   - def compute_arc_motion(self, actual_point, coord_head, coord_inv):
108   - p1 = coord_inv
109 112  
110   - pc = coord_head[0], coord_head[1], coord_head[2], coord_inv[3], coord_inv[4], coord_inv[5]
  113 + def compute_arc_motion(self, current_robot_coordinates, head_center_coordinates, new_robot_coordinates):
  114 + head_center = head_center_coordinates[0], head_center_coordinates[1], head_center_coordinates[2], \
  115 + new_robot_coordinates[3], new_robot_coordinates[4], new_robot_coordinates[5]
111 116  
112   - versorfactor1 = self.compute_versors(pc, actual_point)
113   - init_ext_point = actual_point[0] + versorfactor1[0], \
114   - actual_point[1] + versorfactor1[1], \
115   - actual_point[2] + versorfactor1[2], \
116   - actual_point[3], actual_point[4], actual_point[5]
  117 + versor_factor_move_out = self.compute_versors(head_center, current_robot_coordinates)
  118 + init_move_out_point = current_robot_coordinates[0] + versor_factor_move_out[0], \
  119 + current_robot_coordinates[1] + versor_factor_move_out[1], \
  120 + current_robot_coordinates[2] + versor_factor_move_out[2], \
  121 + current_robot_coordinates[3], current_robot_coordinates[4], current_robot_coordinates[5]
117 122  
118   - middle_point = ((p1[0] + actual_point[0]) / 2,
119   - (p1[1] + actual_point[1]) / 2,
120   - (p1[2] + actual_point[2]) / 2,
  123 + middle_point = ((new_robot_coordinates[0] + current_robot_coordinates[0]) / 2,
  124 + (new_robot_coordinates[1] + current_robot_coordinates[1]) / 2,
  125 + (new_robot_coordinates[2] + current_robot_coordinates[2]) / 2,
121 126 0, 0, 0)
  127 + versor_factor_middle_arc = (np.array(self.compute_versors(head_center, middle_point))) * 2
  128 + middle_arc_point = middle_point[0] + versor_factor_middle_arc[0], \
  129 + middle_point[1] + versor_factor_middle_arc[1], \
  130 + middle_point[2] + versor_factor_middle_arc[2]
122 131  
123   - newarr = (np.array(self.compute_versors(pc, middle_point))) * 2
124   - middle_arc_point = middle_point[0] + newarr[0], \
125   - middle_point[1] + newarr[1], \
126   - middle_point[2] + newarr[2]
127   -
128   - versorfactor3 = self.compute_versors(pc, p1)
129   -
130   - final_ext_arc_point = p1[0] + versorfactor3[0], \
131   - p1[1] + versorfactor3[1], \
132   - p1[2] + versorfactor3[2], \
133   - coord_inv[3], coord_inv[4], coord_inv[5], 0
  132 + versor_factor_arc = self.compute_versors(head_center, new_robot_coordinates)
  133 + final_ext_arc_point = new_robot_coordinates[0] + versor_factor_arc[0], \
  134 + new_robot_coordinates[1] + versor_factor_arc[1], \
  135 + new_robot_coordinates[2] + versor_factor_arc[2], \
  136 + new_robot_coordinates[3], new_robot_coordinates[4], new_robot_coordinates[5], 0
134 137  
135 138 target_arc = middle_arc_point + final_ext_arc_point
136 139  
137   - return init_ext_point, target_arc
  140 + return init_move_out_point, target_arc
138 141  
139 142 def compute_head_move_threshold(self, current_ref):
140 143 self.coord_vel.append(current_ref)
... ... @@ -158,24 +161,23 @@ class TrackerProcessing:
158 161  
159 162 return False
160 163  
161   - def head_move_compensation(self, current_ref, m_change_robot_to_head):
162   - trans = tr.translation_matrix(current_ref[:3])
163   - a, b, g = np.radians(current_ref[3:6])
164   - rot = tr.euler_matrix(a, b, g, 'rzyx')
165   - M_current_ref = tr.concatenate_matrices(trans, rot)
166   -
167   - m_robot_new = M_current_ref @ m_change_robot_to_head
168   - _, _, angles, translate, _ = tr.decompose_matrix(m_robot_new)
169   - angles = np.degrees(angles)
170   -
171   - return m_robot_new[0, -1], m_robot_new[1, -1], m_robot_new[2, -1], angles[0], angles[1], angles[2]
172   -
173   - def estimate_head_center(self, tracker, current_ref):
174   - m_probe_ref_left, m_probe_ref_right, m_probe_ref_nasion = tracker.GetMatrixTrackerFiducials()
175   - m_current_ref = dcr.compute_marker_transformation(np.array([current_ref]), 0)
176   -
177   - m_ear_left_new = m_current_ref @ m_probe_ref_left
178   - m_ear_right_new = m_current_ref @ m_probe_ref_right
  164 + def compute_head_move_compensation(self, current_head, M_change_robot_to_head):
  165 + M_current_head = dco.coordinates_to_transformation_matrix(
  166 + position=current_head[:3],
  167 + orientation=current_head[3:6],
  168 + axes='rzyx',
  169 + )
  170 + M_robot_new = M_current_head @ M_change_robot_to_head
  171 + angles_as_deg, translate = dco.transformation_matrix_to_coordinates(M_robot_new, axes='rzyx')
  172 + #TODO: check this with robot
  173 + return list(translate) + list(angles_as_deg)
  174 +
  175 + def estimate_head_center(self, tracker, current_head):
  176 + m_probe_head_left, m_probe_head_right, m_probe_head_nasion = tracker.GetMatrixTrackerFiducials()
  177 + m_current_head = dcr.compute_marker_transformation(np.array([current_head]), 0)
  178 +
  179 + m_ear_left_new = m_current_head @ m_probe_head_left
  180 + m_ear_right_new = m_current_head @ m_probe_head_right
179 181  
180 182 return (m_ear_left_new[:3, -1] + m_ear_right_new[:3, -1])/2
181 183  
... ...
invesalius/gui/dialogs.py
  Diff comments   View file @c21b81c
... ... @@ -4324,6 +4324,10 @@ class SetOptitrackconfigs(wx.Dialog):
4324 4324 return fn_cal, fn_userprofile
4325 4325  
4326 4326 class SetTrackerDeviceToRobot(wx.Dialog):
  4327 + """
  4328 + Robot navigation requires a tracker device to tracker the head position and the object (coil) position.
  4329 + A dialog pops up showing a combobox with all trackers but debugs and the robot itself (const.TRACKERS[:-3])
  4330 + """
4327 4331 def __init__(self, title=_("Setting tracker device:")):
4328 4332 wx.Dialog.__init__(self, wx.GetApp().GetTopWindow(), -1, title, size=wx.Size(1000, 200),
4329 4333 style=wx.DEFAULT_DIALOG_STYLE|wx.FRAME_FLOAT_ON_PARENT|wx.STAY_ON_TOP|wx.RESIZE_BORDER)
... ... @@ -4339,7 +4343,6 @@ class SetTrackerDeviceToRobot(wx.Dialog):
4339 4343 choice_trck.SetToolTip(tooltip)
4340 4344 choice_trck.SetSelection(const.DEFAULT_TRACKER)
4341 4345 choice_trck.Bind(wx.EVT_COMBOBOX, partial(self.OnChoiceTracker, ctrl=choice_trck))
4342   - #self.choice_trck = choice_trck
4343 4346  
4344 4347 btn_ok = wx.Button(self, wx.ID_OK)
4345 4348 btn_ok.SetHelpText("")
... ...
invesalius/navigation/robot.py
  Diff comments   View file @c21b81c
... ... @@ -154,18 +154,18 @@ class ControlRobot(threading.Thread):
154 154  
155 155 return coord_raw, coord_robot_raw, markers_flag
156 156  
157   - def robot_move_decision(self, distance_target, coord_inv, coord_robot_raw, current_ref_filtered):
  157 + def robot_move_decision(self, distance_target, new_robot_coordinates, current_robot_coordinates, current_head_filtered):
158 158 if distance_target < const.ROBOT_ARC_THRESHOLD_DISTANCE and not self.arc_motion_flag:
159   - self.trck_init_robot.SendCoordinates(coord_inv, const.ROBOT_MOTIONS["normal"])
  159 + self.trck_init_robot.SendCoordinates(new_robot_coordinates, const.ROBOT_MOTIONS["normal"])
160 160  
161 161 elif distance_target >= const.ROBOT_ARC_THRESHOLD_DISTANCE or self.arc_motion_flag:
162   - actual_point = coord_robot_raw
  162 + actual_point = current_robot_coordinates
163 163 if not self.arc_motion_flag:
164   - coord_head = self.process_tracker.estimate_head_center(self.tracker,
165   - current_ref_filtered).tolist()
  164 + head_center_coordinates = self.process_tracker.estimate_head_center(self.tracker,
  165 + current_head_filtered).tolist()
166 166  
167   - self.target_linear_out, self.target_arc = self.process_tracker.compute_arc_motion(coord_robot_raw, coord_head,
168   - coord_inv)
  167 + self.target_linear_out, self.target_arc = self.process_tracker.compute_arc_motion(current_robot_coordinates, head_center_coordinates,
  168 + new_robot_coordinates)
169 169 self.arc_motion_flag = True
170 170 self.arc_motion_step_flag = const.ROBOT_MOTIONS["linear out"]
171 171  
... ... @@ -176,15 +176,15 @@ class ControlRobot(threading.Thread):
176 176 coord = self.target_arc
177 177  
178 178 elif self.arc_motion_flag and self.arc_motion_step_flag == const.ROBOT_MOTIONS["arc"]:
179   - coord_head = self.process_tracker.estimate_head_center(self.tracker,
180   - current_ref_filtered).tolist()
  179 + head_center_coordinates = self.process_tracker.estimate_head_center(self.tracker,
  180 + current_head_filtered).tolist()
181 181  
182   - _, new_target_arc = self.process_tracker.compute_arc_motion(coord_robot_raw, coord_head,
183   - coord_inv)
  182 + _, new_target_arc = self.process_tracker.compute_arc_motion(current_robot_coordinates, head_center_coordinates,
  183 + new_robot_coordinates)
184 184 if np.allclose(np.array(new_target_arc[3:-1]), np.array(self.target_arc[3:-1]), 0, 1):
185 185 None
186 186 else:
187   - if self.process_tracker.correction_distance_calculation_target(coord_inv, coord_robot_raw) >= \
  187 + if self.process_tracker.correction_distance_calculation_target(new_robot_coordinates, current_robot_coordinates) >= \
188 188 const.ROBOT_ARC_THRESHOLD_DISTANCE*0.8:
189 189 self.target_arc = new_target_arc
190 190  
... ... @@ -193,40 +193,42 @@ class ControlRobot(threading.Thread):
193 193 if np.allclose(np.array(actual_point), np.array(self.target_arc[3:-1]), 0, 10):
194 194 self.arc_motion_flag = False
195 195 self.arc_motion_step_flag = const.ROBOT_MOTIONS["normal"]
196   - coord = coord_inv
  196 + coord = new_robot_coordinates
197 197  
198 198 self.trck_init_robot.SendCoordinates(coord, self.arc_motion_step_flag)
199 199  
200   - def robot_control(self, coords_tracker_in_robot, coord_robot_raw, markers_flag):
201   - coord_ref_tracker_in_robot = coords_tracker_in_robot[1]
202   - coord_obj_tracker_in_robot = coords_tracker_in_robot[2]
  200 + def robot_control(self, current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag):
  201 + coord_head_tracker_in_robot = current_tracker_coordinates_in_robot[1]
  202 + marker_head_flag = markers_flag[1]
  203 + coord_obj_tracker_in_robot = current_tracker_coordinates_in_robot[2]
  204 + marker_obj_flag = markers_flag[2]
203 205  
204 206 if self.robot_tracker_flag:
205   - current_ref = coord_ref_tracker_in_robot
206   - if current_ref is not None and markers_flag[1]:
207   - current_ref_filtered = self.process_tracker.kalman_filter(current_ref)
208   - if self.process_tracker.compute_head_move_threshold(current_ref_filtered):
209   - coord_inv = self.process_tracker.head_move_compensation(current_ref_filtered,
210   - self.m_change_robot_to_head)
  207 + current_head = coord_head_tracker_in_robot
  208 + if current_head is not None and marker_head_flag:
  209 + current_head_filtered = self.process_tracker.kalman_filter(current_head)
  210 + if self.process_tracker.compute_head_move_threshold(current_head_filtered):
  211 + new_robot_coordinates = self.process_tracker.compute_head_move_compensation(current_head_filtered,
  212 + self.m_change_robot_to_head)
211 213 if self.coord_inv_old is None:
212   - self.coord_inv_old = coord_inv
  214 + self.coord_inv_old = new_robot_coordinates
213 215  
214   - if np.allclose(np.array(coord_inv), np.array(coord_robot_raw), 0, 0.01):
  216 + if np.allclose(np.array(new_robot_coordinates), np.array(current_robot_coordinates), 0, 0.01):
215 217 # print("At target within range 1")
216 218 pass
217   - elif not np.allclose(np.array(coord_inv), np.array(self.coord_inv_old), 0, 5):
  219 + elif not np.allclose(np.array(new_robot_coordinates), np.array(self.coord_inv_old), 0, 5):
218 220 self.trck_init_robot.StopRobot()
219   - self.coord_inv_old = coord_inv
  221 + self.coord_inv_old = new_robot_coordinates
220 222 else:
221   - distance_target = self.process_tracker.correction_distance_calculation_target(coord_inv, coord_robot_raw)
222   - self.robot_move_decision(distance_target, coord_inv, coord_robot_raw, current_ref_filtered)
223   - self.coord_inv_old = coord_inv
  223 + distance_target = self.process_tracker.correction_distance_calculation_target(new_robot_coordinates, current_robot_coordinates)
  224 + self.robot_move_decision(distance_target, new_robot_coordinates, current_robot_coordinates, current_head_filtered)
  225 + self.coord_inv_old = new_robot_coordinates
224 226 else:
225 227 self.trck_init_robot.StopRobot()
226 228  
227 229 def run(self):
228 230 while not self.event_robot.is_set():
229   - coords_tracker_in_robot, coord_robot_raw, markers_flag = self.get_coordinates_from_tracker_devices()
  231 + current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag = self.get_coordinates_from_tracker_devices()
230 232  
231 233 if self.robot_target_queue.empty():
232 234 None
... ... @@ -240,4 +242,4 @@ class ControlRobot(threading.Thread):
240 242 self.target_flag = self.object_at_target_queue.get_nowait()
241 243 self.object_at_target_queue.task_done()
242 244  
243   - self.robot_control(coords_tracker_in_robot, coord_robot_raw, markers_flag)
  245 + self.robot_control(current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag)
... ...
optional-requirements.txt
  Diff comments   View file @c21b81c
... ... @@ -5,3 +5,4 @@ python-rtmidi==1.4.9
5 5 python-socketio[client]==5.3.0
6 6 requests==2.26.0
7 7 uvicorn[standard]==0.15.0
  8 +opencv-python==4.5.3
... ...