fixed visualisation of gaze due to publishing in camera space rather …

…than ros space

implemented tf2 rather than tf
averaging of latency for human readability

rt_gene/CMakeLists.txt

@@ -112,7 +112,7 @@ generate_dynamic_reconfigure_options(
 catkin_package(
 #  INCLUDE_DIRS include
 #  LIBRARIES rt_gene
-  CATKIN_DEPENDS rospy std_msgs geometry_msgs sensor_msgs image_geometry cv_bridge image_transport tf tf_conversions message_runtime
+  CATKIN_DEPENDS rospy std_msgs geometry_msgs sensor_msgs image_geometry cv_bridge image_transport tf tf2_ros message_runtime
 #  DEPENDS system_lib
 )
 

rt_gene/launch/estimate_gaze.launch

@@ -7,11 +7,11 @@
     <arg name="visualise_eyepose" default="True" />
 
     <arg name="video_namespace" default="/kinect2/hd" />
-    <arg name="video_image_topic" default="image_color_rect" />
+    <arg name="video_image_topic" default="image_color" />
     <arg name="video_info_topic" default="camera_info" />
 
-    <arg name="camera_frame" default="/kinect2_link" />
-    <arg name="ros_frame" default="/kinect2_ros_frame" />
+    <arg name="camera_frame" default="kinect2_link" />
+    <arg name="ros_frame" default="kinect2_ros_frame" />
 
     <arg name="group_name" default="gaze" />
     <arg name="tf_prefix" default="$(arg group_name)" />
@@ -23,19 +23,20 @@
     <env name="KMP_BLOCKTIME" value="1" />
 
     <group ns="$(arg group_name)">
-      <node pkg="rt_gene" type="extract_landmarks_node.py" name="extract_landmarks_new" output="screen">
+      <node pkg="rt_gene" type="extract_landmarks_node.py" name="$(arg group_name)_extract_landmarks_new" output="screen">
           <param name="device_id_facedetection"  value="$(arg device_id_facedetection)" />
           <param name="use_face_encoding_tracker" value="$(arg use_face_encoding_tracker)" />
           <param name="face_encoding_threshold" value="$(arg face_encoding_threshold)" />
           <param name="camera_frame" value="$(arg camera_frame)" />
+          <param name="ros_tf_frame" value="$(arg ros_frame)" />
           <param name="tf_prefix" value="$(arg tf_prefix)" />
           <param name="visualise_headpose" value="$(arg visualise_headpose)" />
 
           <remap from="/camera_info" to="$(arg video_namespace)/$(arg video_info_topic)"/>
           <remap from="/image" to="$(arg video_namespace)/$(arg video_image_topic)"/>
       </node>
 
-      <node pkg="rt_gene" type="estimate_gaze.py" name="estimate_gaze_twoeyes" output="screen">
+      <node pkg="rt_gene" type="estimate_gaze.py" name="$(arg group_name)_estimate_gaze_twoeyes" output="screen">
           <rosparam param="model_files">['model_nets/Model_allsubjects1.h5']</rosparam>
           <!-- rosparam param="model_files">['model_nets/all_subjects_mpii_prl_utmv_0_02.h5', 'model_nets/all_subjects_mpii_prl_utmv_1_02.h5', 'model_nets/all_subjects_mpii_prl_utmv_2_02.h5', 'model_nets/all_subjects_mpii_prl_utmv_3_02.h5']</rosparam -->
           <param name="tf_prefix" value="$(arg tf_prefix)" />/

rt_gene/launch/start_video.launch

@@ -1,6 +1,6 @@
 <launch>
     <arg name="video_namespace" default="/kinect2/hd"/>
-    <arg name="video_image_topic" default="image_color_rect"/>
+    <arg name="video_image_topic" default="image_color"/>
     <arg name="video_info_topic" default="camera_info"/>
     <arg name="camera_name" default="kinect2"/>
     <arg name="video_file" default="/home/tobias/video.mov"/>

rt_gene/launch/start_webcam.launch

@@ -3,7 +3,7 @@
   <arg name="camera_info_name" default="webcam_blue_26010230.yaml" />
 
   <arg name="video_namespace" default="/kinect2/hd"/>
-  <arg name="video_image_topic" default="image_color_rect"/>
+  <arg name="video_image_topic" default="image_color"/>
   <arg name="video_info_topic" default="camera_info"/>
 
   <arg name="rgb_frame_id" default="/kinect2_link"/>

rt_gene/package.xml

@@ -37,7 +37,7 @@
   <depend>cv_bridge</depend>
   <depend>image_transport</depend>
   <depend>tf</depend>
-  <depend>tf_conversions</depend>
+  <depend>tf2_ros</depend>
   <!--depend>camera_info_manager_py</depend-->
   <depend>uvc_camera</depend>
   <depend>dynamic_reconfigure</depend>

rt_gene/scripts/estimate_gaze.py

@@ -13,13 +13,15 @@
 import os
 import numpy as np
 from tqdm import tqdm
+import collections
 
 import rospkg
 import rospy
+from geometry_msgs.msg import TransformStamped
 from sensor_msgs.msg import Image
 from cv_bridge import CvBridge
-from tf import TransformBroadcaster, TransformListener
-import tf.transformations
+import tf2_ros
+from tf import transformations
 
 import rt_gene.gaze_tools as gaze_tools
 from rt_gene.subject_ros_bridge import SubjectListBridge
@@ -33,19 +35,22 @@ def __init__(self, device_id_gaze, model_files):
         self.bridge = CvBridge()
         self.subjects_bridge = SubjectListBridge()
 
-        self.tf_broadcaster = TransformBroadcaster()
-        self.tf_listener = TransformListener()
+        self.tf2_broadcaster = tf2_ros.TransformBroadcaster()
+        self.tf2_buffer = tf2_ros.Buffer()
+        self.tf2_listener = tf2_ros.TransformListener(self.tf2_buffer)
 
         self.tf_prefix = rospy.get_param("~tf_prefix", "gaze")
         self.headpose_frame = self.tf_prefix + "/head_pose_estimated"
-        self.ros_tf_frame = rospy.get_param("~ros_tf_frame", "/kinect2_ros_frame")
+        self.ros_tf_frame = rospy.get_param("~ros_tf_frame", "kinect2_ros_frame")
 
         self.image_subscriber = rospy.Subscriber("/subjects/images", MSG_SubjectImagesList, self.image_callback, queue_size=3, buff_size=2 ** 24)
         self.subjects_gaze_img = rospy.Publisher("/subjects/gazeimages", Image, queue_size=3)
 
         self.visualise_eyepose = rospy.get_param("~visualise_eyepose", default=True)
 
         self._last_time = rospy.Time().now()
+        self._freq_deque = collections.deque(maxlen=30)  # average frequency statistic over roughly one second
+        self._latency_deque = collections.deque(maxlen=30)
 
     def publish_image(self, image, image_publisher, timestamp):
         """This image publishes the `image` to the `image_publisher` with the given `timestamp`."""
@@ -68,16 +73,17 @@ def image_callback(self, subject_image_list):
         valid_subject_list = []
         for subject_id, s in subjects_dict.items():
             try:
-                (trans_head, rot_head) = self.tf_listener.lookupTransform(self.ros_tf_frame, self.headpose_frame + str(subject_id), timestamp)
-                euler_angles_head = list(tf.transformations.euler_from_quaternion(rot_head))
+                transform_msg = self.tf2_buffer.lookup_transform(self.ros_tf_frame, self.headpose_frame + str(subject_id), timestamp)
+                rot_head = transform_msg.transform.rotation
+                euler_angles_head = list(transformations.euler_from_quaternion([rot_head.x, rot_head.y, rot_head.z, rot_head.w]))
                 euler_angles_head = gaze_tools.limit_yaw(euler_angles_head)
 
                 phi_head, theta_head = gaze_tools.get_phi_theta_from_euler(euler_angles_head)
                 input_head_list.append([theta_head, phi_head])
                 input_r_list.append(self.input_from_image(s.right))
                 input_l_list.append(self.input_from_image(s.left))
                 valid_subject_list.append(subject_id)
-            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, tf.Exception):
+            except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException, tf2_ros.TransformException):
                 pass
 
         if len(valid_subject_list) == 0:
@@ -105,21 +111,39 @@ def image_callback(self, subject_image_list):
 
         _now = rospy.Time().now()
         _freq = 1.0 / (_now - self._last_time).to_sec()
+        self._freq_deque.append(_freq)
+        self._latency_deque.append(_now.to_sec() - timestamp.to_sec())
         self._last_time = _now
         tqdm.write(
-            '\033[2K\033[1;32mTime now: {:.2f} message color: {:.2f} diff: {:.2f}s for {} subjects {:.0f}Hz\033[0m'.format((_now.to_sec()), timestamp.to_sec(),
-                                                                                                                          _now.to_sec() - timestamp.to_sec(),
-                                                                                                                          len(valid_subject_list),
-                                                                                                                          _freq), end="\r")
+            '\033[2K\033[1;32mTime now: {:.2f} message color: {:.2f} latency: {:.2f}s for {} subject(s) {:.0f}Hz\033[0m'.format(
+                (_now.to_sec()), timestamp.to_sec(), np.mean(self._latency_deque), len(valid_subject_list), np.mean(self._freq_deque)), end="\r")
 
     def publish_gaze(self, est_gaze, msg_stamp, subject_id):
         """Publish the gaze vector as a PointStamped."""
         theta_gaze = est_gaze[0]
         phi_gaze = est_gaze[1]
         euler_angle_gaze = gaze_tools.get_euler_from_phi_theta(phi_gaze, theta_gaze)
-        quaternion_gaze = tf.transformations.quaternion_from_euler(*euler_angle_gaze)
-        self.tf_broadcaster.sendTransform((0, 0, 0.05),  # publish it 5cm above the head pose's origin (nose tip)
-                                          quaternion_gaze, msg_stamp, self.tf_prefix + "/world_gaze" + str(subject_id), self.headpose_frame + str(subject_id))
+        quaternion_gaze = transformations.quaternion_from_euler(*euler_angle_gaze)
+
+        t = TransformStamped()
+        t.header.stamp = msg_stamp
+        t.header.frame_id = self.headpose_frame + str(subject_id)
+        t.child_frame_id = self.tf_prefix + "/world_gaze" + str(subject_id)
+        t.transform.translation.x = 0
+        t.transform.translation.y = 0
+        t.transform.translation.z = 0.05  # publish it 5cm above the head pose's origin (nose tip)
+        t.transform.rotation.x = quaternion_gaze[0]
+        t.transform.rotation.y = quaternion_gaze[1]
+        t.transform.rotation.z = quaternion_gaze[2]
+        t.transform.rotation.w = quaternion_gaze[3]
+
+        try:
+            self.tf2_broadcaster.sendTransform([t])
+        except rospy.ROSException as exc:
+            if str(exc) == "publish() to a closed topic":
+                pass
+            else:
+                raise exc
 
 
 if __name__ == "__main__":

rt_gene/scripts/extract_landmarks_node.py

@@ -13,9 +13,11 @@
 from cv_bridge import CvBridge
 from rt_gene.extract_landmarks_method_base import LandmarkMethodBase
 from sensor_msgs.msg import Image, CameraInfo
+from geometry_msgs.msg import TransformStamped
 from tqdm import tqdm
 from image_geometry import PinholeCameraModel
-from tf import TransformBroadcaster, transformations
+from tf2_ros import TransformBroadcaster, TransformListener, Buffer
+from tf import transformations
 from dynamic_reconfigure.server import Server
 import rospy
 
@@ -39,9 +41,12 @@ def __init__(self, img_proc=None):
         self.bridge = CvBridge()
         self.__subject_bridge = SubjectListBridge()
 
-        self.camera_frame = rospy.get_param("~camera_frame", "/kinect2_link")
+        self.camera_frame = rospy.get_param("~camera_frame", "kinect2_link")
+        self.ros_tf_frame = rospy.get_param("~ros_tf_frame", "kinect2_ros_frame")
 
-        self.tf_broadcaster = TransformBroadcaster()
+        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)
 
@@ -108,11 +113,16 @@ def process_image(self, color_msg):
                 self.publish_pose(timestamp, translation_vector, head_rpy, subject_id)
 
                 if self.visualise_headpose:
-                    roll_pitch_yaw = gaze_tools.limit_yaw(head_rpy)
+                    # 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(roll_pitch_yaw)))
+                        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())
@@ -187,9 +197,29 @@ def publish_subject_list(self, timestamp, subjects):
         self.subject_pub.publish(subject_list_message)
 
     def publish_pose(self, timestamp, nose_center_3d_tf, head_rpy, subject_id):
-        self.tf_broadcaster.sendTransform(nose_center_3d_tf, transformations.quaternion_from_euler(*head_rpy), timestamp,
-                                          self.tf_prefix + "/head_pose_estimated" + str(subject_id),
-                                          self.camera_frame)
+        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)