def run(self):
coreg_data = self.coreg_data
view_obj = 0
trck_init, trck_id = self.tracker.GetTrackerInfo()
# print('CoordCoreg: event {}'.format(self.event.is_set()))
while not self.event.is_set():
try:
if self.icp_queue.empty():
None
else:
self.icp, self.m_icp = self.icp_queue.get_nowait()
# print(f"Set the coordinate")
#print(self.icp, self.m_icp)
coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates()
coord, m_img = corregistrate_dynamic(coreg_data, coord_raw, self.ref_mode_id, [self.icp, self.m_icp])
# print("Coord: ", coord)
m_img_flip = m_img.copy()
m_img_flip[1, -1] = -m_img_flip[1, -1]
if self.icp:
m_img = bases.transform_icp(m_img, self.m_icp)
self.coord_queue.put_nowait([coord, [coord_raw, markers_flag], m_img, view_obj])
if self.view_tracts:
self.coord_tracts_queue.put_nowait(m_img_flip)
if not self.icp_queue.empty():
self.icp_queue.task_done()
# The sleep has to be in both threads
sleep(self.sle)
except queue.Full:
pass
def corregistrate_object_dynamic(inp, coord_raw, ref_mode_id, icp):
m_change, obj_ref_mode, t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img = inp
# transform raw marker coordinate to object center
m_probe = object_marker_to_center(coord_raw, obj_ref_mode, t_obj_raw, s0_raw, r_s0_raw)
# transform object center to reference marker if specified as dynamic reference
if ref_mode_id:
m_probe_ref = object_to_reference(coord_raw, m_probe)
else:
m_probe_ref = m_probe
# invert y coordinate
m_probe_ref[2, -1] = -m_probe_ref[2, -1]
# corregistrate from tracker to image space
m_img = tracker_to_image(m_change, m_probe_ref, r_obj_img, m_obj_raw, s0_dyn)
if icp[0]:
m_img = bases.transform_icp(m_img, icp[1])
# compute rotation angles
angles = tr.euler_from_matrix(m_img, axes='sxyz')
# create output coordinate list
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])
return coord, m_img
def corregistrate_dynamic(inp, coord_raw, ref_mode_id, icp):
m_change, obj_ref_mode = inp
# transform raw marker coordinate to object center
m_probe = compute_marker_transformation(coord_raw, obj_ref_mode)
# transform object center to reference marker if specified as dynamic reference
if ref_mode_id:
m_ref = compute_marker_transformation(coord_raw, 1)
m_probe_ref = np.linalg.inv(m_ref) @ m_probe
else:
m_probe_ref = m_probe
# invert y coordinate
m_probe_ref[2, -1] = -m_probe_ref[2, -1]
# corregistrate from tracker to image space
m_img = m_change @ m_probe_ref
if icp[0]:
m_img = bases.transform_icp(m_img, icp[1])
# compute rotation angles
angles = tr.euler_from_matrix(m_img, axes='sxyz')
# create output coordinate list
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])
return coord, m_img
def run(self):
coreg_data = self.coreg_data
view_obj = 1
trck_init, trck_id = self.tracker.GetTrackerInfo()
# print('CoordCoreg: event {}'.format(self.event.is_set()))
while not self.event.is_set():
try:
if not self.icp_queue.empty():
self.icp, self.m_icp = self.icp_queue.get_nowait()
if not self.object_at_target_queue.empty():
self.target_flag = self.object_at_target_queue.get_nowait()
# print(f"Set the coordinate")
coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates()
coord, m_img = corregistrate_object_dynamic(coreg_data, coord_raw, self.ref_mode_id, [self.icp, self.m_icp])
# XXX: This is not the best place to do the logic related to approaching the target when the
# debug tracker is in use. However, the trackers (including the debug trackers) operate in
# the tracker space where it is hard to make the tracker approach the target in the image space.
# Ideally, the transformation from the tracker space to the image space (the function
# corregistrate_object_dynamic above) would be encapsulated in a class together with the
# tracker, and then the whole class would be mocked when using the debug tracker. However,
# those abstractions do not currently exist and doing them would need a larger refactoring.
#
if self.tracker_id == const.DEBUGTRACKAPPROACH and self.target is not None:
if self.last_coord is None:
self.last_coord = np.array(coord)
else:
coord = self.last_coord + (self.target - self.last_coord) * 0.05
self.last_coord = coord
angles = [np.radians(coord[3]), np.radians(coord[4]), np.radians(coord[5])]
translate = coord[0:3]
m_img = tr.compose_matrix(angles=angles, translate=translate)
m_img_flip = m_img.copy()
m_img_flip[1, -1] = -m_img_flip[1, -1]
# self.pipeline.set_message(m_img_flip)
if self.icp:
m_img = bases.transform_icp(m_img, self.m_icp)
self.coord_queue.put_nowait([coord, [coord_raw, markers_flag], m_img, view_obj])
# print('CoordCoreg: put {}'.format(count))
# count += 1
if self.view_tracts:
self.coord_tracts_queue.put_nowait(m_img_flip)
if not self.icp_queue.empty():
self.icp_queue.task_done()
# The sleep has to be in both threads
sleep(self.sle)
except queue.Full:
pass
def apply_icp(m_img, icp):
use_icp, m_icp = icp
if use_icp:
m_img = bases.transform_icp(m_img, m_icp)
return m_img