Initial Folder Creation

people_tracking_v2/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_v2/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_v2/rosdoc.yaml

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

people_tracking_v2/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_v2/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)

people_tracking_v2/src/people_tracking/colour_check.py

@@ -0,0 +1,91 @@
+#!/usr/bin/env python
+import rospy
+import cv2
+import numpy as np
+from cv_bridge import CvBridge
+from typing import List
+
+from people_tracking.msg import ColourTarget, DetectedPerson
+from std_srvs.srv import Empty, EmptyResponse
+
+NODE_NAME = 'HoC'
+TOPIC_PREFIX = '/hero/'
+
+
+class HOC:
+    """Class for the histogram of colour node."""
+    def __init__(self) -> None:
+        # ROS Initialize
+        rospy.init_node(NODE_NAME, anonymous=True)
+        self.subscriber = rospy.Subscriber(TOPIC_PREFIX + 'person_detections', DetectedPerson, self.callback,
+                                           queue_size=1)
+        self.publisher = rospy.Publisher(TOPIC_PREFIX + 'HoC', ColourTarget, queue_size=2)
+        self.reset_service = rospy.Service(TOPIC_PREFIX + NODE_NAME + '/reset', Empty, self.reset)
+
+        # Variables
+        self.HoC_detections = []
+        self.last_batch_processed = 0
+
+    def reset(self, request):
+        """ Reset all stored variables in Class to their default values."""
+        self.HoC_detections = []
+        self.last_batch_processed = 0
+        return EmptyResponse()
+
+    @staticmethod
+    def get_vector(image, bins: int = 32) -> List[float]:
+        """ Return HSV-colour histogram vector from image.
+
+        :param image: cv2 image to turn into vector.
+        :param bins: amount of bins in histogram.
+        :return: HSV-colour histogram vector from image.
+        """
+        hsv_image = cv2.cvtColor(image, cv2.COLOR_RGB2HSV)  # Convert image to HSV
+
+        histograms = [cv2.calcHist([hsv_image], [i], None, [bins], [1, 256])
+                      for i in range(3)]  # Get color histograms
+
+        histograms = [hist / hist.sum() for hist in histograms]  # Normalize histograms
+
+        vector = np.concatenate(histograms, axis=0).reshape(-1)  # Create colour histogram vector
+        return vector.tolist()
+
+    def callback(self, data: DetectedPerson) -> None:
+        """ Get the colour vectors for each detected person and publish this."""
+        time = data.time
+        nr_batch = data.nr_batch
+        nr_persons = data.nr_persons
+        detected_persons = data.detected_persons
+        x_positions = data.x_positions
+        y_positions = data.y_positions
+        z_positions = data.z_positions
+
+        if nr_batch <= self.last_batch_processed:
+            return
+        if nr_persons < 1:
+            return
+
+        bridge = CvBridge()
+        colour_vectors = [self.get_vector(bridge.imgmsg_to_cv2(person, desired_encoding='passthrough')) for person in
+                          detected_persons]
+
+        msg = ColourTarget()
+        msg.time = time
+        msg.nr_batch = nr_batch
+        msg.nr_persons = nr_persons
+        msg.x_positions = x_positions
+        msg.y_positions = y_positions
+        msg.z_positions = z_positions
+        msg.colour_vectors = [item for sublist in colour_vectors for item in sublist]
+
+        self.publisher.publish(msg)
+        self.last_batch_processed = nr_batch
+
+
+if __name__ == '__main__':
+    try:
+        node_hoc = HOC()
+        rospy.spin()
+    except rospy.exceptions.ROSInterruptException:
+        rospy.loginfo("Failed to launch HoC Node")
+        pass