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 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
"""
M = coordinates_to_transformation_matrix(
position=reference[:3],
orientation=reference[3:],
axes='rzyx',
)
M_p = coordinates_to_transformation_matrix(
position=probe[:3],
orientation=probe[3:],
axes='rzyx',
)
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 transformation_matrix_to_coordinates(matrix, axes='sxyz'):
"""
Given a matrix that combines the rotation and translation, return the position and the orientation
determined by the matrix. The orientation is given as three Euler angles.
The inverse of coordinates_of_transformation_matrix when the parameter 'axes' matches.
:param matrix: A 4x4 transformation matrix.
:param axes: The order in which the rotations are done for the axes. See transformations.py for details. Defaults to 'sxyz'.
:return: The position (a vector of length 3) and Euler angles for the orientation in degrees (a vector of length 3).
"""
angles = tr.euler_from_matrix(matrix, axes=axes)
angles_as_deg = np.degrees(angles)
translation = tr.translation_from_matrix(matrix)
return translation, angles_as_deg
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 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
