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
