Feature/people tracking

people_recognition/package.xml

@@ -15,6 +15,7 @@
   <exec_depend>people_recognition_2d</exec_depend>
   <exec_depend>people_recognition_3d</exec_depend>
   <exec_depend>people_recognition_msgs</exec_depend>
+  <exec_depend>people_tracking</exec_depend>
 
   <export>
     <metapackage/>

people_tracking/CMakeLists.txt

@@ -0,0 +1,55 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(people_tracking)
+
+find_package(catkin REQUIRED COMPONENTS
+  message_generation
+  people_recognition_msgs
+  rgbd
+  rgbd_image_buffer
+  roscpp
+  sensor_msgs
+  visualization_msgs
+)
+
+catkin_python_setup()
+
+add_message_files(
+  FILES
+  FaceTarget.msg
+  ColourTarget.msg
+  DetectPerson.msg
+  DetectedPerson.msg
+)
+
+add_service_files(
+  FILES
+  Depth.srv
+)
+
+generate_messages(
+  DEPENDENCIES
+  sensor_msgs
+)
+
+catkin_package(
+  CATKIN_DEPENDS message_runtime sensor_msgs
+)
+
+include_directories(
+# include
+ SYSTEM
+ ${catkin_INCLUDE_DIRS}
+)
+
+add_executable(people_tracker test/experiments/people_tracker.cpp)
+target_link_libraries(people_tracker ${catkin_LIBRARIES})
+
+install(
+  TARGETS people_tracker
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+if (CATKIN_ENABLE_TESTING)
+  find_package(catkin_lint_cmake REQUIRED)
+  catkin_add_catkin_lint_test(-W2)
+endif()

people_tracking/README.md

@@ -0,0 +1,75 @@
+# People Tracking
+Node for people tracking. #TODO this README has been copied from people_detection_2d. check if it is still up to date
+
+## Custom dependencies
+- [image_recognition_openpose](https://github.com/tue-robotics/image_recognition/tree/master/image_recognition_openpose)
+- [image_recognition_openface](https://github.com/tue-robotics/image_recognition/tree/master/image_recognition_openpose)
+- [image_recognition_keras](https://github.com/tue-robotics/image_recognition/tree/master/image_recognition_keras)
+- [image_recognition_color_extractor](https://github.com/tue-robotics/image_recognition/tree/master/image_recognition_color_extractor)
+- [image_recognition_msgs](https://github.com/tue-robotics/image_recognition/tree/master/image_recognition_msgs)
+
+# How To
+### Start the dependent nodes
+The relative namespaces of the dependent nodes must be as follows so that their advertised services are the same as the values of the [parameters](#parameters) of people recognition node:
+```
+rosrun image_recognition_openpose openpose_node __ns:=openpose
+
+rosrun image_recognition_openface face_recognition_node __ns:=face_recognition
+
+rosrun image_recognition_keras face_properties_node __ns:=face_recognition
+
+rosrun image_recognition_color_extractor color_extractor_node
+```
+
+### Run the people recognition node and service
+```
+rosrun people_recognition_2d people_recognition_2d_node
+```
+This will create a service `detect_people` of type `RecognizePeople2D` and which requires a color image
+(`sensor_msgs/Image`) as input in the service request and returns `people` in the response which is an array of
+custom message type `people_recognition_msgs/Person2D`
+
+### Parameters
+| Name                         | Default Value                         |
+|------------------------------|---------------------------------------|
+| `~openpose_srv_name`         | `openpose/recognize`                  |
+| `~openface_srv_name`         | `face_recognition/recognize`          |
+| `~keras_srv_name`            | `face_recognition/get_face_properties`|
+| `~color_extractor_srv_name`  | `extract_color`                       |
+
+### Message definition of Person2D
+```
+string name
+uint8 age
+uint8 gender
+float64 gender_confidence
+string posture
+string emotion
+string[] shirt_colors
+image_recognition_msgs/Recognition[] body_parts
+  image_recognition_msgs/CategoricalDistribution categorical_distribution
+    image_recognition_msgs/CategoryProbability[] probabilities
+      string label
+      float32 probability
+    float32 unknown_probability
+  sensor_msgs/RegionOfInterest roi
+    uint32 x_offset
+    uint32 y_offset
+    uint32 height
+    uint32 width
+    bool do_rectify
+  uint32 group_id
+```
+### Test service
+```
+roscd people_recognition_2d/scripts
+./people_recognition_2d_srv_test image ../test/assets/example.jpg
+```
+
+# Work Flow
+The node first calls the recognize services of the openpose and openface nodes. ROIs of faces are extracted from
+the recognitions returned by OpenPose and are associated with the recognitions returned by OpenFace through the
+face ROIs to create a `Person2D` object. The ROIs of body parts returned by OpenPose are associated with each
+`Person2D` object. Face images are sent to the Keras node and properties (age and gender) are extracted and
+assoicated with each 'Person2D' object. The ROIs of the faces are shifted vertically to approximate the ROIs of
+the shirts. These are sent to the color extractor to get the dominant colors.

people_tracking/example/example.launch

@@ -0,0 +1,8 @@
+<?xml version="1.0"?>
+<launch>
+  <group ns="hero">
+    <node pkg="people_tracking" type="people_tracker" name="people_tracker">
+      <rosparam command="load" file="$(find people_tracking)/example/example_config.yaml"/>
+    </node>
+  </group>
+</launch>

people_tracking/example/example_config.yaml

@@ -0,0 +1,4 @@
+frame_id: base_link
+rgbd_topic: head_rgbd_sensor/rgbd
+rate: 10
+people_recognition_service: /hero/people_recognition/detect_people

people_tracking/msg/ColourTarget.msg

@@ -0,0 +1,8 @@
+float64 time
+int32 nr_batch
+int32 nr_persons
+
+float32[] colour_vectors
+int32[] x_positions
+int32[] y_positions
+int32[] z_positions

people_tracking/msg/DetectPerson.msg

@@ -0,0 +1,2 @@
+string name
+int32 age

people_tracking/msg/DetectedPerson.msg

@@ -0,0 +1,9 @@
+float64 time
+int32 nr_batch
+int32 nr_persons
+
+sensor_msgs/Image[] detected_persons
+int32[] x_positions
+int32[] y_positions
+int32[] z_positions
+

people_tracking/msg/FaceTarget.msg

@@ -0,0 +1,10 @@
+float64 time
+int32 batch_nr
+int32 nr_persons
+int32[] x_positions
+int32[] y_positions
+int32[] z_positions
+sensor_msgs/Image detected_person
+
+bool[] face_detections_valid
+bool[] face_detections

people_tracking/package.xml

@@ -0,0 +1,55 @@
+<?xml version="1.0"?>
+<?xml-model
+  href="http://download.ros.org/schema/package_format3.xsd"
+  schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>people_tracking</name>
+  <version>0.0.1</version>
+  <description>The people_tracking package</description>
+
+  <maintainer email="[email protected]">Arpit Aggarwal</maintainer>
+
+  <license>MIT</license>
+
+  <url type="bugtracker">https://github.com/tue-robotics/people_recognition/issues</url>
+  <url type="repository">https://github.com/tue-robotics/people_recognition/tree/master/people_recognition_2d</url>
+
+  <author email="[email protected]">Arpit Aggarwal</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <buildtool_depend>python3-setuptools</buildtool_depend>
+
+  <depend>sensor_msgs</depend>
+
+  <build_depend>people_recognition_msgs</build_depend>
+  <build_depend>rgbd</build_depend>
+  <build_depend>rgbd_image_buffer</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+
+  <build_depend>message_generation</build_depend>
+
+  <exec_depend>cv_bridge</exec_depend>
+  <exec_depend>image_recognition_util</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>people_recognition_msgs</exec_depend>
+  <exec_depend>rgbd</exec_depend>
+  <exec_depend>rgbd_image_buffer</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>visualization_msgs</exec_depend>
+
+  <test_depend>catkin_lint_cmake</test_depend>
+
+  <doc_depend>python3-sphinx</doc_depend>
+  <doc_depend>python-sphinx-autoapi-pip</doc_depend>
+  <doc_depend>python-sphinx-rtd-theme-pip</doc_depend>
+  <doc_depend>python3-yaml</doc_depend>
+
+  <export>
+    <rosdoc config="rosdoc.yaml" />
+  </export>
+
+</package>

people_tracking/rosdoc.yaml

@@ -0,0 +1,3 @@
+- builder: sphinx
+  sphinx_root_dir: docs
+  name: Python API

people_tracking/setup.py

@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+
+from setuptools import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['people_tracking'],
+    package_dir={'': 'src'},
+)
+
+setup(**d)

people_tracking/src/people_tracking/UKFclass.py

@@ -0,0 +1,55 @@
+import numpy as np
+from filterpy.kalman import MerweScaledSigmaPoints
+from filterpy.kalman import UnscentedKalmanFilter
+
+
+class UKF:
+    def __init__(self):
+        self.current_time = 0.0
+
+        dt = 0.1  # standard dt
+
+        # Create sigma points
+        self.points = MerweScaledSigmaPoints(6, alpha=0.1, beta=2.0, kappa=-1)
+
+        self.kf = UnscentedKalmanFilter(dim_x=6, dim_z=3, dt=dt, fx=self.fx, hx=self.hx, points=self.points)
+
+        # Initial state: [x, vx, y, vy, z, vz]
+        self.kf.x = np.array([1., 0, 300., 0, 1., 0])
+
+        # Initial uncertainty
+        self.kf.P *= 1
+
+        # Measurement noise covariance matrix
+        z_std = 0.2
+        self.kf.R = np.diag([z_std ** 2, z_std ** 2, z_std ** 2])
+
+        # Process noise covariance matrix
+        process_noise_stds = [3.0 ** 2, 0.1 ** 2, 3 ** 2, 0.1 ** 2, 0.1 ** 2, 0.1 ** 2]
+
+        self.kf.Q = np.diag(process_noise_stds)
+
+    def fx(self, x, dt):
+        """ State transition function """
+        F = np.array([[1, dt, 0, 0, 0, 0],
+                      [0, 1, 0, 0, 0, 0],
+                      [0, 0, 1, dt, 0, 0],
+                      [0, 0, 0, 1, 0, 0],
+                      [0, 0, 0, 0, 1, dt],
+                      [0, 0, 0, 0, 0, 1]], dtype=float)
+        return np.dot(F, x)
+
+    def hx(self, x):
+        """ Measurement function [x, y, z] """
+        return np.array([x[0], x[2], x[4]])
+
+    def predict(self, time):
+        """ Predict the next state """
+        delta_t = time - self.current_time
+        self.kf.predict(dt=delta_t)
+        self.current_time = time
+
+    def update(self, time, z):
+        """ Update filter with measurements z. """
+        self.predict(time)
+        self.kf.update(z)