def dynamic_reference_m2(probe, reference): """ Apply dynamic reference correction to probe coordinates. Uses the alpha, beta and gama rotation angles of reference to rotate the probe coordinate and returns the x, y, z difference between probe and reference. Angles sequences and equation was extracted from Polhemus manual and Attitude matrix in Wikipedia. General equation is: coord = Mrot * (probe - reference) :param probe: sensor one defined as probe :param reference: sensor two defined as reference :return: rotated and translated coordinates """ a, b, g = np.radians(reference[3:6]) a_p, b_p, g_p = np.radians(probe[3:6]) T = tr.translation_matrix(reference[:3]) T_p = tr.translation_matrix(probe[:3]) R = tr.euler_matrix(a, b, g, 'rzyx') R_p = tr.euler_matrix(a_p, b_p, g_p, 'rzyx') M = tr.concatenate_matrices(T, R) M_p = tr.concatenate_matrices(T_p, R_p) M_dyn = np.linalg.inv(M) @ M_p al, be, ga = tr.euler_from_matrix(M_dyn, 'rzyx') coord_rot = tr.translation_from_matrix(M_dyn) coord_rot = np.squeeze(coord_rot) return coord_rot[0], coord_rot[1], coord_rot[2], np.degrees( al), np.degrees(be), np.degrees(ga)
def run(self): m_change, obj_ref_mode = self.coreg_data trck_init, trck_id, trck_mode = self.trck_info while self.nav_id: coord_raw = dco.GetCoordinates(trck_init, trck_id, trck_mode) psi, theta, phi = radians(coord_raw[obj_ref_mode, 3:]) r_probe = tr.euler_matrix(psi, theta, phi, 'rzyx') t_probe = tr.translation_matrix(coord_raw[obj_ref_mode, :3]) m_probe = asmatrix(tr.concatenate_matrices(t_probe, r_probe)) psi_ref, theta_ref, phi_ref = radians(coord_raw[1, 3:]) r_ref = tr.euler_matrix(psi_ref, theta_ref, phi_ref, 'rzyx') t_ref = tr.translation_matrix(coord_raw[1, :3]) m_ref = asmatrix(tr.concatenate_matrices(t_ref, r_ref)) m_dyn = m_ref.I * m_probe m_dyn[2, -1] = -m_dyn[2, -1] m_img = m_change * m_dyn scale, shear, angles, trans, persp = tr.decompose_matrix(m_img) coord = m_img[0, -1], m_img[1, -1], m_img[2, -1], \ degrees(angles[0]), degrees(angles[1]), degrees(angles[2]) wx.CallAfter(Publisher.sendMessage, 'Co-registered points', (m_img, coord)) # TODO: Optimize the value of sleep for each tracking device. sleep(0.175) if self._pause_: return
def run(self): m_change, obj_ref_mode = self.coreg_data trck_init, trck_id, trck_mode = self.trck_info while self.nav_id: coord_raw = dco.GetCoordinates(trck_init, trck_id, trck_mode) psi, theta, phi = radians(coord_raw[obj_ref_mode, 3:]) r_probe = tr.euler_matrix(psi, theta, phi, 'rzyx') t_probe = tr.translation_matrix(coord_raw[obj_ref_mode, :3]) m_probe = asmatrix(tr.concatenate_matrices(t_probe, r_probe)) psi_ref, theta_ref, phi_ref = radians(coord_raw[1, 3:]) r_ref = tr.euler_matrix(psi_ref, theta_ref, phi_ref, 'rzyx') t_ref = tr.translation_matrix(coord_raw[1, :3]) m_ref = asmatrix(tr.concatenate_matrices(t_ref, r_ref)) m_dyn = m_ref.I * m_probe m_dyn[2, -1] = -m_dyn[2, -1] m_img = m_change * m_dyn scale, shear, angles, trans, persp = tr.decompose_matrix(m_img) coord = m_img[0, -1], m_img[1, -1], m_img[2, -1], \ degrees(angles[0]), degrees(angles[1]), degrees(angles[2]) wx.CallAfter(Publisher.sendMessage, 'Co-registered points', arg=m_img, position=coord) # TODO: Optimize the value of sleep for each tracking device. sleep(0.175) if self._pause_: return
def dynamic_reference_m(probe, reference): """ Apply dynamic reference correction to probe coordinates. Uses the alpha, beta and gama rotation angles of reference to rotate the probe coordinate and returns the x, y, z difference between probe and reference. Angles sequences and equation was extracted from Polhemus manual and Attitude matrix in Wikipedia. General equation is: coord = Mrot * (probe - reference) :param probe: sensor one defined as probe :param reference: sensor two defined as reference :return: rotated and translated coordinates """ a, b, g = np.radians(reference[3:6]) trans = tr.translation_matrix(reference[:3]) rot = tr.euler_matrix(a, b, g, 'rzyx') affine = tr.concatenate_matrices(trans, rot) probe_4 = np.vstack((probe[:3].reshape([3, 1]), 1.)) coord_rot = np.linalg.inv(affine) @ probe_4 # minus sign to the z coordinate coord_rot[2, 0] = -coord_rot[2, 0] coord_rot = coord_rot[:3, 0].tolist() coord_rot.extend(probe[3:]) return coord_rot
def dynamic_reference_m(probe, reference): """ Apply dynamic reference correction to probe coordinates. Uses the alpha, beta and gama rotation angles of reference to rotate the probe coordinate and returns the x, y, z difference between probe and reference. Angles sequences and equation was extracted from Polhemus manual and Attitude matrix in Wikipedia. General equation is: coord = Mrot * (probe - reference) :param probe: sensor one defined as probe :param reference: sensor two defined as reference :return: rotated and translated coordinates """ a, b, g = np.radians(reference[3:6]) T = tr.translation_matrix(reference[:3]) R = tr.euler_matrix(a, b, g, 'rzyx') M = np.asmatrix(tr.concatenate_matrices(T, R)) # M = tr.compose_matrix(angles=np.radians(reference[3:6]), translate=reference[:3]) # print M probe_4 = np.vstack((np.asmatrix(probe[:3]).reshape([3, 1]), 1.)) coord_rot = M.I * probe_4 coord_rot = np.squeeze(np.asarray(coord_rot)) return coord_rot[0], coord_rot[1], -coord_rot[2], probe[3], probe[ 4], probe[5]
def object_marker_to_center(coord_raw, obj_ref_mode, t_obj_raw, s0_raw, r_s0_raw): """Translate and rotate the raw coordinate given by the tracking device to the reference system created during the object registration. :param coord_raw: Coordinates returned by the tracking device :type coord_raw: numpy.ndarray :param obj_ref_mode: :type obj_ref_mode: int :param t_obj_raw: :type t_obj_raw: numpy.ndarray :param s0_raw: :type s0_raw: numpy.ndarray :param r_s0_raw: rotation transformation from marker to object basis :type r_s0_raw: numpy.ndarray :return: 4 x 4 numpy double array :rtype: numpy.ndarray """ as1, bs1, gs1 = np.radians(coord_raw[obj_ref_mode, 3:]) r_probe = tr.euler_matrix(as1, bs1, gs1, 'rzyx') t_probe_raw = tr.translation_matrix(coord_raw[obj_ref_mode, :3]) t_offset_aux = np.linalg.inv(r_s0_raw) @ r_probe @ t_obj_raw t_offset = np.identity(4) t_offset[:, -1] = t_offset_aux[:, -1] t_probe = s0_raw @ t_offset @ np.linalg.inv(s0_raw) @ t_probe_raw m_probe = tr.concatenate_matrices(t_probe, r_probe) return m_probe
def dynamic_reference_m2(probe, reference): """ Apply dynamic reference correction to probe coordinates. Uses the alpha, beta and gama rotation angles of reference to rotate the probe coordinate and returns the x, y, z difference between probe and reference. Angles sequences and equation was extracted from Polhemus manual and Attitude matrix in Wikipedia. General equation is: coord = Mrot * (probe - reference) :param probe: sensor one defined as probe :param reference: sensor two defined as reference :return: rotated and translated coordinates """ a, b, g = np.radians(reference[3:6]) a_p, b_p, g_p = np.radians(probe[3:6]) T = tr.translation_matrix(reference[:3]) T_p = tr.translation_matrix(probe[:3]) R = tr.euler_matrix(a, b, g, 'rzyx') R_p = tr.euler_matrix(a_p, b_p, g_p, 'rzyx') M = np.asmatrix(tr.concatenate_matrices(T, R)) M_p = np.asmatrix(tr.concatenate_matrices(T_p, R_p)) # M = tr.compose_matrix(angles=np.radians(reference[3:6]), translate=reference[:3]) # print M M_dyn = M.I * M_p al, be, ga = tr.euler_from_matrix(M_dyn, 'rzyx') coord_rot = tr.translation_from_matrix(M_dyn) coord_rot = np.squeeze(coord_rot) # probe_4 = np.vstack((np.asmatrix(probe[:3]).reshape([3, 1]), 1.)) # coord_rot_test = M.I * probe_4 # coord_rot_test = np.squeeze(np.asarray(coord_rot_test)) # # print "coord_rot: ", coord_rot # print "coord_rot_test: ", coord_rot_test # print "test: ", np.allclose(coord_rot, coord_rot_test[:3]) return coord_rot[0], coord_rot[1], coord_rot[2], np.degrees( al), np.degrees(be), np.degrees(ga)
def coordinates_to_transformation_matrix(position, orientation, axes='sxyz'): """ Transform vectors consisting of position and orientation (in Euler angles) in 3d-space into a 4x4 transformation matrix that combines the rotation and translation. :param position: A vector of three coordinates. :param orientation: A vector of three Euler angles in degrees. :param axes: The order in which the rotations are done for the axes. See transformations.py for details. Defaults to 'sxyz'. :return: The transformation matrix (4x4). """ a, b, g = np.radians(orientation) r_ref = tr.euler_matrix(a, b, g, axes=axes) t_ref = tr.translation_matrix(position) m_img = tr.concatenate_matrices(t_ref, r_ref) return m_img
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 """ a, b, g = np.radians(coord_raw[1, 3:]) r_ref = tr.euler_matrix(a, b, g, 'rzyx') t_ref = tr.translation_matrix(coord_raw[1, :3]) m_ref = tr.concatenate_matrices(t_ref, r_ref) m_dyn = np.linalg.inv(m_ref) @ m_probe return m_dyn
def flip_x_m(point): """ Rotate coordinates of a vector by pi around X axis in static reference frame. InVesalius also require to multiply the z coordinate by (-1). Possibly because the origin of coordinate system of imagedata is located in superior left corner and the origin of VTK scene coordinate system (polygonal surface) is in the interior left corner. Second possibility is the order of slice stacking :param point: list of coordinates x, y and z :return: rotated coordinates """ point_4 = np.hstack((point, 1.)).reshape([4, 1]) point_4[2, 0] = -point_4[2, 0] m_rot = np.asmatrix(tr.euler_matrix(pi, 0, 0)) point_rot = m_rot*point_4 return point_rot[0, 0], point_rot[1, 0], point_rot[2, 0]
def flip_x_m(point): """ Rotate coordinates of a vector by pi around X axis in static reference frame. InVesalius also require to multiply the z coordinate by (-1). Possibly because the origin of coordinate system of imagedata is located in superior left corner and the origin of VTK scene coordinate system (polygonal surface) is in the interior left corner. Second possibility is the order of slice stacking :param point: list of coordinates x, y and z :return: rotated coordinates """ point_4 = np.hstack((point, 1.)).reshape([4, 1]) point_4[2, 0] = -point_4[2, 0] m_rot = np.asmatrix(tr.euler_matrix(pi, 0, 0)) point_rot = m_rot * point_4 return point_rot[0, 0], point_rot[1, 0], point_rot[2, 0]
def run(self): m_change, obj_ref_mode, t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img = self.coreg_data trck_init, trck_id, trck_mode = self.trck_info while self.nav_id: coord_raw = dco.GetCoordinates(trck_init, trck_id, trck_mode) as1, bs1, gs1 = radians(coord_raw[obj_ref_mode, 3:]) r_probe = asmatrix(tr.euler_matrix(as1, bs1, gs1, 'rzyx')) t_probe_raw = asmatrix(tr.translation_matrix(coord_raw[obj_ref_mode, :3])) t_offset_aux = r_s0_raw.I * r_probe * t_obj_raw t_offset = asmatrix(identity(4)) t_offset[:, -1] = t_offset_aux[:, -1] t_probe = s0_raw * t_offset * s0_raw.I * t_probe_raw m_probe = asmatrix(tr.concatenate_matrices(t_probe, r_probe)) a, b, g = radians(coord_raw[1, 3:]) r_ref = tr.euler_matrix(a, b, g, 'rzyx') t_ref = tr.translation_matrix(coord_raw[1, :3]) m_ref = asmatrix(tr.concatenate_matrices(t_ref, r_ref)) m_dyn = m_ref.I * m_probe m_dyn[2, -1] = -m_dyn[2, -1] m_img = m_change * m_dyn r_obj = r_obj_img * m_obj_raw.I * s0_dyn.I * m_dyn * m_obj_raw m_img[:3, :3] = r_obj[:3, :3] scale, shear, angles, trans, persp = tr.decompose_matrix(m_img) coord = m_img[0, -1], m_img[1, -1], m_img[2, -1],\ degrees(angles[0]), degrees(angles[1]), degrees(angles[2]) wx.CallAfter(Publisher.sendMessage, 'Co-registered points', arg=m_img, position=coord) wx.CallAfter(Publisher.sendMessage, 'Update object matrix', m_img=m_img, coord=coord) # TODO: Optimize the value of sleep for each tracking device. sleep(0.175) # Debug tracker is not working with 0.175 so changed to 0.2 # However, 0.2 is too low update frequency ~5 Hz. Need optimization URGENTLY. # sleep(.3) # partially working for translate and offset, # but offset is kept always in same axis, have to fix for rotation # M_dyn = M_reference.I * T_stylus # M_dyn[2, -1] = -M_dyn[2, -1] # M_dyn_ch = M_change * M_dyn # ddd = M_dyn_ch[0, -1], M_dyn_ch[1, -1], M_dyn_ch[2, -1] # M_dyn_ch[:3, -1] = asmatrix(db.flip_x_m(ddd)).reshape([3, 1]) # M_final = S0 * M_obj_trans_0 * S0.I * M_dyn_ch # this works for static reference object rotation # R_dyn = M_vtk * M_obj_rot_raw.I * S0_rot_raw.I * R_stylus * M_obj_rot_raw # this works for dynamic reference in rotation but not in translation # R_dyn = M_vtk * M_obj_rot_raw.I * S0_rot_dyn.I * R_reference.I * R_stylus * M_obj_rot_raw if self._pause_: return
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
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 t_s0_raw = np.asmatrix(tr.translation_matrix(coords[3, :3])) r_s0_raw = np.asmatrix(tr.euler_matrix(np.radians(coords[3, 3]), np.radians(coords[3, 4]), np.radians(coords[3, 5]), 'rzyx')) s0_raw = np.asmatrix(tr.concatenate_matrices(t_s0_raw, r_s0_raw)) # compute change of basis for object fiducials in source frame base_obj_raw, q_obj_raw, base_inv_obj_raw = base_creation(fids_raw[:3, :3]) r_obj_raw = np.asmatrix(np.identity(4)) r_obj_raw[:3, :3] = base_obj_raw[:3, :3] t_obj_raw = np.asmatrix(tr.translation_matrix(q_obj_raw)) m_obj_raw = np.asmatrix(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, :]) fids_dyn[ic, 2] = -fids_dyn[ic, 2] else: # compute object fiducials in source frame fids_dyn[ic, :] = coords[ic, :] # compute object fiducials in vtk head frame a, b, g = np.radians(fids_dyn[ic, 3:]) T_p = tr.translation_matrix(fids_dyn[ic, :3]) R_p = tr.euler_matrix(a, b, g, 'rzyx') M_p = np.asmatrix(tr.concatenate_matrices(T_p, R_p)) M_img = np.asmatrix(m_change) * M_p angles_img = np.degrees(np.asarray(tr.euler_from_matrix(M_img, 'rzyx'))) coord_img = np.asarray(flip_x_m(tr.translation_from_matrix(M_img))) fids_img[ic, :] = np.hstack((coord_img, angles_img)) # compute object base change in vtk head frame base_obj_img, q_obj_img, base_inv_obj_img = base_creation(fids_img[:3, :3]) r_obj_img = np.asmatrix(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_trans_dyn = np.asmatrix(tr.translation_matrix(fids_dyn[3, :3])) s0_rot_dyn = np.asmatrix(tr.euler_matrix(np.radians(fids_dyn[3, 3]), np.radians(fids_dyn[3, 4]), np.radians(fids_dyn[3, 5]), 'rzyx')) s0_dyn = np.asmatrix(tr.concatenate_matrices(s0_trans_dyn, s0_rot_dyn)) return t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img
def run(self): m_change, obj_ref_mode, t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img = self.coreg_data trck_init, trck_id, trck_mode = self.trck_info while self.nav_id: coord_raw = dco.GetCoordinates(trck_init, trck_id, trck_mode) as1, bs1, gs1 = radians(coord_raw[obj_ref_mode, 3:]) r_probe = asmatrix(tr.euler_matrix(as1, bs1, gs1, 'rzyx')) t_probe_raw = asmatrix( tr.translation_matrix(coord_raw[obj_ref_mode, :3])) t_offset_aux = r_s0_raw.I * r_probe * t_obj_raw t_offset = asmatrix(identity(4)) t_offset[:, -1] = t_offset_aux[:, -1] t_probe = s0_raw * t_offset * s0_raw.I * t_probe_raw m_probe = asmatrix(tr.concatenate_matrices(t_probe, r_probe)) a, b, g = radians(coord_raw[1, 3:]) r_ref = tr.euler_matrix(a, b, g, 'rzyx') t_ref = tr.translation_matrix(coord_raw[1, :3]) m_ref = asmatrix(tr.concatenate_matrices(t_ref, r_ref)) m_dyn = m_ref.I * m_probe m_dyn[2, -1] = -m_dyn[2, -1] m_img = m_change * m_dyn r_obj = r_obj_img * m_obj_raw.I * s0_dyn.I * m_dyn * m_obj_raw m_img[:3, :3] = r_obj[:3, :3] scale, shear, angles, trans, persp = tr.decompose_matrix(m_img) coord = m_img[0, -1], m_img[1, -1], m_img[2, -1],\ degrees(angles[0]), degrees(angles[1]), degrees(angles[2]) wx.CallAfter(Publisher.sendMessage, 'Co-registered points', arg=m_img, position=coord) wx.CallAfter(Publisher.sendMessage, 'Update object matrix', m_img=m_img, coord=coord) # TODO: Optimize the value of sleep for each tracking device. sleep(0.175) # Debug tracker is not working with 0.175 so changed to 0.2 # However, 0.2 is too low update frequency ~5 Hz. Need optimization URGENTLY. # sleep(.3) # partially working for translate and offset, # but offset is kept always in same axis, have to fix for rotation # M_dyn = M_reference.I * T_stylus # M_dyn[2, -1] = -M_dyn[2, -1] # M_dyn_ch = M_change * M_dyn # ddd = M_dyn_ch[0, -1], M_dyn_ch[1, -1], M_dyn_ch[2, -1] # M_dyn_ch[:3, -1] = asmatrix(db.flip_x_m(ddd)).reshape([3, 1]) # M_final = S0 * M_obj_trans_0 * S0.I * M_dyn_ch # this works for static reference object rotation # R_dyn = M_vtk * M_obj_rot_raw.I * S0_rot_raw.I * R_stylus * M_obj_rot_raw # this works for dynamic reference in rotation but not in translation # R_dyn = M_vtk * M_obj_rot_raw.I * S0_rot_dyn.I * R_reference.I * R_stylus * M_obj_rot_raw if self._pause_: return
def compute_marker_transformation(coord_raw, obj_ref_mode): psi, theta, phi = np.radians(coord_raw[obj_ref_mode, 3:]) r_probe = tr.euler_matrix(psi, theta, phi, 'rzyx') t_probe = tr.translation_matrix(coord_raw[obj_ref_mode, :3]) m_probe = tr.concatenate_matrices(t_probe, r_probe) return m_probe
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 t_s0_raw = np.asmatrix(tr.translation_matrix(coords[3, :3])) r_s0_raw = np.asmatrix( tr.euler_matrix(np.radians(coords[3, 3]), np.radians(coords[3, 4]), np.radians(coords[3, 5]), 'rzyx')) s0_raw = np.asmatrix(tr.concatenate_matrices(t_s0_raw, r_s0_raw)) # compute change of basis for object fiducials in source frame base_obj_raw, q_obj_raw, base_inv_obj_raw = base_creation(fids_raw[:3, :3]) r_obj_raw = np.asmatrix(np.identity(4)) r_obj_raw[:3, :3] = base_obj_raw[:3, :3] t_obj_raw = np.asmatrix(tr.translation_matrix(q_obj_raw)) m_obj_raw = np.asmatrix(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, :]) fids_dyn[ic, 2] = -fids_dyn[ic, 2] else: # compute object fiducials in source frame fids_dyn[ic, :] = coords[ic, :] # compute object fiducials in vtk head frame a, b, g = np.radians(fids_dyn[ic, 3:]) T_p = tr.translation_matrix(fids_dyn[ic, :3]) R_p = tr.euler_matrix(a, b, g, 'rzyx') M_p = np.asmatrix(tr.concatenate_matrices(T_p, R_p)) M_img = np.asmatrix(m_change) * M_p angles_img = np.degrees(np.asarray(tr.euler_from_matrix(M_img, 'rzyx'))) coord_img = np.asarray(flip_x_m(tr.translation_from_matrix(M_img))) fids_img[ic, :] = np.hstack((coord_img, angles_img)) # compute object base change in vtk head frame base_obj_img, q_obj_img, base_inv_obj_img = base_creation(fids_img[:3, :3]) r_obj_img = np.asmatrix(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_trans_dyn = np.asmatrix(tr.translation_matrix(fids_dyn[3, :3])) s0_rot_dyn = np.asmatrix( tr.euler_matrix(np.radians(fids_dyn[3, 3]), np.radians(fids_dyn[3, 4]), np.radians(fids_dyn[3, 5]), 'rzyx')) s0_dyn = np.asmatrix(tr.concatenate_matrices(s0_trans_dyn, s0_rot_dyn)) return t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img