Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Merged
merged 3 commits into from
Mar 14, 2024
Merged

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

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
4905992
bugfix: mot multi-step prediction is not work as intended. back to on…
technolojin Mar 13, 2024
d28afea
fix: remove comment-out codes
technolojin Mar 13, 2024
c753924
fix: prediction to use variable kalman filter
technolojin Mar 14, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
};

Expand Down
2 changes: 1 addition & 1 deletion perception/multi_object_tracker/src/multi_object_tracker_core.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down
57 changes: 37 additions & 20 deletions perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand Down
Loading