Download as
Browse Code »

Commit 430847f0fc632ecae5c3c9ab13be35f36b78b0b2

Authored by Renan
1 parent 8c526577
Exists in master

MOD: separated robot target from markers

Showing 2 changed files with 54 additions and 57 deletions   Show diff stats
invesalius/data/bases.py
  Diff comments   View file @430847f
... ... @@ -245,6 +245,31 @@ def object_registration(fiducials, orients, coord_raw, m_change):
245 245  
246 246 return t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img
247 247  
  248 +def compute_robot_target_matrix(head, robot):
  249 + """
  250 + :param head: nx6 array of head coordinates from tracking device in robot space
  251 + :param robot: nx6 array of robot coordinates
  252 +
  253 + :return: target_robot_matrix: 3x3 array representing change of basis from robot to head in robot system
  254 + """
  255 + # compute head target matrix
  256 + m_head_target = dco.coordinates_to_transformation_matrix(
  257 + position=head[:3],
  258 + orientation=head[3:],
  259 + axes='rzyx',
  260 + )
  261 +
  262 + # compute robot target matrix
  263 + m_robot_target = dco.coordinates_to_transformation_matrix(
  264 + position=robot[:3],
  265 + orientation=robot[3:],
  266 + axes='rzyx',
  267 + )
  268 + target_robot_matrix = np.linalg.inv(m_head_target) @ m_robot_target
  269 +
  270 + return target_robot_matrix
  271 +
  272 +
248 273 class transform_tracker_to_robot(object):
249 274 M_tracker_to_robot = np.array([])
250 275 def transformation_tracker_to_robot(self, tracker_coord):
... ...
invesalius/gui/task_navigator.py
  Diff comments   View file @430847f
... ... @@ -63,13 +63,13 @@ import invesalius.data.record_coords as rec
63 63 import invesalius.data.vtk_utils as vtk_utils
64 64 import invesalius.gui.dialogs as dlg
65 65 import invesalius.project as prj
  66 +import invesalius.data.bases as db
66 67 from invesalius import utils
67 68 from invesalius.gui import utils as gui_utils
68 69 from invesalius.navigation.icp import ICP
69 70 from invesalius.navigation.navigation import Navigation
70 71 from invesalius.navigation.tracker import Tracker
71 72 from invesalius.navigation.robot import Robot
72   -import invesalius.data.transformations as tr
73 73  
74 74 HAS_PEDAL_CONNECTION = True
75 75 try:
... ... @@ -690,11 +690,9 @@ class NeuronavigationPanel(wx.Panel):
690 690 colour = (0., 1., 0.)
691 691 size = 2
692 692 seed = 3 * [0.]
693   - head = 6 * [0.]
694   - robot = 6 * [0.]
695 693  
696   - Publisher.sendMessage('Create marker', coord=coord, colour=colour, size=size,
697   - label=label, seed=seed, head=head, robot=robot)
  694 + Publisher.sendMessage('Create marker', coord=coord, colour=colour, size=size, label=label, seed=seed)
  695 +
698 696 else:
699 697 for m in [0, 1, 2]:
700 698 self.numctrls_fiducial[n][m].SetValue(float(self.current_coord[m]))
... ... @@ -1136,18 +1134,6 @@ class MarkersPanel(wx.Panel):
1136 1134 x_seed : float = 0
1137 1135 y_seed : float = 0
1138 1136 z_seed : float = 0
1139   - x_head : float = 0
1140   - y_head : float = 0
1141   - z_head : float = 0
1142   - alpha_head : float = 0
1143   - beta_head : float = 0
1144   - gamma_head : float = 0
1145   - x_robot : float = 0
1146   - y_robot : float = 0
1147   - z_robot : float = 0
1148   - alpha_robot : float = 0
1149   - beta_robot: float = 0
1150   - gamma_robot : float = 0
1151 1137 is_target : bool = False
1152 1138 session_id : int = 1
1153 1139  
... ... @@ -1178,24 +1164,6 @@ class MarkersPanel(wx.Panel):
1178 1164 def seed(self, new_seed):
1179 1165 self.x_seed, self.y_seed, self.z_seed = new_seed
1180 1166  
1181   - # x_head, y_head, z_head, alpha_head, beta_head, gamma_head can be jointly accessed as robot
1182   - @property
1183   - def head(self):
1184   - return list((self.x_head, self.y_head, self.z_head, self.alpha_head, self.beta_head, self.gamma_head),)
1185   -
1186   - @head.setter
1187   - def head(self, new_head):
1188   - self.x_head, self.y_head, self.z_head, self.alpha_head, self.beta_head, self.gamma_head = new_head
1189   -
1190   - # x_robot, y_robot, z_robot, alpha_robot, beta_robot, gamma_robot can be jointly accessed as robot
1191   - @property
1192   - def robot(self):
1193   - return list((self.x_robot, self.y_robot, self.z_robot, self.alpha_robot, self.beta_robot, self.gamma_robot),)
1194   -
1195   - @robot.setter
1196   - def robot(self, new_robot):
1197   - self.x_robot, self.y_robot, self.z_robot, self.alpha_robot, self.beta_robot, self.gamma_robot = new_robot
1198   -
1199 1167 @classmethod
1200 1168 def to_string_headers(cls):
1201 1169 """Return the string containing tab-separated list of field names (headers)."""
... ... @@ -1235,6 +1203,19 @@ class MarkersPanel(wx.Panel):
1235 1203 if field.type is bool:
1236 1204 setattr(self, field.name, str_val=='True')
1237 1205  
  1206 + @dataclasses.dataclass
  1207 + class Robot_Marker:
  1208 + """Class for storing robot target."""
  1209 + m_robot_target : list = None
  1210 +
  1211 + @property
  1212 + def robot_target_matrix(self):
  1213 + return self.m_robot_target
  1214 +
  1215 + @robot_target_matrix.setter
  1216 + def robot_target_matrix(self, new_m_robot_target):
  1217 + self.m_robot_target = new_m_robot_target
  1218 +
1238 1219 def __init__(self, parent, tracker):
1239 1220 wx.Panel.__init__(self, parent)
1240 1221 try:
... ... @@ -1252,9 +1233,9 @@ class MarkersPanel(wx.Panel):
1252 1233 self.current_coord = 0, 0, 0, 0, 0, 0
1253 1234 self.current_angle = 0, 0, 0
1254 1235 self.current_seed = 0, 0, 0
  1236 + self.current_robot_target_matrix = [None] * 9
1255 1237 self.markers = []
1256   - self.current_head = 0, 0, 0, 0, 0, 0
1257   - self.current_robot = 0, 0, 0, 0, 0, 0
  1238 + self.robot_markers = []
1258 1239 self.nav_status = False
1259 1240  
1260 1241 self.marker_colour = const.MARKER_COLOUR
... ... @@ -1379,6 +1360,7 @@ class MarkersPanel(wx.Panel):
1379 1360 """
1380 1361 for i in reversed(index):
1381 1362 del self.markers[i]
  1363 + del self.robot_markers[i]
1382 1364 self.lc.DeleteItem(i)
1383 1365 for n in range(0, self.lc.GetItemCount()):
1384 1366 self.lc.SetItem(n, 0, str(n+1))
... ... @@ -1430,8 +1412,9 @@ class MarkersPanel(wx.Panel):
1430 1412 self.current_seed = coord_offset
1431 1413  
1432 1414 def UpdateRobotCoordinates(self, coordinates_raw, markers_flag):
1433   - self.current_head = coordinates_raw[1]
1434   - self.current_robot = coordinates_raw[2]
  1415 + head = coordinates_raw[1]
  1416 + robot = coordinates_raw[2]
  1417 + self.current_robot_target_matrix = db.compute_robot_target_matrix(head, robot)
1435 1418  
1436 1419 def OnMouseRightDown(self, evt):
1437 1420 # TODO: Enable the "Set as target" only when target is created with registered object
... ... @@ -1504,22 +1487,9 @@ class MarkersPanel(wx.Panel):
1504 1487 if isinstance(evt, int):
1505 1488 self.lc.Focus(evt)
1506 1489  
1507   - robot = self.markers[self.lc.GetFocusedItem()].robot
1508   - head = self.markers[self.lc.GetFocusedItem()].head
1509   -
1510   - trans = tr.translation_matrix(robot[:3])
1511   - a, b, g = np.radians(robot[3:])
1512   - rot = tr.euler_matrix(a, b, g, 'rzyx')
1513   - m_robot_target = tr.concatenate_matrices(trans, rot)
1514   -
1515   - trans = tr.translation_matrix(head[:3])
1516   - a, b, g = np.radians(head[3:])
1517   - rot = tr.euler_matrix(a, b, g, 'rzyx')
1518   - m_ref_target = tr.concatenate_matrices(trans, rot)
1519   -
1520   - m_change_robot_to_head = np.linalg.inv(m_ref_target) @ m_robot_target
  1490 + m_target_robot = self.robot_markers[self.lc.GetFocusedItem()].robot_target_matrix
1521 1491  
1522   - Publisher.sendMessage('Robot target matrix', robot_tracker_flag=True, m_change_robot_to_head=m_change_robot_to_head)
  1492 + Publisher.sendMessage('Robot target matrix', robot_tracker_flag=True, m_change_robot_to_head=m_target_robot)
1523 1493  
1524 1494 def OnDeleteAllMarkers(self, evt=None):
1525 1495 if evt is None:
... ... @@ -1536,6 +1506,7 @@ class MarkersPanel(wx.Panel):
1536 1506 wx.MessageBox(_("Target deleted."), _("InVesalius 3"))
1537 1507  
1538 1508 self.markers = []
  1509 + self.robot_markers = []
1539 1510 Publisher.sendMessage('Remove all markers', indexes=self.lc.GetItemCount())
1540 1511 self.lc.DeleteAllItems()
1541 1512 Publisher.sendMessage('Stop Blink Marker', index='DeleteAll')
... ... @@ -1656,7 +1627,7 @@ class MarkersPanel(wx.Panel):
1656 1627 def OnChangeCurrentSession(self, new_session_id):
1657 1628 self.current_session = new_session_id
1658 1629  
1659   - def CreateMarker(self, coord=None, colour=None, size=None, label='*', is_target=False, seed=None, head=None, robot=None, session_id=None):
  1630 + def CreateMarker(self, coord=None, colour=None, size=None, label='*', is_target=False, seed=None, session_id=None):
1660 1631 new_marker = self.Marker()
1661 1632 new_marker.coord = coord or self.current_coord
1662 1633 new_marker.colour = colour or self.marker_colour
... ... @@ -1664,9 +1635,9 @@ class MarkersPanel(wx.Panel):
1664 1635 new_marker.label = label
1665 1636 new_marker.is_target = is_target
1666 1637 new_marker.seed = seed or self.current_seed
1667   - new_marker.head = head or self.current_head
1668   - new_marker.robot = robot or self.current_robot
1669 1638 new_marker.session_id = session_id or self.current_session
  1639 + new_robot_marker = self.Robot_Marker()
  1640 + new_robot_marker.robot_target_matrix = self.current_robot_target_matrix
1670 1641  
1671 1642 # Note that ball_id is zero-based, so we assign it len(self.markers) before the new marker is added
1672 1643 Publisher.sendMessage('Add marker', ball_id=len(self.markers),
... ... @@ -1674,6 +1645,7 @@ class MarkersPanel(wx.Panel):
1674 1645 colour=new_marker.colour,
1675 1646 coord=new_marker.coord[:3])
1676 1647 self.markers.append(new_marker)
  1648 + self.robot_markers.append(new_robot_marker)
1677 1649  
1678 1650 # Add item to list control in panel
1679 1651 num_items = self.lc.GetItemCount()
... ...