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.

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
Show file tree
Hide file tree
Changes from all commits
Commits
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
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
Original file line number Diff line number Diff line change
Expand Up @@ -441,7 +441,7 @@
return true;
}

void MultiObjectTracker::publish(const rclcpp::Time & time)
void MultiObjectTracker::publish(const rclcpp::Time & time) const

Check warning on line 444 in perception/multi_object_tracker/src/multi_object_tracker_core.cpp

View check run for this annotation

Codecov / codecov/patch

perception/multi_object_tracker/src/multi_object_tracker_core.cpp#L444

Added line #L444 was not covered by tests
{
const auto subscriber_count = tracked_objects_pub_->get_subscription_count() +
tracked_objects_pub_->get_intra_process_subscription_count();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -316,7 +316,7 @@

// Current state vector X t
Eigen::MatrixXd X_t(DIM, 1);
getStateVector(X_t);
ekf.getX(X_t);

Check warning on line 319 in perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp

View check run for this annotation

Codecov / codecov/patch

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

Added line #L319 was not covered by tests

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));
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -272,7 +272,7 @@

// Current state vector X t
Eigen::MatrixXd X_t(DIM, 1);
getStateVector(X_t);
ekf.getX(X_t);

Check warning on line 275 in perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp

View check run for this annotation

Codecov / codecov/patch

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

Added line #L275 was not covered by tests

const double cos_yaw = std::cos(X_t(IDX::YAW));
const double sin_yaw = std::sin(X_t(IDX::YAW));
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -209,7 +209,7 @@

// Current state vector X t
Eigen::MatrixXd X_t(DIM, 1);
getStateVector(X_t);
ekf.getX(X_t);

Check warning on line 212 in perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp

View check run for this annotation

Codecov / codecov/patch

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

Added line #L212 was not covered by tests

// Predict state vector X t+1
Eigen::MatrixXd X_next_t(DIM, 1); // predicted state
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -46,17 +46,19 @@
if (dt < 0.0) {
return false;
}
if (dt < 1e-6 /*1usec*/) {

Check warning on line 49 in perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp

View check run for this annotation

Codecov / codecov/patch

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

Added line #L49 was not covered by tests
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;

Check warning on line 55 in perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp

View check run for this annotation

Codecov / codecov/patch

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

Added line #L55 was not covered by tests
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;

Check warning on line 58 in perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp

View check run for this annotation

Codecov / codecov/patch

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

Added line #L58 was not covered by tests
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;
}
Expand All @@ -67,25 +69,22 @@
// 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*/) {

Check warning on line 73 in perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp

View check run for this annotation

Codecov / codecov/patch

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

Added line #L73 was not covered by tests
// no prediction, return the current state
ekf_.getX(X);
ekf_.getP(P);
return true;

Check warning on line 77 in perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp

View check run for this annotation

Codecov / codecov/patch

perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp#L75-L77

Added lines #L75 - L77 were not covered by tests
}

// 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_;

Check warning on line 81 in perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp

View check run for this annotation

Codecov / codecov/patch

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

Added line #L81 was not covered by tests
// 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;

Check warning on line 87 in perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp

View check run for this annotation

Codecov / codecov/patch

perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp#L84-L87

Added lines #L84 - L87 were not covered by tests
}
tmp_ekf_for_no_update.getX(X);
tmp_ekf_for_no_update.getP(P);
Expand Down
Loading