Merge pull request #12 from tony23545/master

Add kf_hungarian tracker

Author: SteveMacenski

.gitignore

@@ -0,0 +1,3 @@
+*~
+*.pyc
+\#*

kf_hungarian_tracker/README.md

@@ -0,0 +1,28 @@
+# kf_hungarian_tracker
+
+This package implememts a multi-object tracker with Kalman filter and Hungarian algorithm. 
+Hungarian algorithm is used to associate detection and previously known objects. 
+For each object, a Kalman filter is maintained to track the position and velocity. 
+
+### Hungarian algorithm
+
+[Hungarian algorithm](https://en.wikipedia.org/wiki/Hungarian_algorithm) is a combinatorial optimization algorithm that solves the assignment problem in polynomial time. 
+Here we use an implementation from `scipy.optimize.linear_sum_assignment`. The default cost function is Euclidean distance between two object's center. 
+More cost functions like IoU of bounding boxes could be used.
+
+### Parameters
+
+Parameters can be set in `config/kf_hungarian.yaml`. For more information on parameters for Kalman filter, check out [KalmanFilter](https://docs.opencv.org/master/dd/d6a/classcv_1_1KalmanFilter.html) from OpenCV. 
+
+| parameters       | Meaning        | Default |
+| ---------------- | ------------- | ------- |
+| global_frame     | transform from pointcloud frame to global frame,<br>None means message frame is global  | camera_link   |
+| death_threshold  | maximum missing frames before deleting an obstacle  | 3 |
+| top_down | whether project 3D points on ground plane, set to do top-down tracking | False |
+| measurement_noise_cov | measurement noise for Kalman filter [x,y,z] | [1., 1., 1.] |
+| error_cov_post | initial posteriori error estimate covariance matrix [x,y,z,vx,vy,vz] | [1., 1., 1., 10., 10., 10.] |
+| process_noise_cov | model process noise covariance with estimated [acceleration](https://github.com/tony23545/navigation2_dynamic/blob/master/kf_hungarian_tracker/kf_hungarian_tracker/obstacle_class.py#L59) [ax,ay,az] | [2., 2., 0.5] |
+| vel_filter | minimum and maximum velocity to filter obstacles [min,max] (m/s) | [0.1, 2.0] |
+| height_filter | minimum and maximum height (z) to filter obstacles [min,max] (m) | [-2.0, 2.0] | 
+| cost_filter | filter Hungarian assignments with cost greater than threshold (unit is `m` for Euclidean cost function) | 1.0 |
+* The default units are `m` and `m/s`, could be changed according to the detection.

kf_hungarian_tracker/config/kf_hungarian.yaml

@@ -0,0 +1,14 @@
+kf_hungarian_node:
+  ros__parameters:
+    death_threshold: 3 # maximum mising frames before deleting an obstacle
+    global_frame: "camera_link" # none means camera frame is the global frame
+    # Kamlan filter related
+    top_down: False # whether use top-down (x, y) view or 3D (x, y, z)
+    measurement_noise_cov: [1., 1., 1.] # if it's use top-down view, set the 3rd number to 0
+    error_cov_post: [1., 1., 1., 10., 10., 10.] # if use top-down view, set the 3rd and 6th to 0
+    process_noise_cov: [2., 2., 0.5] # processNoiseCov, model as acceration noise, if use top-down view, the 3rd number should be 0
+    # obstacle filter
+    vel_filter: [0.1, 2.0] # minimum and maximum velocity to filter obstacles
+    height_filter: [-2.0, 2.0] # minimum and maximum height (z) to filter obstacles
+    cost_filter: 1.0 # filter Hungarian assignment with cost greater than threshold
+    

kf_hungarian_tracker/kf_hungarian_tracker/__init__.py

@@ -0,0 +1,261 @@
+import numpy as np 
+from scipy.optimize import linear_sum_assignment
+
+from nav2_dynamic_msgs.msg import Obstacle, ObstacleArray
+from visualization_msgs.msg import Marker, MarkerArray
+
+import rclpy
+from rclpy.node import Node
+import colorsys
+from kf_hungarian_tracker.obstacle_class import ObstacleClass
+
+from tf2_ros import LookupException
+from tf2_ros.buffer import Buffer
+from tf2_ros.transform_listener import TransformListener
+from tf2_geometry_msgs import do_transform_point, do_transform_vector3
+from geometry_msgs.msg import PointStamped, Vector3Stamped
+
+class KFHungarianTracker(Node):
+    '''Use Kalman Fiter and Hungarian algorithm to track multiple dynamic obstacles
+
+    Use Hungarian algorithm to match presenting obstacles with new detection and maintain a kalman filter for each obstacle.
+    spawn ObstacleClass when new obstacles come and delete when they disappear for certain number of frames
+
+    Attributes:
+        obstacle_list: a list of ObstacleClass that currently present in the scene
+        max_id: the maximum id assigned 
+        sec, nanosec: timing from sensor msg
+        detection_sub: subscrib detection result from detection node
+        tracker_obstacle_pub: publish tracking obstacles with ObstacleArray
+        tracker_pose_pub: publish tracking obstacles with PoseArray, for rviz visualization
+    '''
+
+    def __init__(self):
+        '''initialize attributes and setup subscriber and publisher'''
+
+        super().__init__('kf_hungarian_node')
+        self.declare_parameters(
+            namespace='',
+            parameters=[
+                ('global_frame', "camera_link"),
+                ('process_noise_cov', [2., 2., 0.5]),
+                ('top_down', False),
+                ('death_threshold', 3),
+                ('measurement_noise_cov', [1., 1., 1.]),
+                ('error_cov_post', [1., 1., 1., 10., 10., 10.]),
+                ('vel_filter', [0.1, 2.0]),
+                ('height_filter', [-2.0, 2.0]),
+                ('cost_filter', 1.0)
+            ])
+        self.global_frame = self.get_parameter("global_frame")._value
+        self.death_threshold = self.get_parameter("death_threshold")._value
+        self.measurement_noise_cov = self.get_parameter("measurement_noise_cov")._value
+        self.error_cov_post = self.get_parameter("error_cov_post")._value
+        self.process_noise_cov = self.get_parameter("process_noise_cov")._value
+        self.vel_filter = self.get_parameter("vel_filter")._value
+        self.height_filter = self.get_parameter("height_filter")._value
+        self.top_down = self.get_parameter("top_down")._value
+        self.cost_filter = self.get_parameter("cost_filter")._value
+
+        self.obstacle_list = []
+        self.max_id = 0
+        self.sec = 0
+        self.nanosec = 0
+
+        # subscribe to detector 
+        self.detection_sub = self.create_subscription(
+            ObstacleArray,
+            'detection',
+            self.callback,
+            10)
+
+        # publisher for tracking result
+        self.tracker_obstacle_pub = self.create_publisher(ObstacleArray, 'tracking', 10)
+        self.tracker_marker_pub = self.create_publisher(MarkerArray, 'marker', 10)
+
+        # setup tf related
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self)
+
+    def callback(self, msg):
+        '''callback function for detection result'''
+
+        # update delta time
+        dt = (msg.header.stamp.sec - self.sec) + (msg.header.stamp.nanosec - self.nanosec) / 1e9
+        self.sec = msg.header.stamp.sec
+        self.nanosec = msg.header.stamp.nanosec
+
+        # get detection
+        detections = msg.obstacles
+        num_of_detect = len(detections)
+        num_of_obstacle = len(self.obstacle_list)
+
+        # kalman predict
+        for obj in self.obstacle_list:
+            obj.predict(dt)
+
+        # transform to global frame
+        if self.global_frame is not None:
+            try:
+                trans = self.tf_buffer.lookup_transform(self.global_frame, msg.header.frame_id, rclpy.time.Time())
+                msg.header.frame_id = self.global_frame
+                for i in range(len(detections)):
+                    # transform position (point)
+                    p = PointStamped()
+                    p.point = detections[i].position
+                    detections[i].position = do_transform_point(p, trans).point
+                    # transform velocity (vector3)
+                    v = Vector3Stamped()
+                    v.vector = detections[i].velocity
+                    detections[i].velocity = do_transform_vector3(v, trans).vector
+                    # transform size (vector3)
+                    s = Vector3Stamped()
+                    s.vector = detections[i].size
+                    detections[i].size = do_transform_vector3(s, trans).vector
+
+            except LookupException:
+                self.get_logger().info('fail to get tf from {} to {}'.format(msg.header.frame_id, self.global_frame))
+                return
+
+        # hungarian matching
+        cost = np.zeros((num_of_obstacle, num_of_detect))
+        for i in range(num_of_obstacle):
+            for j in range(num_of_detect):
+                cost[i, j] = self.obstacle_list[i].distance(detections[j])
+        obs_ind, det_ind = linear_sum_assignment(cost)
+
+        # filter assignment according to cost
+        new_obs_ind = []
+        new_det_ind = []
+        for o, d in zip(obs_ind, det_ind):
+            if cost[o, d] < self.cost_filter:
+                new_obs_ind.append(o)
+                new_det_ind.append(d)
+        obs_ind = new_obs_ind
+        det_ind = new_det_ind
+
+        # kalman update
+        for o, d in zip(obs_ind, det_ind):
+            self.obstacle_list[o].correct(detections[d])
+
+        # birth of new detection obstacles and death of disappear obstacle
+        self.birth(det_ind, num_of_detect, detections)
+        dead_object_list = self.death(obs_ind, num_of_obstacle)
+
+        # apply velocity and height filter
+        filtered_obstacle_list = []
+        for obs in self.obstacle_list:
+            obs_vel = np.linalg.norm(np.array([obs.msg.velocity.x, obs.msg.velocity.y, obs.msg.velocity.z]))
+            obs_height = obs.msg.position.z
+            if obs_vel > self.vel_filter[0] and obs_vel < self.vel_filter[1] and obs_height > self.height_filter[0] and obs_height < self.height_filter[1]:
+                filtered_obstacle_list.append(obs)
+
+        # construct ObstacleArray
+        if self.tracker_obstacle_pub.get_subscription_count() > 0:
+            obstacle_array = ObstacleArray()
+            obstacle_array.header = msg.header
+            track_list = []
+            for obs in filtered_obstacle_list:
+                # do not publish obstacles with low speed
+                track_list.append(obs.msg)
+            obstacle_array.obstacles = track_list
+            self.tracker_obstacle_pub.publish(obstacle_array)
+
+        # rviz visualization
+        if self.tracker_marker_pub.get_subscription_count() > 0:
+            marker_array = MarkerArray()
+            marker_list = []
+            # add current active obstacles
+            for obs in filtered_obstacle_list:
+                (r, g, b) = colorsys.hsv_to_rgb(obs.msg.id * 30. % 360 / 360., 1., 1.) # encode id with rgb color
+                # make a cube 
+                marker = Marker()
+                marker.header = msg.header
+                marker.id = obs.msg.id
+                marker.type = 1 # CUBE
+                marker.action = 0
+                marker.color.a = 0.5
+                marker.color.r = r
+                marker.color.g = g
+                marker.color.b = b
+                marker.pose.position = obs.msg.position
+                angle = np.arctan2(obs.msg.velocity.y, obs.msg.velocity.x)
+                marker.pose.orientation.z = np.float(np.sin(angle / 2))
+                marker.pose.orientation.w = np.float(np.cos(angle / 2))
+                marker.scale = obs.msg.size
+                marker_list.append(marker)
+                # make an arrow
+                arrow = Marker()
+                arrow.header = msg.header
+                arrow.id = 255 - obs.msg.id
+                arrow.type = 0
+                arrow.action = 0
+                arrow.color.a = 1.0
+                arrow.color.r = r
+                arrow.color.g = g
+                arrow.color.b = b
+                arrow.pose.position = obs.msg.position
+                arrow.pose.orientation.z = np.float(np.sin(angle / 2))
+                arrow.pose.orientation.w = np.float(np.cos(angle / 2))
+                arrow.scale.x = np.linalg.norm([obs.msg.velocity.x, obs.msg.velocity.y, obs.msg.velocity.z])
+                arrow.scale.y = 0.05
+                arrow.scale.z = 0.05
+                marker_list.append(arrow)
+            # add dead obstacles to delete in rviz
+            for idx in dead_object_list:
+                marker = Marker()
+                marker.header = msg.header
+                marker.id = idx
+                marker.action = 2 # delete
+                arrow = Marker()
+                arrow.header = msg.header
+                arrow.id = 255 - idx
+                arrow.action = 2
+                marker_list.append(marker)
+                marker_list.append(arrow)
+
+            marker_array.markers = marker_list
+            self.tracker_marker_pub.publish(marker_array)
+
+    def birth(self, det_ind, num_of_detect, detections):
+        '''generate new ObstacleClass for detections that do not match any in current obstacle list'''
+        for det in range(num_of_detect):
+            if det not in det_ind:
+                self.obstacle_list.append(ObstacleClass(detections[det], self.max_id, self.top_down, self.measurement_noise_cov, self.error_cov_post, self.process_noise_cov))
+                self.max_id =  self.max_id  + 1
+
+    def death(self, obj_ind, num_of_obstacle):
+        '''count obstacles' missing frames and delete when reach threshold'''
+        new_object_list = []
+        dead_object_list = []
+        # for previous obstacles
+        for obs in range(num_of_obstacle):
+            if obs not in obj_ind:
+                self.obstacle_list[obs].dying += 1
+            else:
+                self.obstacle_list[obs].dying = 0
+
+            if self.obstacle_list[obs].dying < self.death_threshold:
+                new_object_list.append(self.obstacle_list[obs])
+            else:
+                dead_object_list.append(self.obstacle_list[obs].msg.id)
+        
+        # add newly born obstacles
+        for obs in range(num_of_obstacle, len(self.obstacle_list)):
+            new_object_list.append(self.obstacle_list[obs])
+
+        self.obstacle_list = new_object_list
+        return dead_object_list
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    node = KFHungarianTracker()
+    node.get_logger().info("start spining tracker node...")
+
+    rclpy.spin(node)
+
+    rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()