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 run(self): # m_change = self.coreg_data[0] # obj_ref_mode = self.coreg_data[2] # # trck_init = self.trck_info[0] # trck_id = self.trck_info[1] # trck_mode = self.trck_info[2] 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 = coord_raw[obj_ref_mode, 3:] t_probe_raw = asmatrix(tr.translation_matrix(coord_raw[obj_ref_mode, :3])) t_probe_raw[2, -1] = -t_probe_raw[2, -1] m_img = m_change * t_probe_raw coord = m_img[0, -1], m_img[1, -1], m_img[2, -1], psi, theta, phi 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 run(self): # m_change = self.coreg_data[0] # obj_ref_mode = self.coreg_data[2] # # trck_init = self.trck_info[0] # trck_id = self.trck_info[1] # trck_mode = self.trck_info[2] 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 = coord_raw[obj_ref_mode, 3:] t_probe_raw = asmatrix( tr.translation_matrix(coord_raw[obj_ref_mode, :3])) t_probe_raw[2, -1] = -t_probe_raw[2, -1] m_img = m_change * t_probe_raw coord = m_img[0, -1], m_img[1, -1], m_img[2, -1], psi, theta, phi 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 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 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 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