def __init__(self, img_proc=None):
super(LandmarkMethodROS,
self).__init__(device_id_facedetection=rospy.get_param(
"~device_id_facedetection", default="cuda:0"))
self.subject_tracker = FaceEncodingTracker() if rospy.get_param(
"~use_face_encoding_tracker",
default=True) else SequentialTracker()
self.bridge = CvBridge()
self.__subject_bridge = SubjectListBridge()
self.ros_tf_frame = rospy.get_param("~ros_tf_frame",
"/kinect2_nonrotated_link")
self.tf_broadcaster = TransformBroadcaster()
self.tf_prefix = rospy.get_param("~tf_prefix", default="gaze")
self.visualise_headpose = rospy.get_param("~visualise_headpose",
default=True)
self.pose_stabilizers = {} # Introduce scalar stabilizers for pose.
try:
if img_proc is None:
tqdm.write("Wait for camera message")
cam_info = rospy.wait_for_message("/camera_info",
CameraInfo,
timeout=None)
self.img_proc = PinholeCameraModel()
# noinspection PyTypeChecker
self.img_proc.fromCameraInfo(cam_info)
else:
self.img_proc = img_proc
if np.array_equal(
self.img_proc.intrinsicMatrix().A,
np.array([[0., 0., 0.], [0., 0., 0.], [0., 0., 0.]])):
raise Exception(
'Camera matrix is zero-matrix. Did you calibrate'
'the camera and linked to the yaml file in the launch file?'
)
tqdm.write("Camera message received")
except rospy.ROSException:
raise Exception("Could not get camera info")
# multiple person images publication
self.subject_pub = rospy.Publisher("/subjects/images",
MSG_SubjectImagesList,
queue_size=3)
# multiple person faces publication for visualisation
self.subject_faces_pub = rospy.Publisher("/subjects/faces",
Image,
queue_size=3)
Server(ModelSizeConfig, self._dyn_reconfig_callback)
# have the subscriber the last thing that's run to avoid conflicts
self.color_sub = rospy.Subscriber("/image",
Image,
self.process_image,
buff_size=2**24,
queue_size=3)
class LandmarkMethodROS(LandmarkMethodBase):
def __init__(self, img_proc=None):
super(LandmarkMethodROS,
self).__init__(device_id_facedetection=rospy.get_param(
"~device_id_facedetection", default="cuda:0"))
self.subject_tracker = FaceEncodingTracker() if rospy.get_param(
"~use_face_encoding_tracker",
default=True) else SequentialTracker()
self.bridge = CvBridge()
self.__subject_bridge = SubjectListBridge()
self.camera_frame = rospy.get_param("~camera_frame", "kinect2_link")
self.ros_tf_frame = rospy.get_param("~ros_tf_frame",
"kinect2_ros_frame")
self.tf2_broadcaster = TransformBroadcaster()
self.tf2_buffer = Buffer()
self.tf2_listener = TransformListener(self.tf2_buffer)
self.tf_prefix = rospy.get_param("~tf_prefix", default="gaze")
self.visualise_headpose = rospy.get_param("~visualise_headpose",
default=True)
self.pose_stabilizers = {} # Introduce scalar stabilizers for pose.
try:
if img_proc is None:
tqdm.write("Wait for camera message")
cam_info = rospy.wait_for_message("/camera_info",
CameraInfo,
timeout=None)
self.img_proc = PinholeCameraModel()
# noinspection PyTypeChecker
self.img_proc.fromCameraInfo(cam_info)
else:
self.img_proc = img_proc
if np.array_equal(
self.img_proc.intrinsicMatrix().A,
np.array([[0., 0., 0.], [0., 0., 0.], [0., 0., 0.]])):
raise Exception(
'Camera matrix is zero-matrix. Did you calibrate'
'the camera and linked to the yaml file in the launch file?'
)
tqdm.write("Camera message received")
except rospy.ROSException:
raise Exception("Could not get camera info")
# multiple person images publication
self.subject_pub = rospy.Publisher("/subjects/images",
MSG_SubjectImagesList,
queue_size=3)
# multiple person faces publication for visualisation
self.subject_faces_pub = rospy.Publisher("/subjects/faces",
Image,
queue_size=3)
Server(ModelSizeConfig, self._dyn_reconfig_callback)
# have the subscriber the last thing that's run to avoid conflicts
self.color_sub = rospy.Subscriber("/image",
Image,
self.process_image,
buff_size=2**24,
queue_size=3)
def _dyn_reconfig_callback(self, config, _):
self.model_points /= (self.model_size_rescale *
self.interpupillary_distance)
self.model_size_rescale = config["model_size"]
self.interpupillary_distance = config["interpupillary_distance"]
self.model_points *= (self.model_size_rescale *
self.interpupillary_distance)
self.head_pitch = config["head_pitch"]
return config
def process_image(self, color_msg):
color_img = gaze_tools.convert_image(color_msg, "bgr8")
timestamp = color_msg.header.stamp
self.update_subject_tracker(color_img)
if not self.subject_tracker.get_tracked_elements():
tqdm.write("\033[2K\033[1;31mNo face found\033[0m", end="\r")
return
self.subject_tracker.update_eye_images(self.eye_image_size)
final_head_pose_images = []
for subject_id, subject in self.subject_tracker.get_tracked_elements(
).items():
if subject.left_eye_color is None or subject.right_eye_color is None:
continue
if subject_id not in self.pose_stabilizers:
self.pose_stabilizers[subject_id] = [
Stabilizer(state_num=2,
measure_num=1,
cov_process=0.1,
cov_measure=0.1) for _ in range(6)
]
success, head_rpy, translation_vector = self.get_head_pose(
subject.marks, subject_id)
if success:
# Publish all the data
self.publish_pose(timestamp, translation_vector, head_rpy,
subject_id)
if self.visualise_headpose:
# pitch roll yaw
trans_msg = self.tf2_buffer.lookup_transform(
self.ros_tf_frame, self.tf_prefix +
"/head_pose_estimated" + str(subject_id), timestamp)
rotation = trans_msg.transform.rotation
euler_angles_head = list(
transformations.euler_from_quaternion(
[rotation.x, rotation.y, rotation.z, rotation.w]))
euler_angles_head = gaze_tools.limit_yaw(euler_angles_head)
face_image_resized = cv2.resize(
subject.face_color,
dsize=(224, 224),
interpolation=cv2.INTER_CUBIC)
final_head_pose_images.append(
LandmarkMethodROS.visualize_headpose_result(
face_image_resized,
gaze_tools.get_phi_theta_from_euler(
euler_angles_head)))
if len(self.subject_tracker.get_tracked_elements().items()) > 0:
self.publish_subject_list(
timestamp, self.subject_tracker.get_tracked_elements())
if len(final_head_pose_images) > 0:
headpose_image_ros = self.bridge.cv2_to_imgmsg(
np.hstack(final_head_pose_images), "bgr8")
headpose_image_ros.header.stamp = timestamp
self.subject_faces_pub.publish(headpose_image_ros)
def get_head_pose(self, landmarks, subject_id):
"""
We are given a set of 2D points in the form of landmarks. The basic idea is that we assume a generic 3D head
model, and try to fit the 2D points to the 3D model based on the Levenberg-Marquardt optimization. We can use
OpenCV's implementation of SolvePnP for this.
This method is inspired by http://www.learnopencv.com/head-pose-estimation-using-opencv-and-dlib/
We are planning to replace this with our own head pose estimator.
:param landmarks: Landmark positions to be used to determine head pose
:param subject_id: ID of the subject that corresponds to the given landmarks
:return: success - Whether the pose was successfully extracted
rotation_vector - rotation vector that along with translation vector brings points from the model
coordinate system to the camera coordinate system
translation_vector - see rotation_vector
"""
camera_matrix = self.img_proc.intrinsicMatrix()
dist_coeffs = self.img_proc.distortionCoeffs()
try:
success, rodrigues_rotation, translation_vector, _ = cv2.solvePnPRansac(
self.model_points,
landmarks.reshape(len(self.model_points), 1, 2),
cameraMatrix=camera_matrix,
distCoeffs=dist_coeffs,
flags=cv2.SOLVEPNP_DLS)
except cv2.error as e:
tqdm.write(
'\033[2K\033[1;31mCould not estimate head pose: {}\033[0m'.
format(e),
end="\r")
return False, None, None
if not success:
tqdm.write(
'\033[2K\033[1;31mUnsuccessful in solvingPnPRanscan\033[0m',
end="\r")
return False, None, None
# this is generic point stabiliser, the underlying representation doesn't matter
rotation_vector, translation_vector = self.apply_kalman_filter_head_pose(
subject_id, rodrigues_rotation, translation_vector / 1000.0)
rotation_vector[0] += self.head_pitch
_rotation_matrix, _ = cv2.Rodrigues(rotation_vector)
_rotation_matrix = np.matmul(
_rotation_matrix, np.array([[0, 1, 0], [0, 0, -1], [-1, 0, 0]]))
_m = np.zeros((4, 4))
_m[:3, :3] = _rotation_matrix
_m[3, 3] = 1
_rpy_rotation = np.array(transformations.euler_from_matrix(_m))
return success, _rpy_rotation, translation_vector
def apply_kalman_filter_head_pose(self, subject_id,
rotation_vector_unstable,
translation_vector_unstable):
stable_pose = []
pose_np = np.array(
(rotation_vector_unstable, translation_vector_unstable)).flatten()
for value, ps_stb in zip(pose_np, self.pose_stabilizers[subject_id]):
ps_stb.update([value])
stable_pose.append(ps_stb.state[0])
stable_pose = np.reshape(stable_pose, (-1, 3))
rotation_vector = stable_pose[0]
translation_vector = stable_pose[1]
return rotation_vector, translation_vector
def publish_subject_list(self, timestamp, subjects):
assert (subjects is not None)
subject_list_message = self.__subject_bridge.images_to_msg(
subjects, timestamp)
self.subject_pub.publish(subject_list_message)
def publish_pose(self, timestamp, nose_center_3d_tf, head_rpy, subject_id):
t = TransformStamped()
t.header.frame_id = self.camera_frame
t.header.stamp = timestamp
t.child_frame_id = self.tf_prefix + "/head_pose_estimated" + str(
subject_id)
t.transform.translation.x = nose_center_3d_tf[0]
t.transform.translation.y = nose_center_3d_tf[1]
t.transform.translation.z = nose_center_3d_tf[2]
rotation = transformations.quaternion_from_euler(*head_rpy)
t.transform.rotation.x = rotation[0]
t.transform.rotation.y = rotation[1]
t.transform.rotation.z = rotation[2]
t.transform.rotation.w = rotation[3]
try:
self.tf2_broadcaster.sendTransform([t])
except rospy.ROSException as exc:
if str(exc) == "publish() to a closed topic":
pass
else:
raise exc
self.tf2_buffer.set_transform(t, 'extract_landmarks')
def update_subject_tracker(self, color_img):
faceboxes = self.get_face_bb(color_img)
if len(faceboxes) == 0:
self.subject_tracker.clear_elements()
return
tracked_subjects = self.get_subjects_from_faceboxes(
color_img, faceboxes)
# track the new faceboxes according to the previous ones
self.subject_tracker.track(tracked_subjects)