feat(multi_object_tracker): tracker refactoring (#7271)

* feat: separate filters Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: object validator to modular Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: replace xyzrpy covariance index Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: refactor tracker class members Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: refactor Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: refactoring destructor Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: refactoring getUpdatingObject Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: refactoring object size checker Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: debugger delay calculation order is fixed Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: bicycle size update bug fix Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: renamed message package Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: refactoring Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * feat: object parameter class Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * style(pre-commit): autofix Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: refactor includes Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * feat: initial impl. of object model Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * feat: impl. object model to bicycle, big, normal Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * feat: impl. object model to pedestrian Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * feat: impl. cont Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * style(pre-commit): autofix Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: fix missing refactoring Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: align unit of angles, yaw rates Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: rename tier4_autoware_utils to autoware_universe_utils Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: missing unit conversion Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * style(pre-commit): autofix Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: sources Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: Update include and import statements Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: unit convert bug Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: update to autoware::universe_utils Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: mis-implementation of process noise Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> --------- Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Jun 23, 2024 · 6ac4b24 · 6ac4b24
1 parent 30ecaad
commit 6ac4b24
Show file tree

Hide file tree

Showing 32 changed files with 1,036 additions and 896 deletions.
diff --git a/...n/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp b/...n/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp
@@ -19,19 +19,20 @@
 #ifndef MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_
 #define MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_
 
-#include <list>
-#include <memory>
-#include <unordered_map>
-#include <vector>
-
 #define EIGEN_MPL2_ONLY
+
 #include "multi_object_tracker/data_association/solver/gnn_solver.hpp"
 #include "multi_object_tracker/tracker/tracker.hpp"
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 
-#include <autoware_perception_msgs/msg/detected_objects.hpp>
+#include "autoware_perception_msgs/msg/detected_objects.hpp"
+
+#include <list>
+#include <memory>
+#include <unordered_map>
+#include <vector>
 
 class DataAssociation
 {

diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp
@@ -15,15 +15,15 @@
 #ifndef MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_
 #define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_
 
+#include "autoware/universe_utils/ros/uuid_helper.hpp"
 #include "multi_object_tracker/tracker/model/tracker_base.hpp"
 
-#include <autoware/universe_utils/ros/uuid_helper.hpp>
 #include <rclcpp/rclcpp.hpp>
 
-#include "unique_identifier_msgs/msg/uuid.hpp"
-#include <autoware_perception_msgs/msg/detected_objects.hpp>
-#include <autoware_perception_msgs/msg/tracked_objects.hpp>
+#include "autoware_perception_msgs/msg/detected_objects.hpp"
+#include "autoware_perception_msgs/msg/tracked_objects.hpp"
 #include <geometry_msgs/msg/point.hpp>
+#include <unique_identifier_msgs/msg/uuid.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
 #include <boost/functional/hash.hpp>

diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp
@@ -15,16 +15,16 @@
 #ifndef MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_
 #define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_
 
+#include "autoware/universe_utils/ros/debug_publisher.hpp"
+#include "autoware/universe_utils/ros/published_time_publisher.hpp"
 #include "multi_object_tracker/debugger/debug_object.hpp"
 
-#include <autoware/universe_utils/ros/debug_publisher.hpp>
-#include <autoware/universe_utils/ros/published_time_publisher.hpp>
 #include <diagnostic_updater/diagnostic_updater.hpp>
 #include <diagnostic_updater/publisher.hpp>
 #include <rclcpp/rclcpp.hpp>
 
-#include <autoware_perception_msgs/msg/detected_objects.hpp>
-#include <autoware_perception_msgs/msg/tracked_objects.hpp>
+#include "autoware_perception_msgs/msg/detected_objects.hpp"
+#include "autoware_perception_msgs/msg/tracked_objects.hpp"
 #include <geometry_msgs/msg/pose_stamped.hpp>
 
 #include <list>

diff --git a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp
@@ -27,8 +27,8 @@
 
 #include <rclcpp/rclcpp.hpp>
 
-#include <autoware_perception_msgs/msg/detected_objects.hpp>
-#include <autoware_perception_msgs/msg/tracked_objects.hpp>
+#include "autoware_perception_msgs/msg/detected_objects.hpp"
+#include "autoware_perception_msgs/msg/tracked_objects.hpp"
 #include <geometry_msgs/msg/pose_stamped.hpp>
 
 #include <tf2/LinearMath/Transform.h>

diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp
@@ -19,8 +19,8 @@
 
 #include <rclcpp/rclcpp.hpp>
 
-#include <autoware_perception_msgs/msg/detected_objects.hpp>
-#include <autoware_perception_msgs/msg/tracked_objects.hpp>
+#include "autoware_perception_msgs/msg/detected_objects.hpp"
+#include "autoware_perception_msgs/msg/tracked_objects.hpp"
 
 #include <list>
 #include <map>

diff --git a/...ption/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/...ption/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp
@@ -19,24 +19,18 @@
 #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_
 #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_
 
+#include "kalman_filter/kalman_filter.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>
+#include "multi_object_tracker/tracker/object_model/object_model.hpp"
 
 class BicycleTracker : public Tracker
 {
 private:
   autoware_perception_msgs::msg::DetectedObject object_;
   rclcpp::Logger logger_;
 
-private:
-  struct EkfParams
-  {
-    double r_cov_x;
-    double r_cov_y;
-    double r_cov_yaw;
-  } ekf_params_;
+  object_model::ObjectModel object_model_ = object_model::bicycle;
 
   double z_;
 
@@ -48,9 +42,7 @@ class BicycleTracker : public Tracker
   };
   BoundingBox bounding_box_;
 
-private:
   BicycleMotionModel motion_model_;
-  const char DIM = motion_model_.DIM;
   using IDX = BicycleMotionModel::IDX;
 
 public:
@@ -63,15 +55,16 @@ class BicycleTracker : public Tracker
   bool measure(
     const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
     const geometry_msgs::msg::Transform & self_transform) override;
-  autoware_perception_msgs::msg::DetectedObject getUpdatingObject(
-    const autoware_perception_msgs::msg::DetectedObject & object,
-    const geometry_msgs::msg::Transform & self_transform);
   bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object);
   bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object);
   bool getTrackedObject(
     const rclcpp::Time & time,
     autoware_perception_msgs::msg::TrackedObject & object) const override;
-  virtual ~BicycleTracker() {}
+
+private:
+  autoware_perception_msgs::msg::DetectedObject getUpdatingObject(
+    const autoware_perception_msgs::msg::DetectedObject & object,
+    const geometry_msgs::msg::Transform & self_transform) const;
 };
 
 #endif  // MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_
diff --git a/...n/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/...n/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp
@@ -19,25 +19,19 @@
 #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_
 #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_
 
+#include "kalman_filter/kalman_filter.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>
+#include "multi_object_tracker/tracker/object_model/object_model.hpp"
 
 class BigVehicleTracker : public Tracker
 {
 private:
   autoware_perception_msgs::msg::DetectedObject object_;
   rclcpp::Logger logger_;
 
-private:
-  struct EkfParams
-  {
-    double r_cov_x;
-    double r_cov_y;
-    double r_cov_yaw;
-    double r_cov_vel;
-  } ekf_params_;
+  object_model::ObjectModel object_model_ = object_model::big_vehicle;
+
   double velocity_deviation_threshold_;
 
   double z_;
@@ -51,9 +45,7 @@ class BigVehicleTracker : public Tracker
   BoundingBox bounding_box_;
   Eigen::Vector2d tracking_offset_;
 
-private:
   BicycleMotionModel motion_model_;
-  const char DIM = motion_model_.DIM;
   using IDX = BicycleMotionModel::IDX;
 
 public:
@@ -66,15 +58,16 @@ class BigVehicleTracker : public Tracker
   bool measure(
     const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
     const geometry_msgs::msg::Transform & self_transform) override;
-  autoware_perception_msgs::msg::DetectedObject getUpdatingObject(
-    const autoware_perception_msgs::msg::DetectedObject & object,
-    const geometry_msgs::msg::Transform & self_transform);
   bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object);
   bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object);
   bool getTrackedObject(
     const rclcpp::Time & time,
     autoware_perception_msgs::msg::TrackedObject & object) const override;
-  virtual ~BigVehicleTracker() {}
+
+private:
+  autoware_perception_msgs::msg::DetectedObject getUpdatingObject(
+    const autoware_perception_msgs::msg::DetectedObject & object,
+    const geometry_msgs::msg::Transform & self_transform);
 };
 
 #endif  // MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_
diff --git a/...ti_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp b/...ti_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp
@@ -19,11 +19,11 @@
 #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_
 #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_
 
+#include "kalman_filter/kalman_filter.hpp"
 #include "multi_object_tracker/tracker/model/big_vehicle_tracker.hpp"
 #include "multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp"
 #include "multi_object_tracker/tracker/model/tracker_base.hpp"
 
-#include <kalman_filter/kalman_filter.hpp>
 #include <rclcpp/time.hpp>
 
 class MultipleVehicleTracker : public Tracker

diff --git a/...ulti_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/...ulti_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp
@@ -19,25 +19,19 @@
 #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_
 #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_
 
+#include "kalman_filter/kalman_filter.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>
+#include "multi_object_tracker/tracker/object_model/object_model.hpp"
 
 class NormalVehicleTracker : public Tracker
 {
 private:
   autoware_perception_msgs::msg::DetectedObject object_;
   rclcpp::Logger logger_;
 
-private:
-  struct EkfParams
-  {
-    double r_cov_x;
-    double r_cov_y;
-    double r_cov_yaw;
-    double r_cov_vel;
-  } ekf_params_;
+  object_model::ObjectModel object_model_ = object_model::normal_vehicle;
+
   double velocity_deviation_threshold_;
 
   double z_;
@@ -51,9 +45,7 @@ class NormalVehicleTracker : public Tracker
   BoundingBox bounding_box_;
   Eigen::Vector2d tracking_offset_;
 
-private:
   BicycleMotionModel motion_model_;
-  const char DIM = motion_model_.DIM;
   using IDX = BicycleMotionModel::IDX;
 
 public:
@@ -66,15 +58,16 @@ class NormalVehicleTracker : public Tracker
   bool measure(
     const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
     const geometry_msgs::msg::Transform & self_transform) override;
-  autoware_perception_msgs::msg::DetectedObject getUpdatingObject(
-    const autoware_perception_msgs::msg::DetectedObject & object,
-    const geometry_msgs::msg::Transform & self_transform);
   bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object);
   bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object);
   bool getTrackedObject(
     const rclcpp::Time & time,
     autoware_perception_msgs::msg::TrackedObject & object) const override;
-  virtual ~NormalVehicleTracker() {}
+
+private:
+  autoware_perception_msgs::msg::DetectedObject getUpdatingObject(
+    const autoware_perception_msgs::msg::DetectedObject & object,
+    const geometry_msgs::msg::Transform & self_transform);
 };
 
 #endif  // MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_
diff --git a/.../multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/.../multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp
@@ -19,10 +19,9 @@
 #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_
 #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_
 
+#include "kalman_filter/kalman_filter.hpp"
 #include "tracker_base.hpp"
 
-#include <kalman_filter/kalman_filter.hpp>
-
 class PassThroughTracker : public Tracker
 {
 private:
@@ -43,7 +42,6 @@ class PassThroughTracker : public Tracker
   bool getTrackedObject(
     const rclcpp::Time & time,
     autoware_perception_msgs::msg::TrackedObject & object) const override;
-  virtual ~PassThroughTracker() {}
 };
 
 #endif  // MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_
diff --git a/...ect_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp b/...ect_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp
@@ -19,12 +19,11 @@
 #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_
 #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_
 
+#include "kalman_filter/kalman_filter.hpp"
 #include "multi_object_tracker/tracker/model/bicycle_tracker.hpp"
 #include "multi_object_tracker/tracker/model/pedestrian_tracker.hpp"
 #include "multi_object_tracker/tracker/model/tracker_base.hpp"
 
-#include <kalman_filter/kalman_filter.hpp>
-
 class PedestrianAndBicycleTracker : public Tracker
 {
 private:

diff --git a/...on/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/...on/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp
@@ -19,10 +19,10 @@
 #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_
 #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_
 
+#include "kalman_filter/kalman_filter.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>
+#include "multi_object_tracker/tracker/object_model/object_model.hpp"
 
 // cspell: ignore CTRV
 
@@ -32,13 +32,7 @@ class PedestrianTracker : public Tracker
   autoware_perception_msgs::msg::DetectedObject object_;
   rclcpp::Logger logger_;
 
-private:
-  struct EkfParams
-  {
-    double r_cov_x;
-    double r_cov_y;
-    double r_cov_yaw;
-  } ekf_params_;
+  object_model::ObjectModel object_model_ = object_model::pedestrian;
 
   double z_;
 
@@ -56,9 +50,7 @@ class PedestrianTracker : public Tracker
   BoundingBox bounding_box_;
   Cylinder cylinder_;
 
-private:
   CTRVMotionModel motion_model_;
-  const char DIM = motion_model_.DIM;
   using IDX = CTRVMotionModel::IDX;
 
 public:
@@ -71,15 +63,16 @@ class PedestrianTracker : public Tracker
   bool measure(
     const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
     const geometry_msgs::msg::Transform & self_transform) override;
-  autoware_perception_msgs::msg::DetectedObject getUpdatingObject(
-    const autoware_perception_msgs::msg::DetectedObject & object,
-    const geometry_msgs::msg::Transform & self_transform);
   bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object);
   bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object);
   bool getTrackedObject(
     const rclcpp::Time & time,
     autoware_perception_msgs::msg::TrackedObject & object) const override;
-  virtual ~PedestrianTracker() {}
+
+private:
+  autoware_perception_msgs::msg::DetectedObject getUpdatingObject(
+    const autoware_perception_msgs::msg::DetectedObject & object,
+    const geometry_msgs::msg::Transform & self_transform) const;
 };
 
 #endif  // MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp
@@ -28,8 +28,8 @@
 
 #include "autoware_perception_msgs/msg/detected_object.hpp"
 #include "autoware_perception_msgs/msg/tracked_object.hpp"
-#include "geometry_msgs/msg/point.hpp"
-#include "unique_identifier_msgs/msg/uuid.hpp"
+#include <geometry_msgs/msg/point.hpp>
+#include <unique_identifier_msgs/msg/uuid.hpp>
 
 #include <vector>
 
@@ -54,7 +54,7 @@ class Tracker
     const rclcpp::Time & time,
     const std::vector<autoware_perception_msgs::msg::ObjectClassification> & classification,
     const size_t & channel_size);
-  virtual ~Tracker() {}
+  virtual ~Tracker() = default;
 
   void initializeExistenceProbabilities(
     const uint & channel_index, const float & existence_probability);