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):
trck_info = self.trck_info
coreg_data = self.coreg_data
view_obj = 0
trck_init, trck_id, trck_mode = trck_info
# print('CoordCoreg: event {}'.format(self.event.is_set()))
while not self.event.is_set():
try:
# print(f"Set the coordinate")
coord_raw = dco.GetCoordinates(trck_init, trck_id, trck_mode)
coord, m_img = corregistrate_dynamic(coreg_data, coord_raw,
self.ref_mode_id)
# print("Coord: ", coord)
m_img_flip = m_img.copy()
m_img_flip[1, -1] = -m_img_flip[1, -1]
self.coord_queue.put_nowait([coord, m_img, view_obj])
if self.view_tracts:
self.coord_tracts_queue.put_nowait(m_img_flip)
# The sleep has to be in both threads
sleep(self.sle)
except queue.Full:
pass
def run(self):
trck_info = self.trck_info
coreg_data = self.coreg_data
view_obj = 1
trck_init, trck_id, trck_mode = trck_info
# print('CoordCoreg: event {}'.format(self.event.is_set()))
while not self.event.is_set():
try:
# print(f"Set the coordinate")
coord_raw = dco.GetCoordinates(trck_init, trck_id, trck_mode)
coord, m_img = corregistrate_object_dynamic(
coreg_data, coord_raw, self.ref_mode_id)
# m_img = np.array([[0.38, -0.8, -0.45, 40.17],
# [0.82, 0.52, -0.24, 152.28],
# [0.43, -0.28, 0.86, 235.78],
# [0., 0., 0., 1.]])
# angles = [-0.318, -0.441, 1.134]
# coord = m_img[0, -1], m_img[1, -1], m_img[2, -1], \
# np.degrees(angles[0]), np.degrees(angles[1]), np.degrees(angles[2])
m_img_flip = m_img.copy()
m_img_flip[1, -1] = -m_img_flip[1, -1]
# self.pipeline.set_message(m_img_flip)
self.coord_queue.put_nowait([coord, m_img, view_obj])
# print('CoordCoreg: put {}'.format(count))
# count += 1
if self.view_tracts:
self.coord_tracts_queue.put_nowait(m_img_flip)
# The sleep has to be in both threads
sleep(self.sle)
except queue.Full:
pass
def run(self):
m_inv = self.bases[0]
n = self.bases[1]
q1 = self.bases[2]
q2 = self.bases[3]
trck_init = self.trck_info[0]
trck_id = self.trck_info[1]
trck_mode = self.trck_info[2]
while self.nav_id:
trck_coord = dco.GetCoordinates(trck_init, trck_id, trck_mode)
trck_xyz = mat([[trck_coord[0]], [trck_coord[1]], [trck_coord[2]]])
img = q1 + (m_inv*n)*(trck_xyz - q2)
coord = (float(img[0]), float(img[1]), float(img[2]), trck_coord[3],
trck_coord[4], trck_coord[5])
# Tried several combinations and different locations to send the messages,
# however only this one does not block the GUI during navigation.
wx.CallAfter(Publisher.sendMessage, 'Co-registered points', coord[0:3])
wx.CallAfter(Publisher.sendMessage, 'Set camera in volume', coord[0:3])
# TODO: Optimize the value of sleep for each tracking device.
# 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)
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',
(m_img, coord))
# TODO: Optimize the value of sleep for each tracking device.
sleep(0.175)
if self._pause_:
return
def OnTrackerFiducials(self, evt):
btn_id = const.BTNS_TRK[evt.GetId()].keys()[0]
coord = None
if self.trk_init and self.tracker_id:
coord = dco.GetCoordinates(self.trk_init, self.tracker_id,
self.ref_mode_id)
else:
dlg.NavigationTrackerWarning(0, 'choose')
# Update number controls with tracker coordinates
if coord is not None:
self.fiducials[btn_id, :] = coord[0:3]
for n in [0, 1, 2]:
self.numctrls_coord[btn_id][n].SetValue(float(coord[n]))
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
