def compute_marker_transformation(coord_raw, obj_ref_mode):
m_probe = dco.coordinates_to_transformation_matrix(
position=coord_raw[obj_ref_mode, :3],
orientation=coord_raw[obj_ref_mode, 3:],
axes='rzyx',
)
return m_probe
def image_to_tracker(m_change, target, icp):
"""Compute the transformation matrix to the tracker coordinate system
:param m_change: Corregistration transformation obtained from fiducials
:type m_change: numpy.ndarray
:param target: Target in invesalius coordinate system
:type target: numpy.ndarray
:param icp: ICP transformation matrix
:type icp: numpy.ndarray
:return: 4 x 4 numpy double array
:rtype: numpy.ndarray
"""
m_target_in_image = dco.coordinates_to_transformation_matrix(
position=target[:3],
orientation=[0, 0, 0],
axes='sxyz',
)
if icp.use_icp:
m_target_in_image = bases.inverse_transform_icp(m_target_in_image, icp.m_icp)
m_target_in_tracker = np.linalg.inv(m_change) @ m_target_in_image
# invert y coordinate
m_target_in_tracker[2, -1] = -m_target_in_tracker[2, -1]
return m_target_in_tracker
def compute_robot_to_head_matrix(raw_target_robot):
"""
:param head: nx6 array of head coordinates from tracking device in robot space
:param robot: nx6 array of robot coordinates
:return: target_robot_matrix: 3x3 array representing change of basis from robot to head in robot system
"""
head, robot = raw_target_robot
# compute head target matrix
m_head_target = dco.coordinates_to_transformation_matrix(
position=head[:3],
orientation=head[3:],
axes='rzyx',
)
# compute robot target matrix
m_robot_target = dco.coordinates_to_transformation_matrix(
position=robot[:3],
orientation=robot[3:],
axes='rzyx',
)
robot_to_head_matrix = np.linalg.inv(m_head_target) @ m_robot_target
return robot_to_head_matrix
def compute_head_move_compensation(self, current_head, m_change_robot_to_head):
"""
Estimates the new robot position to reach the target
"""
M_current_head = dco.coordinates_to_transformation_matrix(
position=current_head[:3],
orientation=current_head[3:6],
axes='rzyx',
)
m_robot_new = M_current_head @ m_change_robot_to_head
translation, angles_as_deg = dco.transformation_matrix_to_coordinates(m_robot_new, axes='sxyz')
new_robot_position = list(translation) + list(angles_as_deg)
return new_robot_position
def transformation_tracker_to_robot(self, tracker_coord):
if not transform_tracker_to_robot.M_tracker_to_robot.any():
return None
M_tracker = dco.coordinates_to_transformation_matrix(
position=tracker_coord[:3],
orientation=tracker_coord[3:6],
axes='rzyx',
)
M_tracker_in_robot = transform_tracker_to_robot.M_tracker_to_robot @ M_tracker
translation, angles_as_deg = dco.transformation_matrix_to_coordinates(
M_tracker_in_robot, axes='rzyx')
tracker_in_robot = list(translation) + list(angles_as_deg)
return tracker_in_robot
def object_to_reference(coord_raw, m_probe):
"""Compute affine transformation matrix to the reference basis
:param coord_raw: Coordinates returned by the tracking device
:type coord_raw: numpy.ndarray
:param m_probe: Probe coordinates
:type m_probe: numpy.ndarray
:return: 4 x 4 numpy double array
:rtype: numpy.ndarray
"""
m_ref = dco.coordinates_to_transformation_matrix(
position=coord_raw[1, :3],
orientation=coord_raw[1, 3:],
axes='rzyx',
)
m_dyn = np.linalg.inv(m_ref) @ m_probe
return m_dyn
def convert_invesalius_to_world(position, orientation):
"""
Convert position and orientation from InVesalius space to the world space.
The axis definition for the Euler angles returned is 'sxyz', see transformations.py for more
information.
Uses 'affine' matrix defined in the project created or opened by the user. If it is
undefined, return Nones as the coordinates for both position and orientation.
More information: https://nipy.org/nibabel/coordinate_systems.html
:param position: a vector of 3 coordinates in InVesalius space.
:param orientation: a vector of 3 Euler angles in InVesalius space.
:return: a pair consisting of 3 coordinates and 3 Euler angles in the world space, or Nones if
'affine' matrix is not defined in the project.
"""
slice = sl.Slice()
if slice.affine is None:
position_world = (None, None, None)
orientation_world = (None, None, None)
return position_world, orientation_world
position_voxel = convert_invesalius_to_voxel(position)
M_invesalius = dco.coordinates_to_transformation_matrix(
position=position_voxel,
orientation=orientation,
axes='sxyz',
)
M_world = np.linalg.inv(slice.affine) @ M_invesalius
position_world, orientation_world = dco.transformation_matrix_to_coordinates(
M_world,
axes='sxyz',
)
return position_world, orientation_world
def object_registration(fiducials, orients, coord_raw, m_change):
"""
:param fiducials: 3x3 array of fiducials translations
:param orients: 3x3 array of fiducials orientations in degrees
:param coord_raw: nx6 array of coordinates from tracking device where n = 1 is the reference attached to the head
:param m_change: 3x3 array representing change of basis from head in tracking system to vtk head system
:return:
"""
coords_aux = np.hstack((fiducials, orients))
mask = np.ones(len(coords_aux), dtype=bool)
mask[[3]] = False
coords = coords_aux[mask]
fids_dyn = np.zeros([4, 6])
fids_img = np.zeros([4, 6])
fids_raw = np.zeros([3, 3])
# compute fiducials of object with reference to the fixed probe in source frame
for ic in range(0, 3):
fids_raw[ic, :] = dco.dynamic_reference_m2(coords[ic, :],
coords[3, :])[:3]
# compute initial alignment of probe fixed in the object in source frame
# XXX: Some duplicate processing is done here: the Euler angles are calculated once by
# the lines below, and then again in dco.coordinates_to_transformation_matrix.
#
a, b, g = np.radians(coords[3, 3:])
r_s0_raw = tr.euler_matrix(a, b, g, axes='rzyx')
s0_raw = dco.coordinates_to_transformation_matrix(
position=coords[3, :3],
orientation=coords[3, 3:],
axes='rzyx',
)
# compute change of basis for object fiducials in source frame
base_obj_raw, q_obj_raw = base_creation(fids_raw[:3, :3])
r_obj_raw = np.identity(4)
r_obj_raw[:3, :3] = base_obj_raw[:3, :3]
t_obj_raw = tr.translation_matrix(q_obj_raw)
m_obj_raw = tr.concatenate_matrices(t_obj_raw, r_obj_raw)
for ic in range(0, 4):
if coord_raw.any():
# compute object fiducials in reference frame
fids_dyn[ic, :] = dco.dynamic_reference_m2(coords[ic, :],
coord_raw[1, :])
else:
# compute object fiducials in source frame
fids_dyn[ic, :] = coords[ic, :]
fids_dyn[ic, 2] = -fids_dyn[ic, 2]
# compute object fiducials in vtk head frame
M_p = dco.coordinates_to_transformation_matrix(
position=fids_dyn[ic, :3],
orientation=fids_dyn[ic, 3:],
axes='rzyx',
)
M_img = m_change @ M_p
angles_img = np.degrees(np.asarray(tr.euler_from_matrix(M_img,
'rzyx')))
coord_img = list(M_img[:3, -1])
coord_img[1] = -coord_img[1]
fids_img[ic, :] = np.hstack((coord_img, angles_img))
# compute object base change in vtk head frame
base_obj_img, _ = base_creation(fids_img[:3, :3])
r_obj_img = np.identity(4)
r_obj_img[:3, :3] = base_obj_img[:3, :3]
# compute initial alignment of probe fixed in the object in reference (or static) frame
s0_dyn = dco.coordinates_to_transformation_matrix(
position=fids_dyn[3, :3],
orientation=fids_dyn[3, 3:],
axes='rzyx',
)
return t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img
