def publish_pose(self, timestamp, nose_center_3d_tf, rot_head, subject_id):
self.tf_broadcaster.sendTransform(
nose_center_3d_tf, rot_head, timestamp,
self.tf_prefix + "/head_pose_estimated" + str(subject_id),
self.rgb_frame_id_ros)
return gaze_tools.get_head_pose(nose_center_3d_tf, rot_head)
def compute_eye_gaze_estimation(self, subject_id, timestamp, input_r,
input_l):
"""
subject_id : integer, id of the subject
input_x : cv_image, input image of x eye
(phi_x) : double, phi angle estimated using pupil detection
(theta_x) : double, theta angle estimated using pupil detection
"""
try:
lct = self.tf_listener.getLatestCommonTime(
self.rgb_frame_id_ros, self.headpose_frame + str(subject_id))
if (timestamp - lct).to_sec() < 0.25:
tqdm.write('Time diff: ' + str((timestamp - lct).to_sec()))
(trans_head, rot_head) = self.tf_listener.lookupTransform(
self.rgb_frame_id_ros,
self.headpose_frame + str(subject_id), lct)
euler_angles_head = gaze_tools.get_head_pose(
trans_head, rot_head)
phi_head, theta_head = gaze_tools.get_phi_theta_from_euler(
euler_angles_head)
self.last_phi_head, self.last_theta_head = phi_head, theta_head
else:
if self.use_last_headpose and self.last_phi_head is not None:
tqdm.write('Big time diff, use last known headpose! ' +
str((timestamp - lct).to_sec()))
phi_head, theta_head = self.last_phi_head, self.last_theta_head
else:
tqdm.write(
'Too big time diff for head pose, do not estimate gaze!'
+ str((timestamp - lct).to_sec()))
return
start_time = time.time()
est_gaze_c = self.estimate_gaze_twoeyes(
input_l, input_r, np.array([theta_head, phi_head]))
self.gaze_buffer_c[subject_id].append(est_gaze_c)
if len(self.average_weights) == len(
self.gaze_buffer_c[subject_id]):
est_gaze_c_med = np.average(np.array(
self.gaze_buffer_c[subject_id]),
axis=0,
weights=self.average_weights)
self.publish_gaze(est_gaze_c_med, timestamp, subject_id)
tqdm.write('est_gaze_c: ' + str(est_gaze_c_med))
return est_gaze_c_med
tqdm.write('Elapsed: ' + str(time.time() - start_time))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException, tf.Exception) as tf_e:
print(tf_e)
except rospy.ROSException as ros_e:
if str(ros_e) == "publish() to a closed topic":
print("See ya")
return None