feat(bytetracker): modify kalman filter model to get better tracking …

…performance (#5639) * wip: try to replace kalman filter statements in bytetrack Signed-off-by: yoshiri <[email protected]> * add config file for kalman filter Signed-off-by: yoshiri <[email protected]> * add actual kalman filter code Signed-off-by: yoshiri <[email protected]> * update readme Signed-off-by: yoshiri <[email protected]> * Update perception/bytetrack/config/bytetracker.param.yaml Co-authored-by: Shunsuke Miura <[email protected]> * docs(bytetrack): Update demo video on README.md Signed-off-by: Manato HIRABAYASHI <[email protected]> --------- Signed-off-by: yoshiri <[email protected]> Signed-off-by: Manato HIRABAYASHI <[email protected]> Co-authored-by: Shunsuke Miura <[email protected]> Co-authored-by: Manato Hirabayashi <[email protected]>
autowarefoundation · Nov 27, 2023 · 577892b · 577892b
1 parent bd4d973
commit 577892b
Show file tree

Hide file tree

Showing 10 changed files with 256 additions and 321 deletions.
diff --git a/perception/bytetrack/CMakeLists.txt b/perception/bytetrack/CMakeLists.txt
@@ -29,7 +29,10 @@ target_include_directories(bytetrack_lib
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/lib/include>
   $<INSTALL_INTERFACE:include>
 )
-target_link_libraries(bytetrack_lib Eigen3::Eigen)
+target_link_libraries(bytetrack_lib
+  Eigen3::Eigen
+  yaml-cpp
+)
 
 #
 # ROS node
@@ -91,4 +94,5 @@ rclcpp_components_register_node(${PROJECT_NAME}_visualizer
 
 ament_auto_package(INSTALL_TO_SHARE
   launch
+  config
 )
diff --git a/perception/bytetrack/README.md b/perception/bytetrack/README.md
@@ -6,7 +6,7 @@ The core algorithm, named `ByteTrack`, mainly aims to perform multi-object track
 Because the algorithm associates almost every detection box including ones with low detection scores,
 the number of false negatives is expected to decrease by using it.
 
-[demo video](https://user-images.githubusercontent.com/3022416/225920856-745a3bb7-6b35-403d-87b0-6e5085952d70.mp4)
+[demo video](https://github.com/YoshiRi/autoware.universe/assets/3022416/40f4c158-657e-48e1-81c2-8ac39152892d)
 
 ## Inner-workings / Algorithms
 
@@ -20,6 +20,16 @@ the number of false negatives is expected to decrease by using it.
 - This package is ported version toward Autoware from [this repository](https://github.com/ifzhang/ByteTrack/tree/main/deploy/TensorRT/cpp)
   (The C++ implementation by the ByteTrack's authors)
 
+### 2d tracking modification from original codes
+
+The paper just says that the 2d tracking algorithm is a simple Kalman filter.
+Original codes use the `top-left-corner` and `aspect ratio` and `size` as the state vector.
+
+This is sometimes unstable because the aspectratio can be changed by the occlusion.
+So, we use the `top-left` and `size` as the state vector.
+
+Kalman filter settings can be controlled by the parameters in `config/bytetrack_node.param.yaml`.
+
 ## Inputs / Outputs
 
 ### bytetrack_node

diff --git a/perception/bytetrack/config/bytetracker.param.yaml b/perception/bytetrack/config/bytetracker.param.yaml
@@ -0,0 +1,14 @@
+# Kalman filter parameters for 2d tracking
+dt: 0.1           # time step[s]
+dim_x: 8          # tlbr + velocity
+dim_z: 4          # tlbr
+q_cov_x: 0.1
+q_cov_y: 0.1
+q_cov_vx: 0.1
+q_cov_vy: 0.1
+r_cov_x: 0.1
+r_cov_y: 0.1
+p0_cov_x: 100.0
+p0_cov_y: 100.0
+p0_cov_vx: 100.0
+p0_cov_vy: 100.0
diff --git a/perception/bytetrack/lib/include/byte_tracker.h b/perception/bytetrack/lib/include/byte_tracker.h
@@ -96,5 +96,5 @@ class ByteTracker
   std::vector<STrack> tracked_stracks;
   std::vector<STrack> lost_stracks;
   std::vector<STrack> removed_stracks;
-  byte_kalman::KalmanFilter kalman_filter;
+  KalmanFilter kalman_filter;
 };
diff --git a/perception/bytetrack/lib/include/kalman_filter.h b/perception/bytetrack/lib/include/kalman_filter.h
diff --git a/perception/bytetrack/lib/include/strack.h b/perception/bytetrack/lib/include/strack.h
@@ -38,25 +38,26 @@
 
 #pragma once
 
-#include "kalman_filter.h"
-
+// #include "kalman_filter.h"
+#include <kalman_filter/kalman_filter.hpp>
 #include <opencv2/opencv.hpp>
 
 #include <boost/uuid/uuid.hpp>
 
+#include <string>
 #include <vector>
 
 enum TrackState { New = 0, Tracked, Lost, Removed };
 
+/** manage one tracklet*/
 class STrack
 {
 public:
   STrack(std::vector<float> tlwh_, float score, int label);
   ~STrack();
 
   std::vector<float> static tlbr_to_tlwh(std::vector<float> & tlbr);
-  static void multi_predict(
-    std::vector<STrack *> & stracks, byte_kalman::KalmanFilter & kalman_filter);
+  static void multi_predict(std::vector<STrack *> & stracks);
   void static_tlwh();
   void static_tlbr();
   std::vector<float> tlwh_to_xyah(std::vector<float> tlwh_tmp);
@@ -65,30 +66,58 @@ class STrack
   void mark_removed();
   int next_id();
   int end_frame();
+  void init_kalman_filter();
+  void update_kalman_filter(const Eigen::MatrixXd & measurement);
+  void reflect_state();
 
-  void activate(byte_kalman::KalmanFilter & kalman_filter, int frame_id);
+  void activate(int frame_id);
   void re_activate(STrack & new_track, int frame_id, bool new_id = false);
   void update(STrack & new_track, int frame_id);
+  void predict(const int frame_id);
+
+  void load_parameters(const std::string & filename);
 
 public:
   bool is_activated;
   int track_id;
   boost::uuids::uuid unique_id;
   int state;
 
-  std::vector<float> _tlwh;
-  std::vector<float> tlwh;
-  std::vector<float> tlbr;
+  std::vector<float> original_tlwh;  // top left width height
+  std::vector<float> tlwh;           // top left width height
+  std::vector<float> tlbr;           // top left bottom right
   int frame_id;
   int tracklet_len;
   int start_frame;
 
-  KAL_MEAN mean;
-  KAL_COVA covariance;
   float score;
 
   int label;
 
 private:
-  byte_kalman::KalmanFilter kalman_filter;
+  KalmanFilter kalman_filter_;
+  struct KfParams
+  {
+    // dimension
+    char dim_x = 8;
+    char dim_z = 4;
+    // system noise
+    float q_cov_x;
+    float q_cov_y;
+    float q_cov_vx;
+    float q_cov_vy;
+    // measurement noise
+    float r_cov_x;
+    float r_cov_y;
+    // initial state covariance
+    float p0_cov_x;
+    float p0_cov_y;
+    float p0_cov_vx;
+    float p0_cov_vy;
+    // other parameters
+    float dt;  // sampling time
+  };
+  static KfParams _kf_parameters;
+  static bool _parameters_loaded;
+  enum IDX { X1 = 0, Y1 = 1, X2 = 2, Y2 = 3, VX1 = 4, VY1 = 5, VX2 = 6, VY2 = 7 };
 };
diff --git a/perception/bytetrack/lib/src/byte_tracker.cpp b/perception/bytetrack/lib/src/byte_tracker.cpp
@@ -106,7 +106,10 @@ std::vector<STrack> ByteTracker::update(const std::vector<ByteTrackObject> & obj
 
   ////////////////// Step 2: First association, with IoU //////////////////
   strack_pool = joint_stracks(tracked_stracks, this->lost_stracks);
-  STrack::multi_predict(strack_pool, this->kalman_filter);
+  // do prediction for each stracks
+  for (size_t i = 0; i < strack_pool.size(); i++) {
+    strack_pool[i]->predict(this->frame_id);
+  }
 
   std::vector<std::vector<float> > dists;
   int dist_size = 0, dist_size_size = 0;
@@ -196,7 +199,7 @@ std::vector<STrack> ByteTracker::update(const std::vector<ByteTrackObject> & obj
   for (size_t i = 0; i < u_detection.size(); i++) {
     STrack * track = &detections[u_detection[i]];
     if (track->score < this->high_thresh) continue;
-    track->activate(this->kalman_filter, this->frame_id);
+    track->activate(this->frame_id);
     activated_stracks.push_back(*track);
   }