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
