Merge pull request #1194 from tier4/cherry-pick/taekjin

fix(multi_object_tracker): tracker update for bugfix on unstable motorbike-cyclist-pedetrian tracking result

Author: shmpwk

perception/multi_object_tracker/CMakeLists.txt

@@ -22,6 +22,13 @@ include_directories(
 # Generate exe file
 set(MULTI_OBJECT_TRACKER_SRC
   src/multi_object_tracker_core.cpp
+  src/data_association/data_association.cpp
+  src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp
+  src/tracker/motion_model/motion_model_base.cpp
+  src/tracker/motion_model/bicycle_motion_model.cpp
+  # cspell: ignore ctrv
+  src/tracker/motion_model/ctrv_motion_model.cpp
+  src/tracker/motion_model/cv_motion_model.cpp
   src/tracker/model/tracker_base.cpp
   src/tracker/model/big_vehicle_tracker.cpp
   src/tracker/model/normal_vehicle_tracker.cpp
@@ -31,8 +38,6 @@ set(MULTI_OBJECT_TRACKER_SRC
   src/tracker/model/pedestrian_and_bicycle_tracker.cpp
   src/tracker/model/unknown_tracker.cpp
   src/tracker/model/pass_through_tracker.cpp
-  src/data_association/data_association.cpp
-  src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp
 )
 
 ament_auto_add_library(multi_object_tracker_node SHARED

perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp

@@ -124,7 +124,7 @@ class MultiObjectTracker : public rclcpp::Node
     const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
     const geometry_msgs::msg::Transform & self_transform) const;
 
-  void publish(const rclcpp::Time & time);
+  void publish(const rclcpp::Time & time) const;
   inline bool shouldTrackerPublish(const std::shared_ptr<const Tracker> tracker) const;
 };
 

perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp

@@ -20,6 +20,7 @@
 #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_
 
 #include "multi_object_tracker/tracker/model/tracker_base.hpp"
+#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp"
 
 #include <kalman_filter/kalman_filter.hpp>
 
@@ -30,55 +31,40 @@ class BicycleTracker : public Tracker
   rclcpp::Logger logger_;
 
 private:
-  KalmanFilter ekf_;
-  rclcpp::Time last_update_time_;
-  enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 };
   struct EkfParams
   {
-    char dim_x = 5;
-    float q_stddev_acc_long;
-    float q_stddev_acc_lat;
-    float q_stddev_yaw_rate_min;
-    float q_stddev_yaw_rate_max;
-    float q_cov_slip_rate_min;
-    float q_cov_slip_rate_max;
-    float q_max_slip_angle;
-    float p0_cov_vel;
-    float p0_cov_slip;
-    float r_cov_x;
-    float r_cov_y;
-    float r_cov_yaw;
-    float p0_cov_x;
-    float p0_cov_y;
-    float p0_cov_yaw;
+    double r_cov_x;
+    double r_cov_y;
+    double r_cov_yaw;
   } ekf_params_;
 
-  double max_vel_;
-  double max_slip_;
-  double lf_;
-  double lr_;
-  float z_;
+  double z_;
 
-private:
   struct BoundingBox
   {
     double length;
     double width;
     double height;
   };
   BoundingBox bounding_box_;
-  BoundingBox last_input_bounding_box_;
+
+private:
+  BicycleMotionModel motion_model_;
+  const char DIM = motion_model_.DIM;
+  using IDX = BicycleMotionModel::IDX;
 
 public:
   BicycleTracker(
     const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
     const geometry_msgs::msg::Transform & self_transform);
 
   bool predict(const rclcpp::Time & time) override;
-  bool predict(const double dt, KalmanFilter & ekf) const;
   bool measure(
     const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
     const geometry_msgs::msg::Transform & self_transform) override;
+  autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject(
+    const autoware_auto_perception_msgs::msg::DetectedObject & object,
+    const geometry_msgs::msg::Transform & self_transform);
   bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object);
   bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object);
   bool getTrackedObject(

perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp

@@ -20,47 +20,28 @@
 #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_
 
 #include "multi_object_tracker/tracker/model/tracker_base.hpp"
+#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp"
 
 #include <kalman_filter/kalman_filter.hpp>
+
 class BigVehicleTracker : public Tracker
 {
 private:
   autoware_auto_perception_msgs::msg::DetectedObject object_;
   rclcpp::Logger logger_;
-  int last_nearest_corner_index_;
 
 private:
-  KalmanFilter ekf_;
-  rclcpp::Time last_update_time_;
-  enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 };
   struct EkfParams
   {
-    char dim_x = 5;
-    float q_stddev_acc_long;
-    float q_stddev_acc_lat;
-    float q_stddev_yaw_rate_min;
-    float q_stddev_yaw_rate_max;
-    float q_cov_slip_rate_min;
-    float q_cov_slip_rate_max;
-    float q_max_slip_angle;
-    float p0_cov_vel;
-    float p0_cov_slip;
-    float r_cov_x;
-    float r_cov_y;
-    float r_cov_yaw;
-    float r_cov_vel;
-    float p0_cov_x;
-    float p0_cov_y;
-    float p0_cov_yaw;
+    double r_cov_x;
+    double r_cov_y;
+    double r_cov_yaw;
+    double r_cov_vel;
   } ekf_params_;
-  double max_vel_;
-  double max_slip_;
-  double lf_;
-  double lr_;
-  float z_;
   double velocity_deviation_threshold_;
 
-private:
+  double z_;
+
   struct BoundingBox
   {
     double length;
@@ -70,25 +51,41 @@ class BigVehicleTracker : public Tracker
   BoundingBox bounding_box_;
   BoundingBox last_input_bounding_box_;
   Eigen::Vector2d tracking_offset_;
+  int last_nearest_corner_index_;
+
+private:
+  BicycleMotionModel motion_model_;
+  const char DIM = motion_model_.DIM;
+  using IDX = BicycleMotionModel::IDX;
 
 public:
   BigVehicleTracker(
     const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
     const geometry_msgs::msg::Transform & self_transform);
 
   bool predict(const rclcpp::Time & time) override;
-  bool predict(const double dt, KalmanFilter & ekf) const;
   bool measure(
     const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
     const geometry_msgs::msg::Transform & self_transform) override;
+  autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject(
+    const autoware_auto_perception_msgs::msg::DetectedObject & object,
+    const geometry_msgs::msg::Transform & self_transform);
   bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object);
   bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object);
   bool getTrackedObject(
     const rclcpp::Time & time,
     autoware_auto_perception_msgs::msg::TrackedObject & object) const override;
-  double getMeasurementYaw(const autoware_auto_perception_msgs::msg::DetectedObject & object);
-  void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform);
   virtual ~BigVehicleTracker() {}
+
+private:
+  void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform)
+  {
+    Eigen::MatrixXd X_t(DIM, 1);
+    motion_model_.getStateVector(X_t);
+    last_nearest_corner_index_ = utils::getNearestCornerOrSurface(
+      X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length,
+      self_transform);
+  }
 };
 
 #endif  // MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_

perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp

@@ -19,7 +19,8 @@
 #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_
 #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_
 
-#include "tracker_base.hpp"
+#include "multi_object_tracker/tracker/model/tracker_base.hpp"
+#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp"
 
 #include <kalman_filter/kalman_filter.hpp>
 
@@ -28,42 +29,19 @@ class NormalVehicleTracker : public Tracker
 private:
   autoware_auto_perception_msgs::msg::DetectedObject object_;
   rclcpp::Logger logger_;
-  int last_nearest_corner_index_;
 
 private:
-  KalmanFilter ekf_;
-  rclcpp::Time last_update_time_;
-  enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 };
-
   struct EkfParams
   {
-    char dim_x = 5;
-    float q_stddev_acc_long;
-    float q_stddev_acc_lat;
-    float q_stddev_yaw_rate_min;
-    float q_stddev_yaw_rate_max;
-    float q_cov_slip_rate_min;
-    float q_cov_slip_rate_max;
-    float q_max_slip_angle;
-    float p0_cov_vel;
-    float p0_cov_slip;
-    float r_cov_x;
-    float r_cov_y;
-    float r_cov_yaw;
-    float r_cov_vel;
-    float p0_cov_x;
-    float p0_cov_y;
-    float p0_cov_yaw;
+    double r_cov_x;
+    double r_cov_y;
+    double r_cov_yaw;
+    double r_cov_vel;
   } ekf_params_;
-
-  double max_vel_;
-  double max_slip_;
-  double lf_;
-  double lr_;
-  float z_;
   double velocity_deviation_threshold_;
 
-private:
+  double z_;
+
   struct BoundingBox
   {
     double length;
@@ -73,25 +51,41 @@ class NormalVehicleTracker : public Tracker
   BoundingBox bounding_box_;
   BoundingBox last_input_bounding_box_;
   Eigen::Vector2d tracking_offset_;
+  int last_nearest_corner_index_;
+
+private:
+  BicycleMotionModel motion_model_;
+  const char DIM = motion_model_.DIM;
+  using IDX = BicycleMotionModel::IDX;
 
 public:
   NormalVehicleTracker(
     const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
     const geometry_msgs::msg::Transform & self_transform);
 
   bool predict(const rclcpp::Time & time) override;
-  bool predict(const double dt, KalmanFilter & ekf) const;
   bool measure(
     const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
     const geometry_msgs::msg::Transform & self_transform) override;
+  autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject(
+    const autoware_auto_perception_msgs::msg::DetectedObject & object,
+    const geometry_msgs::msg::Transform & self_transform);
   bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object);
   bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object);
   bool getTrackedObject(
     const rclcpp::Time & time,
     autoware_auto_perception_msgs::msg::TrackedObject & object) const override;
-  void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform);
-  double getMeasurementYaw(const autoware_auto_perception_msgs::msg::DetectedObject & object);
   virtual ~NormalVehicleTracker() {}
+
+private:
+  void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform)
+  {
+    Eigen::MatrixXd X_t(DIM, 1);
+    motion_model_.getStateVector(X_t);
+    last_nearest_corner_index_ = utils::getNearestCornerOrSurface(
+      X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length,
+      self_transform);
+  }
 };
 
 #endif  // MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_

perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp

@@ -20,48 +20,28 @@
 #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_
 
 #include "multi_object_tracker/tracker/model/tracker_base.hpp"
+#include "multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp"
 
 #include <kalman_filter/kalman_filter.hpp>
 
+// cspell: ignore CTRV
+
 class PedestrianTracker : public Tracker
 {
 private:
   autoware_auto_perception_msgs::msg::DetectedObject object_;
   rclcpp::Logger logger_;
 
 private:
-  KalmanFilter ekf_;
-  rclcpp::Time last_update_time_;
-  enum IDX {
-    X = 0,
-    Y = 1,
-    YAW = 2,
-    VX = 3,
-    WZ = 4,
-  };
   struct EkfParams
   {
-    char dim_x = 5;
-    float q_cov_x;
-    float q_cov_y;
-    float q_cov_yaw;
-    float q_cov_wz;
-    float q_cov_vx;
-    float p0_cov_vx;
-    float p0_cov_wz;
-    float r_cov_x;
-    float r_cov_y;
-    float r_cov_yaw;
-    float p0_cov_x;
-    float p0_cov_y;
-    float p0_cov_yaw;
+    double r_cov_x;
+    double r_cov_y;
+    double r_cov_yaw;
   } ekf_params_;
 
-  double max_vx_;
-  double max_wz_;
-  float z_;
+  double z_;
 
-private:
   struct BoundingBox
   {
     double length;
@@ -76,16 +56,23 @@ class PedestrianTracker : public Tracker
   BoundingBox bounding_box_;
   Cylinder cylinder_;
 
+private:
+  CTRVMotionModel motion_model_;
+  const char DIM = motion_model_.DIM;
+  using IDX = CTRVMotionModel::IDX;
+
 public:
   PedestrianTracker(
     const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
     const geometry_msgs::msg::Transform & self_transform);
 
   bool predict(const rclcpp::Time & time) override;
-  bool predict(const double dt, KalmanFilter & ekf) const;
   bool measure(
     const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
     const geometry_msgs::msg::Transform & self_transform) override;
+  autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject(
+    const autoware_auto_perception_msgs::msg::DetectedObject & object,
+    const geometry_msgs::msg::Transform & self_transform);
   bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object);
   bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object);
   bool getTrackedObject(