From 49059926bc5359547725ade2568cd18ab3775e1f Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Wed, 13 Mar 2024 22:15:27 +0900
Subject: [PATCH 1/3] bugfix: mot multi-step prediction is not work as
intended. back to one-step prediction.
Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
.../multi_object_tracker_core.hpp | 2 +-
.../src/multi_object_tracker_core.cpp | 2 +-
.../motion_model/motion_model_base.cpp | 57 ++++++++++++-------
3 files changed, 39 insertions(+), 22 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
index 95d33b78ff184..3b05af9601f1e 100644
--- 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
index cc662768ce197..1e684e78fc45a 100644
--- 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/motion_model_base.cpp b/perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp
index b4af422fd2329..46834b80a5588 100644
--- 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,16 +46,28 @@ bool MotionModel::predictState(const rclcpp::Time & time)
if (dt < 0.0) {
return false;
}
- // 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_, ekf_)) {
- return false;
- }
- // add interval to last_update_time_
- last_update_time_ += rclcpp::Duration::from_seconds(dt_);
+ if (dt < 1e-6 /*1usec*/) {
+ return true;
}
+
+ // // 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_, ekf_)) {
+ // return false;
+ // }
+ // // add interval to last_update_time_
+ // last_update_time_ += rclcpp::Duration::from_seconds(dt_);
+ // }
+ // std::cout << "MotionModel::predictState predict dt: " << dt << ", dt_: " << dt_ << ",
+ // repeat:
+ // "<< repeat<< std::endl;
+ // }
+
+ if (!predictStateStep(dt, ekf_)) return false;
+
// update last_update_time_ to the estimation time
last_update_time_ = time;
return true;
@@ -76,17 +88,22 @@ bool MotionModel::getPredictedState(
dt = 0.0;
}
- // 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;
- }
- }
- }
+ // // predict only when dt is small enough
+ // if (1e-6 /*1usec*/ < dt) {
+ // // 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;
+ // }
+ // }
+ // std::cout << "MotionModel::getPredictedStateMatrix predict dt: " << dt << ", dt_: " << dt_ <<
+ // ", repeat: "<< repeat<< std::endl;
+ // }
+
+ if (!predictStateStep(dt, tmp_ekf_for_no_update)) return false;
+
tmp_ekf_for_no_update.getX(X);
tmp_ekf_for_no_update.getP(P);
return true;
From d28afeaf556b2bae655e2fcebeb8185ee01c8df4 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Thu, 14 Mar 2024 08:43:30 +0900
Subject: [PATCH 2/3] fix: remove comment-out codes
Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
.../motion_model/motion_model_base.cpp | 32 +------------------
1 file changed, 1 insertion(+), 31 deletions(-)
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
index 46834b80a5588..61763d9664ef9 100644
--- 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
@@ -50,25 +50,9 @@ bool MotionModel::predictState(const rclcpp::Time & time)
return true;
}
- // // 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_, ekf_)) {
- // return false;
- // }
- // // add interval to last_update_time_
- // last_update_time_ += rclcpp::Duration::from_seconds(dt_);
- // }
- // std::cout << "MotionModel::predictState predict dt: " << dt << ", dt_: " << dt_ << ",
- // repeat:
- // "<< repeat<< std::endl;
- // }
-
if (!predictStateStep(dt, ekf_)) return false;
- // update last_update_time_ to the estimation time
+ // update last_update_time_
last_update_time_ = time;
return true;
}
@@ -88,20 +72,6 @@ bool MotionModel::getPredictedState(
dt = 0.0;
}
- // // predict only when dt is small enough
- // if (1e-6 /*1usec*/ < dt) {
- // // 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;
- // }
- // }
- // std::cout << "MotionModel::getPredictedStateMatrix predict dt: " << dt << ", dt_: " << dt_ <<
- // ", repeat: "<< repeat<< std::endl;
- // }
-
if (!predictStateStep(dt, tmp_ekf_for_no_update)) return false;
tmp_ekf_for_no_update.getX(X);
From c753924863eab655d0da1e6e6392c2ad019e79dd Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Thu, 14 Mar 2024 11:45:52 +0900
Subject: [PATCH 3/3] 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>
---
.../motion_model/bicycle_motion_model.cpp | 2 +-
.../motion_model/ctrv_motion_model.cpp | 2 +-
.../tracker/motion_model/cv_motion_model.cpp | 2 +-
.../motion_model/motion_model_base.cpp | 34 +++++++++++++------
4 files changed, 26 insertions(+), 14 deletions(-)
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
index d992ea926746e..a22fb0035f1ea 100644
--- 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
@@ -316,7 +316,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
index b10fc70d1bba7..9e8327e381057 100644
--- 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
index 28ba59f4f19f9..a9ad03e7875c5 100644
--- 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
index 61763d9664ef9..4d51021de634b 100644
--- 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
@@ -50,9 +50,15 @@ bool MotionModel::predictState(const rclcpp::Time & time)
return true;
}
- if (!predictStateStep(dt, ekf_)) return false;
-
- // update last_update_time_
+ // 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_, ekf_)) return false;
+ last_update_time_ += rclcpp::Duration::from_seconds(dt_);
+ }
+ // reset the last_update_time_ to the prediction time
last_update_time_ = time;
return true;
}
@@ -63,17 +69,23 @@ bool MotionModel::getPredictedState(
// check if the state is initialized
if (!checkInitialized()) return false;
+ const double dt = getDeltaTime(time);
+ if (dt < 1e-6 /*1usec*/) {
+ // no prediction, return the current state
+ ekf_.getX(X);
+ ekf_.getP(P);
+ return true;
+ }
+
// 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;
+ // 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;
}
-
- if (!predictStateStep(dt, tmp_ekf_for_no_update)) return false;
-
tmp_ekf_for_no_update.getX(X);
tmp_ekf_for_no_update.getP(P);
return true;