fix(multi_object_tracker): mot multi-step prediction is not work as i…

…ntended (autowarefoundation#6611) * bugfix: mot multi-step prediction is not work as intended. back to one-step prediction. Signed-off-by: Taekjin LEE <[email protected]> * fix: remove comment-out codes Signed-off-by: Taekjin LEE <[email protected]> * 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 <[email protected]> --------- Signed-off-by: Taekjin LEE <[email protected]> Signed-off-by: kaigohirao <[email protected]>
kaigohirao · Mar 22, 2024 · 2e338f0 · 2e338f0
1 parent 8418460
commit 2e338f0
Show file tree

Hide file tree

Showing 6 changed files with 27 additions and 28 deletions.
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
@@ -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;
 };
 

diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/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();

diff --git a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp b/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));

diff --git a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp b/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));

diff --git a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp b/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

diff --git a/perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp b/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);