diff --git a/docker/perception/tracking/tracking.Dockerfile b/docker/perception/tracking/tracking.Dockerfile index 368c9ac6..6cd50403 100644 --- a/docker/perception/tracking/tracking.Dockerfile +++ b/docker/perception/tracking/tracking.Dockerfile @@ -7,7 +7,7 @@ WORKDIR ${AMENT_WS}/src # Copy in source code COPY src/perception/tracking tracking -COPY src/wato_msgs/sample_msgs sample_msgs +COPY src/wato_msgs/perception_msgs/tracking_msgs tracking_msgs # Scan for rosdeps RUN apt-get -qq update && rosdep update && \ @@ -19,6 +19,16 @@ RUN apt-get -qq update && rosdep update && \ ################################# Dependencies ################################ FROM ${BASE_IMAGE} as dependencies +# Install pip +RUN apt-get update && apt-get install -y \ + python3 \ + python3-pip + +# Install python packages +COPY src/perception/tracking/requirements.txt requirements.txt +RUN python3 -m pip install -r requirements.txt +RUN rm requirements.txt + # Install Rosdep requirements COPY --from=source /tmp/colcon_install_list /tmp/colcon_install_list RUN apt-fast install -qq -y --no-install-recommends $(cat /tmp/colcon_install_list) diff --git a/modules/dev_overrides/docker-compose.perception.yaml b/modules/dev_overrides/docker-compose.perception.yaml index 3a1a522d..feb6345c 100644 --- a/modules/dev_overrides/docker-compose.perception.yaml +++ b/modules/dev_overrides/docker-compose.perception.yaml @@ -64,3 +64,4 @@ services: command: tail -F anything volumes: - ${MONO_DIR}/src/perception/tracking:/home/bolty/ament_ws/src/tracking + - ${MONO_DIR}/src/wato_msgs/perception_msgs/tracking_msgs:/home/bolty/ament_ws/src/tracking_msgs diff --git a/src/perception/tracking/CMakeLists.txt b/src/perception/tracking/CMakeLists.txt deleted file mode 100644 index ffb74e71..00000000 --- a/src/perception/tracking/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(tracking) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) - -ament_package() diff --git a/src/perception/tracking/config/AB3DMOT.yaml b/src/perception/tracking/config/AB3DMOT.yaml new file mode 100644 index 00000000..f1a9f489 --- /dev/null +++ b/src/perception/tracking/config/AB3DMOT.yaml @@ -0,0 +1,7 @@ + +TRACKER: + COVARIANCE: 'BaselineCovariance' # see covariance.py + MATCH_THRESHOLD: -0.1 # -1 * Minimum IOU threshold + MATCH_ALGORITHM: 'hungarian_match' # see algorithms.py + SCORE_METRIC: 'iou_metric' + USE_ANGULAR_VELOCITY: False diff --git a/src/perception/tracking/config/mahalanobis.yaml b/src/perception/tracking/config/mahalanobis.yaml new file mode 100644 index 00000000..5e5f3e83 --- /dev/null +++ b/src/perception/tracking/config/mahalanobis.yaml @@ -0,0 +1,7 @@ + +TRACKER: + COVARIANCE: 'BaselineCovariance' # see covariance.py + MATCH_THRESHOLD: 11 # maximum mahalanobis distance + MATCH_ALGORITHM: 'greedy_match' # see algorithms.py + SCORE_METRIC: 'mahalanobis_euclidian_metric' + USE_ANGULAR_VELOCITY: False diff --git a/src/perception/tracking/config/params.yaml b/src/perception/tracking/config/params.yaml new file mode 100644 index 00000000..fac34b73 --- /dev/null +++ b/src/perception/tracking/config/params.yaml @@ -0,0 +1,19 @@ +obstacle_class_names: + - UNKNOWN + - PEDISTRIAN + - CYCLIST + +traffic_sign_class_names: + - LEFT_TURN_SIGN + - RIGHT_TURN_SIGN + - STOP_SIGN + - PED_CROSSING + - DO_NOT_ENTER + - 5MPH_SIGN + - 10MPH_SIGN + - 15MPH_SIGN + - 20MPH_SIGN + - 25MPH_SIGN + - HANDICAP_SIGN + - RAILROAD_CROSSING + - PARKING_SIGN diff --git a/src/perception/tracking/launch/tracking.launch.py b/src/perception/tracking/launch/tracking.launch.py new file mode 100644 index 00000000..a7c8c04c --- /dev/null +++ b/src/perception/tracking/launch/tracking.launch.py @@ -0,0 +1,14 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + """Launch Radar object detection system.""" + trackingnode = Node( + package='tracking', + executable='tracking_node', + ) + + return LaunchDescription([ + trackingnode + ]) diff --git a/src/perception/tracking/package.xml b/src/perception/tracking/package.xml index bff8967c..bc112866 100644 --- a/src/perception/tracking/package.xml +++ b/src/perception/tracking/package.xml @@ -4,13 +4,16 @@ tracking 0.0.0 TODO: Package description - bolty + Steven Gong TODO: License declaration ament_cmake + tf_transformations + vision_msgs + tracking_msgs - ament_cmake + ament_python diff --git a/src/perception/tracking/readme.md b/src/perception/tracking/readme.md new file mode 100644 index 00000000..e1ba9da9 --- /dev/null +++ b/src/perception/tracking/readme.md @@ -0,0 +1,9 @@ +watod build tracking + +watod up tracking foxglove data_stream + +watod -t tracking +colcon build +ros2 launch tracking tracking.launch.py + +watod run data_stream ros2 bag play ./nuscenes/NuScenes-v1.0-mini-scene-0061/NuScenes-v1.0-mini-scene-0061_0.mcap --loop \ No newline at end of file diff --git a/src/perception/tracking/requirements.txt b/src/perception/tracking/requirements.txt new file mode 100644 index 00000000..6670735f --- /dev/null +++ b/src/perception/tracking/requirements.txt @@ -0,0 +1,5 @@ +transforms3d +numba +filterpy==1.4.5 +scipy +easydict diff --git a/src/perception/tracking/resource/tracking b/src/perception/tracking/resource/tracking new file mode 100755 index 00000000..e69de29b diff --git a/src/perception/tracking/setup.cfg b/src/perception/tracking/setup.cfg new file mode 100755 index 00000000..27254da0 --- /dev/null +++ b/src/perception/tracking/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/tracking +[install] +install_scripts=$base/lib/tracking diff --git a/src/perception/tracking/setup.py b/src/perception/tracking/setup.py new file mode 100755 index 00000000..080972e5 --- /dev/null +++ b/src/perception/tracking/setup.py @@ -0,0 +1,32 @@ +from setuptools import setup, find_packages +import os +from glob import glob + +package_name = 'tracking' + +setup( + name=package_name, + version='0.0.0', + # packages=[package_name], + packages=find_packages(), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'launch', 'include'), glob('launch/include/*.launch.py')), + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Steven', + maintainer_email='s36gong@watonomous.ca', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'tracking_node = tracking.tracker:main' + ], + }, +) diff --git a/src/perception/tracking/tracking/__init__.py b/src/perception/tracking/tracking/__init__.py new file mode 100755 index 00000000..e69de29b diff --git a/src/perception/tracking/tracking/core/__init__.py b/src/perception/tracking/tracking/core/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/perception/tracking/tracking/core/ab3dmot.py b/src/perception/tracking/tracking/core/ab3dmot.py new file mode 100644 index 00000000..2d168f80 --- /dev/null +++ b/src/perception/tracking/tracking/core/ab3dmot.py @@ -0,0 +1,327 @@ +# We implemented our method on top of AB3DMOT's KITTI tracking open-source code + +from __future__ import print_function + +import copy +import numpy as np +import time + +from filterpy.kalman import KalmanFilter + +from .utils.config import cfg, cfg_from_yaml_file, log_config_to_file +from . import algorithms, covariance + +def normalize_angle(angle): + """ + angle: In radians + returns: angle in radians between -pi and pi + """ + # reduce the angle + twopi = (2 * np.pi) + angle = angle % twopi + + # force it to be the positive remainder, so that 0 <= angle < 360 + angle = (angle + twopi) % twopi + + # force into the minimum absolute value residue class, so that -180 < angle <= 180 + if (angle > np.pi): + angle -= twopi + return angle + +class KalmanBoxTracker(object): + """ + This class represents the internel state of individual tracked objects observed as bbox. + """ + count = 0 + def __init__(self, bbox3D, + track_score, + tracking_name, + timestamp, + use_angular_velocity=False): + """ + Initialises a tracker using initial bounding box. + """ + #define constant velocity model + # [x, y, z, rot_y, l, w, h, x_dot, y_dot, z_dot] + if not use_angular_velocity: + self.kf = KalmanFilter(dim_x=10, dim_z=7) + self.kf.F = np.array([[1,0,0,0,0,0,0,1,0,0], # state transition matrix + [0,1,0,0,0,0,0,0,1,0], + [0,0,1,0,0,0,0,0,0,1], + [0,0,0,1,0,0,0,0,0,0], + [0,0,0,0,1,0,0,0,0,0], + [0,0,0,0,0,1,0,0,0,0], + [0,0,0,0,0,0,1,0,0,0], + [0,0,0,0,0,0,0,1,0,0], + [0,0,0,0,0,0,0,0,1,0], + [0,0,0,0,0,0,0,0,0,1]]) + + self.kf.H = np.array([[1,0,0,0,0,0,0,0,0,0], # measurement function, + [0,1,0,0,0,0,0,0,0,0], + [0,0,1,0,0,0,0,0,0,0], + [0,0,0,1,0,0,0,0,0,0], + [0,0,0,0,1,0,0,0,0,0], + [0,0,0,0,0,1,0,0,0,0], + [0,0,0,0,0,0,1,0,0,0]]) + else: + # with angular velocity + # [x, y, z, rot_y, l, w, h, x_dot, y_dot, z_dot, rot_y_dot] + self.kf = KalmanFilter(dim_x=11, dim_z=7) + self.kf.F = np.array([[1,0,0,0,0,0,0,1,0,0,0], # state transition matrix + [0,1,0,0,0,0,0,0,1,0,0], + [0,0,1,0,0,0,0,0,0,1,0], + [0,0,0,1,0,0,0,0,0,0,1], + [0,0,0,0,1,0,0,0,0,0,0], + [0,0,0,0,0,1,0,0,0,0,0], + [0,0,0,0,0,0,1,0,0,0,0], + [0,0,0,0,0,0,0,1,0,0,0], + [0,0,0,0,0,0,0,0,1,0,0], + [0,0,0,0,0,0,0,0,0,1,0], + [0,0,0,0,0,0,0,0,0,0,1]]) + + self.kf.H = np.array([[1,0,0,0,0,0,0,0,0,0,0], # measurement function, + [0,1,0,0,0,0,0,0,0,0,0], + [0,0,1,0,0,0,0,0,0,0,0], + [0,0,0,1,0,0,0,0,0,0,0], + [0,0,0,0,1,0,0,0,0,0,0], + [0,0,0,0,0,1,0,0,0,0,0], + [0,0,0,0,0,0,1,0,0,0,0]]) + + # Initialize the covariance matrix, see covariance.py for more details + cov = getattr(covariance, cfg.TRACKER.COVARIANCE)(tracking_name=tracking_name) + self.kf.P = cov.P + self.kf.Q = cov.Q + self.kf.R = cov.R + + self.kf.x[:7] = bbox3D.reshape((7, 1)) + + self.last_update = timestamp + self.id = KalmanBoxTracker.count + KalmanBoxTracker.count += 1 + self.history = [(timestamp, self.get_state())] # History of observations / predictions. List of pairs (timestamp, bbox) + self.hits = 1 # number of total hits including the first detection + self.hit_streak = 1 # number of continuing hit considering the first detection + self.first_continuing_hit = 1 + self.still_first = True + self.age = 0 + self.track_score = track_score + self.tracking_name = tracking_name + self.use_angular_velocity = use_angular_velocity + + def update(self, bbox3D, info, timestamp): + """ + Updates the state vector with observed bbox. + bbox3D: list[float] - [x,y,z,theta,l,w,h] + info: int, float - (label, confidence score) + timestamp: float - unix timestamp in seconds + """ + self.last_update = timestamp + self.hits += 1 + self.hit_streak += 1 # number of continuing hit + if self.still_first: + self.first_continuing_hit += 1 # number of continuing hit in the fist time + + ######################### + # Orientation Correction. Primarily aimed at cars. + # Not so usefull for us since Perception does not give proper + # orientation estimates for pedestrians + ######################### + self.kf.x[3] = normalize_angle(self.kf.x[3]) + + new_theta = bbox3D[3] + new_theta = normalize_angle(new_theta) + bbox3D[3] = new_theta + + predicted_theta = self.kf.x[3] + angle_diff = normalize_angle(new_theta - predicted_theta) + if abs(angle_diff) > np.pi / 2.0 and abs(angle_diff) < np.pi * 3 / 2.0: # if the angle of two theta is not acute angle + self.kf.x[3] += np.pi + self.kf.x[3] = normalize_angle(self.kf.x[3]) + + # now the angle is acute: < 90 or > 270, convert the case of > 270 to < 90 + if abs(new_theta - self.kf.x[3]) >= np.pi * 3 / 2.0: + if new_theta > 0: self.kf.x[3] += np.pi * 2 + else: self.kf.x[3] -= np.pi * 2 + + ######################### + # update label based on confidence + ######################### + label, confidence = info + if confidence > self.track_score: + self.track_score = confidence + self.tracking_name = label + + if self.tracking_name in cfg.TRAFFIC_SIGN_CLASSES: + self.kf.x[7:] = 0 + + ######################### + + self.kf.update(bbox3D) + self.kf.x[3] = normalize_angle(self.kf.x[3]) + if (len(self.history) > 0 and self.history[-1][0] == timestamp): + self.history[-1] = (timestamp, self.get_state()) + else: + self.history.append((timestamp, self.get_state())) + + + def predict(self, timestamp): + """ + Advances the state vector and returns the predicted bounding box estimate. + timestamp - unix timestamp in seconds (float) + """ + self.kf.predict() + self.kf.x[3] = normalize_angle(self.kf.x[3]) + + self.age += 1 + if(timestamp - self.last_update > 0): + self.hit_streak = 0 + self.still_first = False + self.history.append((timestamp, self.kf.x.reshape((-1, )))) + return copy.copy(self.kf.x) + + def get_state(self): + """ + Returns the current bounding box estimate. + """ + return self.kf.x.reshape((-1, )) + +class AB3DMOT(object): + def __init__(self, max_age=2, min_hits=2, tracking_name='car'): + """ + observation: + [x, y, z, rot_y, l, w, h] + state: + [x, y, z, rot_y, l, w, h, x_dot, y_dot, z_dot, rot_y_dot] + """ + self.max_age = max_age + self.min_hits = min_hits + self.trackers = [] + self.frame_count = 0 + self.tracking_name = tracking_name + self.use_angular_velocity = cfg.TRACKER.USE_ANGULAR_VELOCITY + self.match_threshold = cfg.TRACKER.MATCH_THRESHOLD + self.score_metric_fn = getattr(algorithms,cfg.TRACKER.SCORE_METRIC) + self.match_algorithm_fn = getattr(algorithms,cfg.TRACKER.MATCH_ALGORITHM) + + def update(self, dets, info, timestamp, update_only=False, distance_threshold=-1): + """ + Params: + dets - a numpy array of detections in the format [[x,y,z,theta,l,w,h],[x,y,z,theta,l,w,h],...] + info - a numpy array of other info for each det + timestamp - unix timestamp in seconds (float) + update_only - only update tracks don't create new ones + + Requires: this method must be called once for each frame even with empty detections. + Returns the a similar array, where the last column is the object ID. + + NOTE: The number of objects returned may differ from the number of detections provided. + """ + + self.frame_count += 1 + + trks = np.zeros((len(self.trackers),7)) # N x 7 , #get predicted locations from existing trackers. + to_del = [] + ret = [] + + # Run KF Prediction on existing tracks + for t,trk in enumerate(trks): + pos = self.trackers[t].predict(timestamp).reshape((-1, 1)) + trk[:] = [pos[0], pos[1], pos[2], pos[3], pos[4], pos[5], pos[6]] + if(np.any(np.isnan(pos))): + to_del.append(t) + + trks = np.ma.compress_rows(np.ma.masked_invalid(trks)) + for t in reversed(to_del): + self.trackers.pop(t) + + trks_S = [] + if 'mahalanobis' in cfg.TRACKER.SCORE_METRIC: + trks_S = np.array([np.matmul(np.matmul(tracker.kf.H, tracker.kf.P), tracker.kf.H.T) + tracker.kf.R for tracker in self.trackers]) + + matched, unmatched_dets, unmatched_trks = self.associate_detections_to_trackers(dets, trks, trks_S=trks_S) + unmatched_dets = list(unmatched_dets) + #update matched trackers with assigned detections + iou_scores = None + if distance_threshold > 0: + distances = algorithms.euclidian_distance(dets, trks) + + for t,trk in enumerate(self.trackers): + if t not in unmatched_trks: + d = matched[np.where(matched[:,1]==t)[0],0] # a list of index + if len(dets[d]) > 0 and len(info[d]) > 0: + if distance_threshold > 0 and abs(distances[d, t]) > distance_threshold: + if abs(distances[d, t]) > distance_threshold * 3 and not update_only: + unmatched_dets = np.append(unmatched_dets, d).astype(int) + continue + else: + trk.update(dets[d,:][0], info[d,:][0], timestamp) + + # Create and initialise new trackers for unmatched detections + if not update_only: + for i in unmatched_dets: # a scalar of index + tracking_name = info[i][0] # class label + track_score = info[i][1] # confidence + trk = KalmanBoxTracker(dets[i,:], track_score, tracking_name, timestamp, self.use_angular_velocity) + self.trackers.append(trk) + + # Detect and delete old tracks + self.trackers = [trk for trk in self.trackers if abs(timestamp - trk.last_update) < self.max_age] + + for trk in self.trackers: + d = trk.get_state() + + if trk.hits >= self.min_hits or self.frame_count <= self.min_hits: + ret.append(np.array([d, trk.id+1, trk.track_score])) # +1 as MOT benchmark requires positive + + + if(len(ret)>0): + return ret # x, y, z, theta, l, w, h, ID, other info, confidence + + return np.empty((0,15 + 7)) + + def associate_detections_to_trackers(self, detections, trackers, **kwargs): + """ + Assigns detections to tracked object (both represented as bounding boxes) + + detections: N x 7 + trackers: M x 8 + kwargs: { + trks_S: N x 7 x + ... + } + + Returns 3 lists of matches [M, 2], unmatched_detections [?] and unmatched_trackers [?] + """ + + if(len(trackers)==0): + return np.empty((0,2),dtype=int), np.arange(len(detections)), np.empty((0,8,3),dtype=int) + # score matrix between detections and trackers. Lower is better. Either Mahalonobis distance or - IOU + score_matrix = np.zeros((len(detections),len(trackers)),dtype=np.float32) + + score_matrix = self.score_metric_fn(detections, trackers, **kwargs) + matched_indices = self.match_algorithm_fn(score_matrix) + + unmatched_detections = [] + for d,det in enumerate(detections): + if(d not in matched_indices[:,0]): + unmatched_detections.append(d) + unmatched_trackers = [] + for t,trk in enumerate(trackers): + if len(matched_indices) == 0 or (t not in matched_indices[:,1]): + unmatched_trackers.append(t) + + #filter out matched with high score (bad) + matches = [] + for m in matched_indices: + match = score_matrix[m[0],m[1]] < self.match_threshold + if not match: + unmatched_detections.append(m[0]) + unmatched_trackers.append(m[1]) + else: + matches.append(m.reshape(1,2)) + if(len(matches)==0): + matches = np.empty((0,2),dtype=int) + else: + matches = np.concatenate(matches,axis=0) + + return matches, np.array(unmatched_detections), np.array(unmatched_trackers) diff --git a/src/perception/tracking/tracking/core/algorithms.py b/src/perception/tracking/tracking/core/algorithms.py new file mode 100644 index 00000000..0882056d --- /dev/null +++ b/src/perception/tracking/tracking/core/algorithms.py @@ -0,0 +1,103 @@ +# Metrics and matching algorithms for tracking + +import numpy as np +from .utils.geometry_utils import diff_orientation_correction, convert_3dbox_to_8corner, iou3d + +from scipy.optimize import linear_sum_assignment as linear_assignment + +def pre_threshold_match(distance_matrix): + to_max_mask = distance_matrix > mahalanobis_threshold + distance_matrix[to_max_mask] = cfg.TRACKER.MATCH_THRESHOLD + matched_indices = linear_assignment(distance_matrix) # hungarian algorithm + return np.transpose(np.asarray(matched_indices)) + +def hungarian_match(distance_matrix): + return np.transpose(np.asarray(linear_assignment(distance_matrix))) # hungarian algorithm + +def greedy_match(distance_matrix): + ''' + Find the one-to-one matching using greedy allgorithm choosing small distance + distance_matrix: (num_detections, num_tracks) + ''' + matched_indices = [] + + num_detections, num_tracks = distance_matrix.shape + distance_1d = distance_matrix.reshape(-1) + index_1d = np.argsort(distance_1d) + index_2d = np.stack([index_1d // num_tracks, index_1d % num_tracks], axis=1) + detection_id_matches_to_tracking_id = [-1] * num_detections + tracking_id_matches_to_detection_id = [-1] * num_tracks + for sort_i in range(index_2d.shape[0]): + detection_id = int(index_2d[sort_i][0]) + tracking_id = int(index_2d[sort_i][1]) + if tracking_id_matches_to_detection_id[tracking_id] == -1 and detection_id_matches_to_tracking_id[detection_id] == -1: + tracking_id_matches_to_detection_id[tracking_id] = detection_id + detection_id_matches_to_tracking_id[detection_id] = tracking_id + matched_indices.append([detection_id, tracking_id]) + + matched_indices = np.array(matched_indices) + return matched_indices + +def mahalanobis_metric(detections, trackers, **kwargs): + """ + Creates matrix of mahalanobis distances between detections and tracks + + detections: N x 7 + trackers: M x 8 + kwargs: { + trks_S: N x 7 x 7 + } + Returns score matrix [M x N] + """ + score_matrix = np.zeros((len(detections),len(trackers)),dtype=np.float32) + trks_S = kwargs['trks_S'] + for d,det in enumerate(detections): + for t,trk in enumerate(trackers): + S_inv = np.linalg.inv(trks_S[t]) # 7 x 7 + diff = np.expand_dims(det - trk, axis=1) # 7 x 1 + # manual reversed angle by 180 when diff > 90 or < -90 degree + corrected_angle_diff = diff_orientation_correction(det[3], trk[3]) + diff[3] = corrected_angle_diff + score_matrix[d, t] = np.sqrt(np.matmul(np.matmul(diff.T, S_inv), diff)[0][0]) + + return score_matrix + +def iou_metric(detections, trackers, **kwargs): + """ + Creates matrix of negative IOU score between detections and tracks + + detections: N x 7 + trackers: M x 8 + + Returns score matrix [M x N] + """ + score_matrix = np.zeros((len(detections),len(trackers)),dtype=np.float32) + + for d,det in enumerate(detections): + for t,trk in enumerate(trackers): + det_8corner = convert_3dbox_to_8corner(det) + trk_8corner = convert_3dbox_to_8corner(trk) + score_matrix[d,t] = -iou3d(det_8corner,trk_8corner)[0] + + return score_matrix + + +def euclidian_distance(detections, trackers, **kwargs): + """ + Creates matrix of euclidian distance score between detections and tracks (in meters) + + detections: N x 7 + trackers: M x 8 + + Returns score matrix [M x N] + """ + score_matrix = np.zeros((len(detections),len(trackers)),dtype=np.float32) + + for d,det in enumerate(detections): + for t,trk in enumerate(trackers): + score_matrix[d,t] = ((det[0] - trk[0])**2 + (det[1] - trk[1])**2 + (det[3] - trk[3])**2)**0.5 + + return score_matrix + +def mahalanobis_euclidian_metric(detections, trackers, **kwargs): + return euclidian_distance(detections, trackers, **kwargs) + mahalanobis_metric(detections, trackers, **kwargs) diff --git a/src/perception/tracking/tracking/core/bbox.py b/src/perception/tracking/tracking/core/bbox.py new file mode 100644 index 00000000..5e88dc08 --- /dev/null +++ b/src/perception/tracking/tracking/core/bbox.py @@ -0,0 +1,81 @@ +from typing import Optional + +class Bbox3d: + def __init__(self, x, y, z, rz, w, l, h, cls="UNKNOWN"): + """ + Dimensions are in the odom frame. + The relationships between these measurements are: + x, w <-> east, y, l <-> north, z, h <-> up + + x: x-coordinate of the center of the bounding box + y: y-coordinate of the center of the bounding box + z: z-coordinate of the center of the bounding box + rz: Rotation about the z-axis. This is yaw. + w: Width of the bounding box + l: Length of the bounding box + h: Height of the bounding box + cls: The class of the obstacle. Ex: "PEDESTRIAN", "CYCLIST". + """ + self.x = x + self.y = y + self.z = z + self.rz = rz + self.w = w + self.l = l + self.h = h + self.cls = cls + + +class Bbox2d: + def __init__(self, x, y, w, l, cls="UNKNOWN"): + """ + x, y are in the camera frame, with (x, y) = (0, 0) representing the top-left of the frame. + + x: x-coordinate of the top left corner of the bounding box + y: y-coordinate of the top left corner of the bounding box + w: Width of the bounding box + l: Length of the bounding box + cls: The class of the obstacle. Ex: "PEDESTRIAN", "CYCLIST". + """ + self.x = x + self.y = y + self.w = w + self.l = l + self.cls = cls + + +class BboxRadar: + def __init__(self, x, y, rz, w, l, vel_x, vel_y): + """ + Dimensions are in the odom frame. + The relationships between these measurements are: + x, w, vel_x <-> east, y, l, vel_y <-> north + + x: x=coordinate of the center of the bounding box + y: y-coordinate of the center of the bounding box + rz: Rotation about the z-axis. This is yaw. + vel_x: x-component of the velocity observed by the radar + vel_y: y-component of the velocity observed by the radar + """ + self.x = x + self.y = y + self.rz = rz + self.w = w + self.l = l + self.vel_x = vel_x + self.vel_y = vel_y + + +class FusedBbox: + def __init__(self, + bbox_3d: Optional[Bbox3d] = None, + bbox_2d: Optional[Bbox2d] = None, + bbox_radar: Optional[BboxRadar] = None): + """ + bbox_3d: Optional 3d bounding box + bbox_2d: Optional 2d bounding box + bbox_radar: Optional bounding box from radar detections + """ + self.bbox_3d = bbox_3d + self.bbox_2d = bbox_2d + self.bbox_radar = bbox_radar diff --git a/src/perception/tracking/tracking/core/covariance.py b/src/perception/tracking/tracking/core/covariance.py new file mode 100644 index 00000000..ae1f7e73 --- /dev/null +++ b/src/perception/tracking/tracking/core/covariance.py @@ -0,0 +1,123 @@ +import numpy as np +from .utils.config import cfg + +class BaselineCovariance(object): + ''' + Define different Kalman Filter covariance matrix as in AB3DMOT + Kalman Filter states: + [x, y, z, rot_y, l, w, h, x_dot, y_dot, z_dot] + ''' + def __init__(self, **kwargs): + self.num_states = 10 + self.num_observations = 7 + self.P = np.eye(self.num_states) + self.Q = np.eye(self.num_states) + self.R = np.eye(self.num_observations) + + self.P[self.num_observations:, self.num_observations:] *= 1000. + self.P *= 10. + self.Q[self.num_observations:, self.num_observations:] *= 0.01 + + +class KittiCovariance(object): + ''' + Define different Kalman Filter covariance matrix based on Kitti data + Kalman Filter states: + [x, y, z, rot_y, l, w, h, x_dot, y_dot, z_dot] + ''' + def __init__(self, **kwargs): + + self.num_states = 10 + self.num_observations = 7 + self.P = np.eye(self.num_states) + self.Q = np.eye(self.num_states) + self.R = np.eye(self.num_observations) + + # from kitti stats + self.P[0,0] = 0.01969623 + self.P[1,1] = 0.01179107 + self.P[2,2] = 0.04189842 + self.P[3,3] = 0.52534431 + self.P[4,4] = 0.11816206 + self.P[5,5] = 0.00983173 + self.P[6,6] = 0.01602004 + self.P[7,7] = 0.01334779 + self.P[8,8] = 0.00389245 + self.P[9,9] = 0.01837525 + + self.Q[0,0] = 2.94827444e-03 + self.Q[1,1] = 2.18784125e-03 + self.Q[2,2] = 6.85044585e-03 + self.Q[3,3] = 1.10964054e-01 + self.Q[4,4] = 0 + self.Q[5,5] = 0 + self.Q[6,6] = 0 + self.Q[7,7] = 2.94827444e-03 + self.Q[8,8] = 2.18784125e-03 + self.Q[9,9] = 6.85044585e-03 + + self.R[0,0] = 0.01969623 + self.R[1,1] = 0.01179107 + self.R[2,2] = 0.04189842 + self.R[3,3] = 0.52534431 + self.R[4,4] = 0.11816206 + self.R[5,5] = 0.00983173 + self.R[6,6] = 0.01602004 + + +class NuScenesCovariance(object): + ''' + Define different Kalman Filter covariance matrix based on NuScenes data + This includes angular velocity + Kalman Filter states: + [x, y, z, rot_y, l, w, h, x_dot, y_dot, z_dot] + ''' + def __init__(self, **kwargs): + + self.num_states = 10 + self.num_observations = 7 + self.P = np.eye(self.num_states) + self.Q = np.eye(self.num_states) + self.R = np.eye(self.num_observations) + + # nuscenes + # see get_nuscenes_stats.py for the details on how the numbers come from + #Kalman Filter state: [x, y, z, rot_z, l, w, h, x_dot, y_dot, z_dot, rot_z_dot] + + P = { + 'bicycle': [0.05390982, 0.05039431, 0.01863044, 1.29464435, 0.02713823, 0.01169572, 0.01295084, 0.04560422, 0.04097244, 0.01725477, 1.21635902], + 'bus': [0.17546469, 0.13818929, 0.05947248, 0.1979503 , 0.78867322, 0.05507407, 0.06684149, 0.13263319, 0.11508148, 0.05033665, 0.22529652], + 'car': [0.08900372, 0.09412005, 0.03265469, 1.00535696, 0.10912802, 0.02359175, 0.02455134, 0.08120681, 0.08224643, 0.02266425, 0.99492726], + 'motorcycle': [0.04052819, 0.0398904 , 0.01511711, 1.06442726, 0.03291016, 0.00957574, 0.0111605 , 0.0437039 , 0.04327734, 0.01465631, 1.30414345], + 'pedestrian': [0.03855275, 0.0377111 , 0.02482115, 2.0751833 , 0.02286483, 0.0136347 , 0.0203149 , 0.04237008, 0.04092393, 0.01482923, 2.0059979 ], + 'trailer': [0.23228021, 0.22229261, 0.07006275, 1.05163481, 1.37451601, 0.06354783, 0.10500918, 0.2138643 , 0.19625241, 0.05231335, 0.97082174], + 'truck': [0.14862173, 0.1444596 , 0.05417157, 0.73122169, 0.69387238, 0.05484365, 0.07748085, 0.10683797, 0.10248689, 0.0378078 , 0.76188901] + } + + Q = { + 'bicycle': [1.98881347e-02, 1.36552276e-02, 5.10175742e-03, 1.33430252e-01, 0, 0, 0, 1.98881347e-02, 1.36552276e-02, 5.10175742e-03, 1.33430252e-01], + 'bus': [1.17729925e-01, 8.84659079e-02, 1.17616440e-02, 2.09050032e-01, 0, 0, 0, 1.17729925e-01, 8.84659079e-02, 1.17616440e-02, 2.09050032e-01], + 'car': [1.58918523e-01, 1.24935318e-01, 5.35573165e-03, 9.22800791e-02, 0, 0, 0, 1.58918523e-01, 1.24935318e-01, 5.35573165e-03, 9.22800791e-02], + 'motorcycle': [3.23647590e-02, 3.86650974e-02, 5.47421635e-03, 2.34967407e-01, 0, 0, 0, 3.23647590e-02, 3.86650974e-02, 5.47421635e-03, 2.34967407e-01], + 'pedestrian': [3.34814566e-02, 2.47354921e-02, 5.94592529e-03, 4.24962535e-01, 0, 0, 0, 3.34814566e-02, 2.47354921e-02, 5.94592529e-03, 4.24962535e-01], + 'trailer': [4.19985099e-02, 3.68661552e-02, 1.19415050e-02, 5.63166240e-02, 0, 0, 0, 4.19985099e-02, 3.68661552e-02, 1.19415050e-02, 5.63166240e-02], + 'truck': [9.45275998e-02, 9.45620374e-02, 8.38061721e-03, 1.41680460e-01, 0, 0, 0, 9.45275998e-02, 9.45620374e-02, 8.38061721e-03, 1.41680460e-01] + } + + R = { + 'bicycle': [0.05390982, 0.05039431, 0.01863044, 1.29464435, 0.02713823, 0.01169572, 0.01295084], + 'bus': [0.17546469, 0.13818929, 0.05947248, 0.1979503 , 0.78867322, 0.05507407, 0.06684149], + 'car': [0.08900372, 0.09412005, 0.03265469, 1.00535696, 0.10912802, 0.02359175, 0.02455134], + 'motorcycle': [0.04052819, 0.0398904 , 0.01511711, 1.06442726, 0.03291016, 0.00957574, 0.0111605 ], + 'pedestrian': [0.03855275, 0.0377111 , 0.02482115, 2.0751833 , 0.02286483, 0.0136347 , 0.0203149 ], + 'trailer': [0.23228021, 0.22229261, 0.07006275, 1.05163481, 1.37451601, 0.06354783, 0.10500918], + 'truck': [0.14862173, 0.1444596 , 0.05417157, 0.73122169, 0.69387238, 0.05484365, 0.07748085] + } + + self.P = np.diag(P[kwargs['tracking_name']]) + self.Q = np.diag(Q[kwargs['tracking_name']]) + self.R = np.diag(R[kwargs['tracking_name']]) + + if not cfg.TRACKER.USE_ANGULAR_VELOCITY: + self.P = self.P[:-1,:-1] + self.Q = self.Q[:-1,:-1] diff --git a/src/perception/tracking/tracking/core/utils/__init__.py b/src/perception/tracking/tracking/core/utils/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/perception/tracking/tracking/core/utils/config.py b/src/perception/tracking/tracking/core/utils/config.py new file mode 100644 index 00000000..fed81e09 --- /dev/null +++ b/src/perception/tracking/tracking/core/utils/config.py @@ -0,0 +1,26 @@ +from easydict import EasyDict +from pathlib import Path +import yaml + +def log_config_to_file(cfg, pre='cfg', logger=None): + for key, val in cfg.items(): + if isinstance(cfg[key], EasyDict): + logger.info('\n%s.%s = edict()' % (pre, key)) + log_config_to_file(cfg[key], pre=pre + '.' + key, logger=logger) + continue + logger.info('%s.%s: %s' % (pre, key, val)) + +def cfg_from_yaml_file(cfg_file, config): + with open(cfg_file, 'r') as f: + try: + new_config = yaml.load(f, Loader=yaml.FullLoader) + except: + new_config = yaml.load(f) + config.update(EasyDict(new_config)) + + return config + + +cfg = EasyDict() +cfg.ROOT_DIR = (Path(__file__).resolve().parent / '../').resolve() + diff --git a/src/perception/tracking/tracking/core/utils/geometry_utils.py b/src/perception/tracking/tracking/core/utils/geometry_utils.py new file mode 100644 index 00000000..62a5da9c --- /dev/null +++ b/src/perception/tracking/tracking/core/utils/geometry_utils.py @@ -0,0 +1,254 @@ +from numba import jit +from scipy.spatial import ConvexHull +import copy +import numpy as np +import tf_transformations as tr + +@jit +def poly_area(x,y): + """ Compute area of a polygon + x: np.array of x coordinates + y: np.array of y coordinates + """ + # https://stackoverflow.com/a/49129646 + correction = x[-1] * y[0] - y[-1] * x[0] + main_area = np.dot(x[:-1], y[1:]) - np.dot(y[:-1], x[1:]) + return 0.5*np.abs(main_area + correction) + +@jit +def box3d_vol(corners): + """ Compute the volume of a 3d bounding box from it's corners + corners: (8,3) no assumption on axis direction + """ + a = np.sqrt(np.sum((corners[0,:] - corners[1,:])**2)) + b = np.sqrt(np.sum((corners[1,:] - corners[2,:])**2)) + c = np.sqrt(np.sum((corners[0,:] - corners[4,:])**2)) + return a*b*c + +def convex_hull_intersection(p1, p2): + """ Compute area of two convex hull's intersection area. + p1,p2 are a list of (x,y) tuples of hull vertices. + return a list of (x,y) for the intersection and its volume + """ + inter_p = polygon_clip(p1,p2) + if inter_p is not None: + hull_inter = ConvexHull(inter_p) + return inter_p, hull_inter.volume + else: + return None, 0.0 + +def polygon_clip(subjectPolygon, clipPolygon): + """ Clip a polygon with another polygon. + Args: + subjectPolygon: a list of (x,y) 2d points, any polygon. + clipPolygon: a list of (x,y) 2d points, has to be *convex* + Note: + **points have to be counter-clockwise ordered** + + Return: + a list of (x,y) vertex point for the intersection polygon. + """ + def inside(p): + return(cp2[0]-cp1[0])*(p[1]-cp1[1]) > (cp2[1]-cp1[1])*(p[0]-cp1[0]) + + def computeIntersection(): + dc = [ cp1[0] - cp2[0], cp1[1] - cp2[1] ] + dp = [ s[0] - e[0], s[1] - e[1] ] + n1 = cp1[0] * cp2[1] - cp1[1] * cp2[0] + n2 = s[0] * e[1] - s[1] * e[0] + n3 = 1.0 / (dc[0] * dp[1] - dc[1] * dp[0]) + return [(n1*dp[0] - n2*dc[0]) * n3, (n1*dp[1] - n2*dc[1]) * n3] + + outputList = subjectPolygon + cp1 = clipPolygon[-1] + + for clipVertex in clipPolygon: + cp2 = clipVertex + inputList = outputList + outputList = [] + s = inputList[-1] + + for subjectVertex in inputList: + e = subjectVertex + if inside(e): + if not inside(s): + outputList.append(computeIntersection()) + outputList.append(e) + elif inside(s): + outputList.append(computeIntersection()) + s = e + cp1 = cp2 + if len(outputList) == 0: + return None + return(outputList) + +def iou3d(corners1, corners2): + ''' Compute 3D bounding box IoU. + + Input: + corners1: numpy array (8,3), assume up direction is Z + corners2: numpy array (8,3), assume up direction is Z + Output: + iou: 3D bounding box IoU + iou_2d: bird's eye view 2D bounding box IoU + + ''' + # corner points are in counter clockwise order + rect1 = [(corners1[i,0], corners1[i,1]) for i in range(3,-1,-1)] + rect2 = [(corners2[i,0], corners2[i,1]) for i in range(3,-1,-1)] + area1 = poly_area(np.array(rect1)[:,0], np.array(rect1)[:,1]) + area2 = poly_area(np.array(rect2)[:,0], np.array(rect2)[:,1]) + inter, inter_area = convex_hull_intersection(rect1, rect2) + iou_2d = inter_area/(area1+area2-inter_area) + zmax = min(corners1[0,2], corners2[0,2]) + zmin = max(corners1[4,2], corners2[4,2]) + inter_vol = inter_area * max(0.0, zmax-zmin) + vol1 = box3d_vol(corners1) + vol2 = box3d_vol(corners2) + iou = inter_vol / (vol1 + vol2 - inter_vol) + return iou, iou_2d + +@jit +def roty(t): + ''' Rotation about the y-axis. ''' + c = np.cos(t) + s = np.sin(t) + return np.array([[c, 0.0, s], + [0.0,1.0,0.0], + [-s, 0.0, c]]) + +@jit +def rotz(t): + ''' Rotation about the z-axis. ''' + c = np.cos(t) + s = np.sin(t) + return np.array([[c, -s, 0.0], + [s, c, 0.0], + [0.0,0.0,1.0]]) + + +def convert_3dbox_to_8corner(bbox3d_input): + ''' Takes an object and a projection matrix (P) and projects the 3d + bounding box into the image plane. + Returns: + corners_2d: (8,2) array in left image coord. + corners_3d: (8,3) array in in rect camera coord. + Note: the output of this function will be passed to the funciton iou3d + for calculating the 3D-IOU. But the function iou3d was written for + kitti, so the caller needs to set nuscenes_to_kitti to True if + the input bbox3d_input is in nuscenes format. + ''' + # compute rotational matrix around yaw axis + bbox3d = copy.copy(bbox3d_input) + + R = rotz(bbox3d[3]) + + # 3d bounding box dimensions + w = bbox3d[4] + l = bbox3d[5] + h = bbox3d[6] + + # 3d bounding box corners + x_corners = [w/2,w/2,-w/2,-w/2,w/2,w/2,-w/2,-w/2] + y_corners = [l/2,-l/2,-l/2,l/2,l/2,-l/2,-l/2,l/2] + z_corners = [h/2,h/2,h/2,h/2,-h/2,-h/2,-h/2,-h/2] + + # rotate and translate 3d bounding box + corners_3d = np.dot(R, np.vstack([x_corners,y_corners,z_corners])) + corners_3d[0,:] = corners_3d[0,:] + bbox3d[0] + corners_3d[1,:] = corners_3d[1,:] + bbox3d[1] + corners_3d[2,:] = corners_3d[2,:] + bbox3d[2] + + return np.transpose(corners_3d) + +def angle_in_range(angle): + ''' + Input angle: -2pi ~ 2pi + Output angle: -pi ~ pi + ''' + if angle > np.pi: + angle -= 2 * np.pi + if angle < -np.pi: + angle += 2 * np.pi + return angle + +def diff_orientation_correction(det, trk): + ''' + return the angle diff = det - trk + if angle diff > 90 or < -90, rotate trk and update the angle diff + ''' + diff = det - trk + diff = angle_in_range(diff) + if diff > np.pi / 2: + diff -= np.pi + if diff < -np.pi / 2: + diff += np.pi + diff = angle_in_range(diff) + return diff + + +class ClippingError(Exception): + pass + +def project_bbox_2d(bbox, to_camera_transform, intrinsic_matrix): + """ + Inputs: + - bbox: [x y z rz w l h] + - to_camera_transform: geometry_msgs/Transform + Outputs: + - bbox_2d: [x1 y1 w l] top left corner coordinates + width (x) and height (y) + """ + points = convert_3dbox_to_8corner(bbox) # Convert to tuple of 8 corners [[xyz], [xyz], ...] + points = transform_points(points, to_camera_transform) # Transform corners to camera frame + # print("Camera", points) + if (np.any(points[:, 2] <= 0)): # Reject boxes behind the camera + raise ClippingError() + + points_cam = np.dot(intrinsic_matrix, points.T).T # Project viewing frustum to 2d + points_cam_norm = points_cam[:,:2] / points_cam[:,2].reshape(-1, 1) # Normalize by z coordinate + + min_xy = np.min(points_cam_norm, axis=0) + max_xy = np.max(points_cam_norm, axis=0) + dimensions = max_xy - min_xy + if (np.any(dimensions == 0) or np.any(points_cam[:,2] > 100)): # Clip boxes outside the camera image + raise ClippingError() + + return np.hstack((min_xy, dimensions)) + + +def transform_points(points, transform): + """ + Transforms a series of points based on a transformationStamped + points: [Nx3] Array of points xyz + transform: Transform ROS message, eg StampedTransform.Transform + """ + if len(points) == 0: + return points + + translation = tr.translation_matrix([transform.translation.x, transform.translation.y, transform.translation.z]) + rotation = tr.quaternion_matrix([transform.rotation.x, transform.rotation.y, + transform.rotation.z, transform.rotation.w]) + combined_hom_transform = np.dot(translation, rotation) + + points_hom = np.hstack((points[:,:3], np.ones((points.shape[0], 1), dtype=np.float32))) + points_transformed = np.dot(combined_hom_transform, points_hom.T).T + + points[:,:3] = points_transformed[:,:3] + + return points + +def transform_boxes(boxes, transform): + """ + Boxes: N * [x y z rz w l h] + Transform: Transformation, ie TransformStamped.Transform + """ + if len(boxes) == 0: + return boxes + + boxes = transform_points(boxes, transform) + + tf_yaw = tr.euler_from_quaternion([transform.rotation.x, transform.rotation.y, + transform.rotation.z, transform.rotation.w])[2] + boxes[:,3] += tf_yaw + + return boxes diff --git a/src/perception/tracking/tracking/core/utils/ros_utils.py b/src/perception/tracking/tracking/core/utils/ros_utils.py new file mode 100644 index 00000000..9d43ef18 --- /dev/null +++ b/src/perception/tracking/tracking/core/utils/ros_utils.py @@ -0,0 +1,85 @@ +#! /usr/bin/env python +import numpy as np +import tf_transformations as tr +import copy + +from tracking_msgs.msg import TrackedDetection3D + + + +def yaw_from_quaternion_msg(quaterion): + (r, p, y) = tr.euler_from_quaternion([quaterion.x, quaterion.y, quaterion.z, quaterion.w]) + return y + +def obstacle_to_bbox(obstacle): + x = obstacle.pose.pose.position.x + y = obstacle.pose.pose.position.y + z = obstacle.pose.pose.position.z + rz = yaw_from_quaternion_msg(obstacle.pose.pose.orientation) + w = obstacle.width_along_x_axis + l = obstacle.height_along_y_axis + h = obstacle.depth_along_z_axis + return [x, y, z, rz, w, l, h] + +def bbox_to_obstacle(bbox, unique_id, label): + # bbox: [x, y, z, rot_y, l, w, h, x_dot, y_dot, z_dot, rot_y_dot?] rot_y_dot is optional + + obstacle = TrackedDetection3D() + obstacle.label = label + obstacle.object_id = unique_id + + obstacle.pose.pose.position.x = bbox[0] + obstacle.pose.pose.position.y = bbox[1] + obstacle.pose.pose.position.z = bbox[2] + + quaternion = tr.quaternion_from_euler(0, 0, bbox[3]) + obstacle.pose.pose.orientation.x = quaternion[0] + obstacle.pose.pose.orientation.y = quaternion[1] + obstacle.pose.pose.orientation.z = quaternion[2] + obstacle.pose.pose.orientation.w = quaternion[3] + + obstacle.width_along_x_axis = bbox[4] + obstacle.height_along_y_axis = bbox[5] + obstacle.depth_along_z_axis = bbox[6] + + obstacle.twist.twist.linear.x = bbox[7] + obstacle.twist.twist.linear.y = bbox[8] + obstacle.twist.twist.linear.z = bbox[9] + if len(bbox) > 10: + obstacle.twist.twist.angular = [0, 0, bbox[10]] + + return obstacle + + +def bbox_to_traffic_sign(bbox, unique_id, label): + + # traffic_sign_message = TrafficSignMsg() + # traffic_sign_message.id = unique_id + # traffic_sign_message.traffic_sign_type = label + + # traffic_sign_message.pose.position.x = bbox[0] + # traffic_sign_message.pose.position.y = bbox[1] + # traffic_sign_message.pose.position.z = bbox[2] + + # quaternion = tf.transformations.quaternion_from_euler(0, 0, bbox[3]) + # traffic_sign_message.pose.orientation.x = quaternion[0] + # traffic_sign_message.pose.orientation.y = quaternion[1] + # traffic_sign_message.pose.orientation.z = quaternion[2] + # traffic_sign_message.pose.orientation.w = quaternion[3] + + # traffic_sign_message.dimensions.x = bbox[4] + # traffic_sign_message.dimensions.y = bbox[5] + # traffic_sign_message.dimensions.z = bbox[6] + + # return traffic_sign_message + return + +def traffic_sign_to_bbox(msg): + x = msg.pose.position.x + y = msg.pose.position.y + z = msg.pose.position.z + rz = yaw_from_quaternion_msg(msg.pose.orientation) + w = msg.dimensions.x + l = msg.dimensions.y + h = msg.dimensions.z + return [x, y, z, rz, w, l, h] diff --git a/src/perception/tracking/tracking/tracker.py b/src/perception/tracking/tracking/tracker.py new file mode 100644 index 00000000..3aaf0edf --- /dev/null +++ b/src/perception/tracking/tracking/tracker.py @@ -0,0 +1,382 @@ +#!/usr/bin/python3 +import time +import numpy as np + +from vision_msgs.msg import Detection3DArray +from tracking_msgs.msg import TrackedDetection3D, TrackedDetection3DArray + +from .core.ab3dmot import AB3DMOT +from .core.utils import ros_utils +from .core.utils import geometry_utils +from .core.utils.config import cfg, cfg_from_yaml_file, log_config_to_file + +import rclpy +from rclpy.node import Node + +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener + +import copy + +class TrackerNode(Node): + def __init__(self): + + super().__init__("tracker_node") + self.get_logger().info("Creating object tracker node...") + + self.declare_parameter("camera_detections_topic", "/augmented_camera_detections") + self.declare_parameter("lidar_detections_topic", "/lidar_detections") + self.declare_parameter("tracked_detections_topic", "/tracked_detections") + self.declare_parameter("velocity_filter_constant", 5) + self.declare_parameter("velocity_prediction_horizon", 0.5) + self.declare_parameter("prediction_resolution", 0.5) + self.declare_parameter("prediction_length", 4.0) + self.declare_parameter("max_lidar_box_dim", 2) + self.declare_parameter("min_lidar_box_dim", 0.5) + self.declare_parameter("lidar_distance_threshold", 1) + self.declare_parameter("default_distance_threshold", 3) + self.declare_parameter("max_age", 5) + self.declare_parameter("min_hits", 3) + self.declare_parameter("reference_frame", "odom") + self.declare_parameter("~traffic_sign_class_names") + self.declare_parameter("~obstacle_class_names") + self.declare_parameter("frequency", 10) + + self.declare_parameter("config_path", "/home/bolty/ament_ws/src/tracking/config/mahalanobis.yaml") + + + self.camera_detections_topic = self.get_parameter("camera_detections_topic").value + self.lidar_detections_topic = self.get_parameter("lidar_detections_topic").value + self.tracked_detections_topic = self.get_parameter("tracked_detections_topic").value + + self.velocity_prediction_horizon = self.get_parameter("velocity_prediction_horizon").value + self.prediction_resolution = self.get_parameter("prediction_resolution").value + self.prediction_length = self.get_parameter("prediction_length").value + self.max_lidar_box_dim = self.get_parameter("max_lidar_box_dim").value + self.min_lidar_box_dim = self.get_parameter("min_lidar_box_dim").value + self.lidar_distance_threshold = self.get_parameter("lidar_distance_threshold").value + self.default_distance_threshold = self.get_parameter("default_distance_threshold").value + self.max_age = self.get_parameter("max_age").value + self.min_hits = self.get_parameter("min_hits").value + self.reference_frame = self.get_parameter("reference_frame").value + cfg.TRAFFIC_SIGN_CLASSES = self.get_parameter("~traffic_sign_class_names").value + cfg.OBSTACLE_CLASSES = self.get_parameter("~obstacle_class_names").value + self.config_path = self.get_parameter("config_path").value + self.frequency = self.get_parameter("frequency").value + self.dt = 1.0 / self.frequency + + cfg_from_yaml_file(self.config_path, cfg) + + + # Initialize Tracker + self.mot_tracker = AB3DMOT(max_age=self.max_age, # max age in seconds of a track before deletion + min_hits=self.min_hits, # min number of detections before publishing + tracking_name="N/A") # default tracking age + + # Velocities for each track + self.velocites = {} + # low pass filter constant for velocity + self.velocity_filter_constant = self.get_parameter("velocity_filter_constant") + + # tf2 listener + self.tf2_buffer = Buffer() + self.tf2_listener = TransformListener(self.tf2_buffer, self) + + # Subscribers / Publishers + self.obstacles_sub = self.create_subscription( + Detection3DArray, self.camera_detections_topic, self.obstacle_callback, 10) + + self.lidar_obstacles_sub = self.create_subscription( + Detection3DArray, self.lidar_detections_topic, self.lidar_obstacle_callback, 10) + + self.tracked_obstacles_publisher = self.create_publisher( + TrackedDetection3DArray, self.tracked_detections_topic, 10) + + self.tracked_obstacles_timer = self.create_timer(0.1, self.publish_tracks, 10) + + # tracked_signs_topic = '/tracked_signs' + # self.tracked_signs_publisher = self.create_publisher( + # tracked_signs_topic, TrafficSignListMsg, queue_size=10) + + def reset(self): + self.mot_tracker = AB3DMOT(max_age=self.max_age, # max age in seconds of a track before deletion + min_hits=self.min_hits, # min number of detections before publishing + tracking_name="N/A") # default tracking age + + def project_objects_2d(self, obstacles, to_camera_transform, intrinsic_name="/camera/right/intrinsic"): + """ + Inputs: + - obstacles: List[Obstacle], describing 3D cuboids + Outputs: + - obstacles_2d: List[Obstacle], describing 2D bounding boxes + """ + if not obstacles: + return [] + + # Retrieve intrinsic matrix + try: + intrinsic_matrix = self.get_parameter(intrinsic_name) + except: + self.get_logger().error("Can't find intrinsic matrix: {}".format(intrinsic_name)) + return [] + intrinsic_matrix = np.array(intrinsic_matrix.split(','), dtype=np.float64).reshape(3, -1) + + # Project obstacles to 2d + obstacles_2d = [] + for obstacle in obstacles: + bbox = ros_utils.obstacle_to_bbox(obstacle) + try: + bbox_2d = geometry_utils.project_bbox_2d(bbox, to_camera_transform, intrinsic_matrix) + except geometry_utils.ClippingError: + continue + obstacle_2d = copy.deepcopy(obstacle) + obstacle_2d.pose.pose.position.x = bbox_2d[0] + obstacle_2d.pose.pose.position.y = bbox_2d[1] + obstacle_2d.pose.pose.position.z = 0 + obstacle_2d.width_along_x_axis = bbox_2d[2] + obstacle_2d.height_along_y_axis = bbox_2d[3] + obstacle_2d.depth_along_z_axis = 0 + obstacles_2d.append(obstacle_2d) + return obstacles_2d + + def track(self, detections, informations, frame_id, timestamp, update_only=False, distance_threshold=None): + """ + Update all trackers. + Input: + detections: [[x,y,z,rz,w,l,h]...] list of list of doubles in the Odom frame + informations: [[String, Double]...] list of list of class labels with confidence + frame_id: String, frame name + timstamp: rclpy.time detections timestamp + update_only: Bool, only update tracks don't create new ones + distance_threshold: euclidian distance threshold for association + """ + if distance_threshold == None: + distance_threshold = self.default_distance_threshold + +# frame_id = "base_link" + detections = np.array(detections) + infos = np.array(informations, dtype=object) + + if (frame_id != self.reference_frame and frame_id != ""): + try: + bbox_to_reference_transform = self.tf2_buffer.lookup_transform(self.reference_frame, frame_id, rclpy.time.Time(), rclpy.time.Duration(0.1)).transform + except: + self.get_logger().error("Failed to look up transform from {} to {}".format(self.reference_frame, frame_id)) + return + + detections = geometry_utils.transform_boxes(detections, bbox_to_reference_transform) + + self.mot_tracker.update(detections, infos, timestamp.to_sec(), update_only, distance_threshold) + + def obstacle_callback(self, detections_msg): + start_time = time.time() + + timestamp = detections_msg.header.stamp + frame_id = detections_msg.header.frame_id + + detections = [] # [x, y, z, rot_y, l, w, h] + informations = [] # [class label, confidence score] + + for detection in detections_msg.detections: + detections.append(ros_utils.obstacle_to_bbox(detection.bbox)) + informations.append([detection.results[0].hypothesis.class_id, detection.results[0].hypothesis.score]) + + self.track(detections, informations, frame_id, timestamp) + + # self.get_logger().info("Obstacle Update Time: {}".format(time.time() - start_time)) + + def traffic_sign_callback(self, sign_list_msg): + + start_time = time.time() + + timestamp = sign_list_msg.header.stamp + frame_id = sign_list_msg.header.frame_id + + detections = [] # [x, y, z, rot_y, l, w, h] + informations = [] # [class label, confidence score] + + for sign in sign_list_msg.traffic_signs: + detections.append(ros_utils.traffic_sign_to_bbox(sign)) + informations.append([sign.traffic_sign_type, sign.confidence]) + + self.track(detections, informations, frame_id, timestamp) + + # self.get_logger().info("Traffic Sign Update Time: {}".format(time.time() - start_time)) + + def lidar_obstacle_callback(self, detections_msg): + + start_time = time.time() + + timestamp = detections_msg.header.stamp + frame_id = detections_msg.header.frame_id + + detections = [] # [x, y, z, rot_y, l, w, h] + informations = [] # [class label, confidence score] + + for box in detections_msg.obstacles: + bbox = ros_utils.obstacle_to_bbox(box) + lwh = np.array(bbox[-3:]) + if (lwh < self.max_lidar_box_dim).all() and (lwh > self.min_lidar_box_dim).any(): # filter out boxes too big or too small + detections.append(bbox) + informations.append(['UNKNOWN', 0.5]) + self.track(detections, informations, frame_id, timestamp, update_only=True, distance_threshold=self.lidar_distance_threshold) + + # self.get_logger().info("Obstacle Update Time: {}".format(time.time() - start_time)) + + def publish_tracks(self): + """ + Publishes the tracks as obj_tracked. Also publishes the node status message + dt: time since last update (in seconds) + """ + tracked_obstacle_list = TrackedDetection3DArray() + # tracked_obstacle_list.header.frame_id = self.reference_frame + # tracked_obstacle_list.header.stamp = rclpy.time.Time() + # tracked_obstacle_list.tracked_obstacles = [] + + # traffic_sign_list = TrafficSignListMsg() + # traffic_sign_list.header.frame_id = self.reference_frame + # traffic_sign_list.header.stamp = rclpy.time.Time() + # traffic_sign_list.traffic_signs = [] + + # for kf_track in self.mot_tracker.trackers: + # tracking_name = kf_track.tracking_name + # if tracking_name in self.traffic_sign_classes: + # traffic_sign_message = self._create_traffic_sign_message(kf_track, tracking_name) + # traffic_sign_list.traffic_signs.append(traffic_sign_message) + # else: + # x = kf_track.get_state() + # tracked_obstacle_message = self._create_tracked_obstacle_message(kf_track, tracking_name, self.dt) + # tracked_obstacle_list.tracked_obstacles.append(tracked_obstacle_message) + # self.tracked_obstacles_publisher.publish(tracked_obstacle_list) + # self.tracked_signs_publisher.publish(traffic_sign_list) + +# self.get_logger().info("Pub Time: {}".format(time.time() - start_time)) + + def _create_traffic_sign_message(self, kf_track, tracking_name): + # [x, y, z, rot_y, l, w, h, x_dot, y_dot, z_dot, rot_y_dot] + bbox = kf_track.get_state() + + traffic_sign_message = ros_utils.bbox_to_traffic_sign(bbox, kf_track.id, tracking_name) + traffic_sign_message.header.frame_id = self.reference_frame + + return traffic_sign_message + + def _create_tracked_obstacle_message(self, kf_track, tracking_name, dt): + """ + Helper that creates the TrackedObstacle message from KalmanBoxTracker (python class) + Args: + kf_track: KalmanBoxTracker (Python class) object that represents the track + tracking_name: String + dt: time since last publish (in seconds) + + Returns: TrackedObstacle (anm_msgs) + """ + + tracked_obstacle_message = TrackedDetection3D() + # [x, y, z, rot_y, l, w, h, x_dot, y_dot, z_dot, rot_y_dot] + bbox = kf_track.get_state() + tracked_obstacle_message.obstacle = ros_utils.bbox_to_obstacle(bbox, kf_track.id, tracking_name) + tracked_obstacle_message.obstacle.header.frame_id = self.reference_frame + tracked_obstacle_message.header.frame_id = self.reference_frame + #tracked_obstacle_message.score = kf_track.track_score + + if len(kf_track.history) == 0: + self.get_logger().error("No History for id {}".format(kf_track.id)) + return tracked_obstacle_message + + if kf_track.id not in self.velocites: + self.velocites[kf_track.id] = np.array([0.0, 0.0]) + + # Estimate velocity using KF track history + latest_timestamp = kf_track.history[-1][0] + for timestamp, history in reversed(kf_track.history): + dt = abs(latest_timestamp - timestamp) + if dt >= self.velocity_prediction_horizon: + # Ignore z velocity. + new_velocity = np.array([(bbox[0] - history[0]) / dt, (bbox[1] - history[1]) / dt]) + # Use low pass filter + alpha = dt / self.velocity_filter_constant + self.velocites[kf_track.id] += alpha * (new_velocity - self.velocites[kf_track.id]) + break + + velocity = self.velocites[kf_track.id] + + # Set velocity + tracked_obstacle_message.obstacle.twist.twist.linear.x = velocity[0] + tracked_obstacle_message.obstacle.twist.twist.linear.y = velocity[1] + + # Create observation history messages + for timestamp, history in kf_track.history: + tracked_obstacle_message.observation_history.append(self.create_tracked_object_state_msg(rclpy.time.from_sec(timestamp), history)) + + last_known_obs = tracked_obstacle_message.observation_history[-1] + + # Add the current observation + tracked_obstacle_message.predicted_states.append(last_known_obs) + + current_time = last_known_obs.header.stamp + max_pred_time = current_time + rclpy.time.Duration.from_sec(self.prediction_length) + delta_time = 0 + + # Initialize - first prediction is actual observation + pred_time = current_time + + while (pred_time < max_pred_time): + # self.get_logger().info('dt:{0}'.format(delta_time)) + delta_time += self.prediction_resolution + + pred_time += rclpy.time.Duration.from_sec(self.prediction_resolution) + + pred_obs = TrackedDetection3DArray() + pred_obs.pose.position.x = last_known_obs.pose.position.x + (delta_time)*velocity[0] + pred_obs.pose.position.y = last_known_obs.pose.position.y + (delta_time)*velocity[1] + + # zero velocity on Up direction + pred_obs.pose.position.z = last_known_obs.pose.position.z + + pred_obs.pose.orientation = last_known_obs.pose.orientation + pred_obs.velocity = last_known_obs.velocity + pred_obs.header.stamp = pred_time + pred_obs.header.frame_id = self.reference_frame + + tracked_obstacle_message.predicted_states.append(pred_obs) + + return tracked_obstacle_message + + def create_tracked_object_state_msg(self, timestamp, bbox): + """Helper that creates creates the TrackedObstacleState message from a KF tracked state + Args: + timestamp: ROS timestamp + bbox: [x, y, z, rot_y, l, w, h, x_dot, y_dot, z_dot, rot_y_dot] + Returns: TrackedObstacleState (anm_msgs) + """ + tracked_object_state = TrackedDetection3DArray() + tracked_object_state.header.frame_id = self.reference_frame + tracked_object_state.header.stamp = timestamp + # [x, y, z, rot_y, l, w, h, x_dot, y_dot, z_dot, rot_y_dot] + tracked_object_state.pose.position.x = bbox[0] + tracked_object_state.pose.position.y = bbox[1] + tracked_object_state.pose.position.z = bbox[2] + q = tr.quaternionerror(0, 0, bbox[3]) + tracked_object_state.pose.orientation.x = q[0] + tracked_object_state.pose.orientation.y = q[1] + tracked_object_state.pose.orientation.z = q[2] + tracked_object_state.pose.orientation.w = q[3] + + tracked_object_state.velocity.linear.x = bbox[7] + tracked_object_state.velocity.linear.y = bbox[8] + tracked_object_state.velocity.linear.z = bbox[9] + if (len(bbox) == 11): + tracked_object_state.velocity.angular.z = bbox[10] + + return tracked_object_state + +def main(args=None): + rclpy.init(args=args) + tracker_node = TrackerNode() + rclpy.spin(tracker_node) + tracker_node.destroy() + rclpy.shutdown() + +if __name__ == "__main__": + main() diff --git a/src/wato_msgs/perception_msgs/tracking_msgs/CMakeLists.txt b/src/wato_msgs/perception_msgs/tracking_msgs/CMakeLists.txt new file mode 100644 index 00000000..dec83355 --- /dev/null +++ b/src/wato_msgs/perception_msgs/tracking_msgs/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.10) +project(tracking_msgs) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) +find_package(vision_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +set(msg_files + msg/TrackedDetection3D.msg + msg/TrackedDetection3DArray.msg) + +rosidl_generate_interfaces(tracking_msgs + ${msg_files} + DEPENDENCIES std_msgs vision_msgs geometry_msgs) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/src/wato_msgs/perception_msgs/tracking_msgs/msg/TrackedDetection3D.msg b/src/wato_msgs/perception_msgs/tracking_msgs/msg/TrackedDetection3D.msg new file mode 100644 index 00000000..19de5ca3 --- /dev/null +++ b/src/wato_msgs/perception_msgs/tracking_msgs/msg/TrackedDetection3D.msg @@ -0,0 +1,13 @@ +std_msgs/Header header + +# ID of the detection being tracked +int64 tracked_id +# The bounding box and current position of the detection in question +vision_msgs/Detection3D detection +# Velocity of the detection +geometry_msgs/Twist velocity + +# History of Observations (duration is configurable) The last element in observation_history corresponds to the current state +vision_msgs/Detection3D[] observation_history +# Linear future predictions based on velocity and position +vision_msgs/Detection3D[] predicted_states diff --git a/src/wato_msgs/perception_msgs/tracking_msgs/msg/TrackedDetection3DArray.msg b/src/wato_msgs/perception_msgs/tracking_msgs/msg/TrackedDetection3DArray.msg new file mode 100644 index 00000000..eba649ce --- /dev/null +++ b/src/wato_msgs/perception_msgs/tracking_msgs/msg/TrackedDetection3DArray.msg @@ -0,0 +1,3 @@ +std_msgs/Header header + +tracking_msgs/TrackedDetection3D[] tracked_detections diff --git a/src/wato_msgs/perception_msgs/tracking_msgs/package.xml b/src/wato_msgs/perception_msgs/tracking_msgs/package.xml new file mode 100644 index 00000000..2a6dda1a --- /dev/null +++ b/src/wato_msgs/perception_msgs/tracking_msgs/package.xml @@ -0,0 +1,22 @@ + + + tracking_msgs + 0.0.0 + Tracking ROS2 messages + + Steven Gong + TODO + + ament_cmake + std_msgs + vision_msgs + geometry_msgs + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + +