fix(multi_object_tracker): mot multi-step prediction is not work as intended (autowarefoundation#6611)

* bugfix: mot multi-step prediction is not work as intended. back to one-step prediction.

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* fix: remove comment-out codes

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* fix: prediction to use variable kalman filter

a bug was found on `predictStateStep` methods

* intention: predict the future state of the variable kalman filter `ekf`
* bug: using the member kalman filter `ekf_`
* fix: get the state vector from the variable kalman filter `ekf`

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

---------

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

Author: technolojin

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/src/multi_object_tracker_core.cpp

@@ -441,7 +441,7 @@ inline bool MultiObjectTracker::shouldTrackerPublish(
   return true;
 }
 
-void MultiObjectTracker::publish(const rclcpp::Time & time)
+void MultiObjectTracker::publish(const rclcpp::Time & time) const
 {
   const auto subscriber_count = tracked_objects_pub_->get_subscription_count() +
                                 tracked_objects_pub_->get_intra_process_subscription_count();

perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp

@@ -317,7 +317,7 @@ bool BicycleMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) c
 
   // Current state vector X t
   Eigen::MatrixXd X_t(DIM, 1);
-  getStateVector(X_t);
+  ekf.getX(X_t);
 
   const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP));
   const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP));

perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp

@@ -272,7 +272,7 @@ bool CTRVMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) cons
 
   // Current state vector X t
   Eigen::MatrixXd X_t(DIM, 1);
-  getStateVector(X_t);
+  ekf.getX(X_t);
 
   const double cos_yaw = std::cos(X_t(IDX::YAW));
   const double sin_yaw = std::sin(X_t(IDX::YAW));

perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp

@@ -209,7 +209,7 @@ bool CVMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) const
 
   // Current state vector X t
   Eigen::MatrixXd X_t(DIM, 1);
-  getStateVector(X_t);
+  ekf.getX(X_t);
 
   // Predict state vector X t+1
   Eigen::MatrixXd X_next_t(DIM, 1);  // predicted state

perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp

@@ -46,17 +46,19 @@ bool MotionModel::predictState(const rclcpp::Time & time)
   if (dt < 0.0) {
     return false;
   }
+  if (dt < 1e-6 /*1usec*/) {
+    return true;
+  }
+
+  // multi-step prediction
   // if dt is too large, shorten dt and repeat prediction
-  const uint32_t repeat = std::ceil(dt / dt_max_);
+  const uint32_t repeat = std::floor(dt / dt_max_) + 1;
   const double dt_ = dt / repeat;
   for (uint32_t i = 0; i < repeat; ++i) {
-    if (!predictStateStep(dt_, ekf_)) {
-      return false;
-    }
-    // add interval to last_update_time_
+    if (!predictStateStep(dt_, ekf_)) return false;
     last_update_time_ += rclcpp::Duration::from_seconds(dt_);
   }
-  // update last_update_time_ to the estimation time
+  // reset the last_update_time_ to the prediction time
   last_update_time_ = time;
   return true;
 }
@@ -67,25 +69,22 @@ bool MotionModel::getPredictedState(
   // check if the state is initialized
   if (!checkInitialized()) return false;
 
-  // copy the predicted state and covariance
-  KalmanFilter tmp_ekf_for_no_update = ekf_;
-
-  double dt = getDeltaTime(time);
-  if (dt < 0.0) {
-    // a naive way to handle the case when the required prediction time is in the past
-    dt = 0.0;
+  const double dt = getDeltaTime(time);
+  if (dt < 1e-6 /*1usec*/) {
+    // no prediction, return the current state
+    ekf_.getX(X);
+    ekf_.getP(P);
+    return true;
   }
 
-  // predict only when dt is small enough
-  if (0.001 /*1msec*/ < dt) {
-    // if dt is too large, shorten dt and repeat prediction
-    const uint32_t repeat = std::ceil(dt / dt_max_);
-    const double dt_ = dt / repeat;
-    for (uint32_t i = 0; i < repeat; ++i) {
-      if (!predictStateStep(dt_, tmp_ekf_for_no_update)) {
-        return false;
-      }
-    }
+  // copy the predicted state and covariance
+  KalmanFilter tmp_ekf_for_no_update = ekf_;
+  // multi-step prediction
+  // if dt is too large, shorten dt and repeat prediction
+  const uint32_t repeat = std::floor(dt / dt_max_) + 1;
+  const double dt_ = dt / repeat;
+  for (uint32_t i = 0; i < repeat; ++i) {
+    if (!predictStateStep(dt_, tmp_ekf_for_no_update)) return false;
   }
   tmp_ekf_for_no_update.getX(X);
   tmp_ekf_for_no_update.getP(P);