From 1e43240c16990d8de416f5519e55245ad04ea195 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 29 Feb 2024 12:46:13 +0900 Subject: [PATCH 01/42] fix: define function to adjust object yaw angle for ekf update Signed-off-by: Taekjin LEE --- .../tracker/model/big_vehicle_tracker.hpp | 1 - .../tracker/model/normal_vehicle_tracker.hpp | 1 - .../multi_object_tracker/utils/utils.hpp | 27 ++++++++ .../src/tracker/model/bicycle_tracker.cpp | 34 ++-------- .../src/tracker/model/big_vehicle_tracker.cpp | 65 ++++++++----------- .../tracker/model/normal_vehicle_tracker.cpp | 58 +++++++---------- 6 files changed, 85 insertions(+), 101 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index e19b6382ad952..b41850c832baf 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -86,7 +86,6 @@ class BigVehicleTracker : public Tracker bool getTrackedObject( const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const override; - double getMeasurementYaw(const autoware_auto_perception_msgs::msg::DetectedObject & object); void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform); virtual ~BigVehicleTracker() {} }; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 05fa0a5ef01ba..5577636944fb3 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -90,7 +90,6 @@ class NormalVehicleTracker : public Tracker const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const override; void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform); - double getMeasurementYaw(const autoware_auto_perception_msgs::msg::DetectedObject & object); virtual ~NormalVehicleTracker() {} }; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp index d2b6ee5de475e..07d7f0f079a12 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp @@ -343,6 +343,33 @@ inline void convertConvexHullToBoundingBox( output_object.shape.dimensions.z = height; } +inline bool getMeasurementYaw( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const double & predicted_yaw, double & measurement_yaw) +{ + if (object.kinematics.orientation_availability == + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE) { + return false; + } + measurement_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + + // check orientation sign is known or not, and fix the limiting delta yaw + double limiting_delta_yaw = M_PI; + if (object.kinematics.orientation_availability == + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN) { + limiting_delta_yaw = M_PI_2; + } + // limiting delta yaw + while (std::fabs(predicted_yaw - measurement_yaw) > limiting_delta_yaw) { + if (measurement_yaw < predicted_yaw) { + measurement_yaw += 2 * limiting_delta_yaw; + } else { + measurement_yaw -= 2 * limiting_delta_yaw; + } + } + return true; +} + } // namespace utils #endif // MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 2d732db6bb709..0bd9240c9f5a0 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -299,38 +299,16 @@ bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const bool BicycleTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - // yaw measurement - double measurement_yaw = tier4_autoware_utils::normalizeRadian( - tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); - - // prediction + // predicted state Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); ekf_.getX(X_t); - // validate if orientation is available - bool use_orientation_information = false; - if ( - object.kinematics.orientation_availability == - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN) { // has 180 degree - // uncertainty - // fix orientation - while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) { - measurement_yaw = measurement_yaw + M_PI; - } - while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) { - measurement_yaw = measurement_yaw - M_PI; - } - use_orientation_information = true; - - } else if ( - object.kinematics.orientation_availability == - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE) { // know full angle - - use_orientation_information = true; - } + // get measurement yaw angle to update + double measurement_yaw = 0.0; + bool is_yaw_available = utils::getMeasurementYaw(object, X_t(IDX::YAW), measurement_yaw); - const int dim_y = - use_orientation_information ? 3 : 2; // pos x, pos y, (pos yaw) depending on Pose output + // pos x, pos y, (pos yaw) depending on pose measurement + const int dim_y = is_yaw_available ? 3 : 2; // Set measurement matrix C and observation vector Y Eigen::MatrixXd Y(dim_y, 1); diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 8516e9a8d02c9..9ef442111c062 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -57,15 +57,15 @@ BigVehicleTracker::BigVehicleTracker( // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty float r_stddev_x = 0.5; // in vehicle coordinate [m] float r_stddev_y = 0.4; // in vehicle coordinate [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad] - float r_stddev_vel = 1.0; // [m/s] + float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // in map coordinate [rad] + float r_stddev_vel = 1.0; // in object coordinate [m/s] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); ekf_params_.r_cov_vel = std::pow(r_stddev_vel, 2.0); // initial state covariance - float p0_stddev_x = 1.5; // [m] - float p0_stddev_y = 0.5; // [m] + float p0_stddev_x = 1.5; // in object coordinate [m] + float p0_stddev_y = 0.5; // in object coordinate [m] float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] @@ -314,16 +314,20 @@ bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const bool BigVehicleTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + // predicted state + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); + ekf_.getX(X_t); + // measurement noise covariance float r_cov_x; float r_cov_y; + using Label = autoware_auto_perception_msgs::msg::ObjectClassification; const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); - if (utils::isLargeVehicleLabel(label)) { r_cov_x = ekf_params_.r_cov_x; r_cov_y = ekf_params_.r_cov_y; } else if (label == Label::CAR) { + // if label is changed, enlarge the measurement noise covariance constexpr float r_stddev_x = 2.0; // [m] constexpr float r_stddev_y = 2.0; // [m] r_cov_x = std::pow(r_stddev_x, 2.0); @@ -333,27 +337,30 @@ bool BigVehicleTracker::measureWithPose( r_cov_y = ekf_params_.r_cov_y; } - // Decide dimension of measurement vector + // get measurement yaw angle to update + double measurement_yaw = 0.0; + bool is_yaw_available = utils::getMeasurementYaw(object, X_t(IDX::YAW), measurement_yaw); + if (!is_yaw_available) { + RCLCPP_WARN(logger_, "Cannot get measurement yaw"); + return false; + } + + // Decide dimension of measurement vector and matrix + // velocity capability is checked only when the object has velocity measurement + // and the predicted velocity is close to the observed velocity bool enable_velocity_measurement = false; if (object.kinematics.has_twist) { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf_.getX(X_t); - const double predicted_vel = X_t(IDX::VEL); - const double observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; - + const double & predicted_vel = X_t(IDX::VEL); + const double & observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; if (std::fabs(predicted_vel - observed_vel) < velocity_deviation_threshold_) { // Velocity deviation is small enable_velocity_measurement = true; } } - - // pos x, pos y, yaw, vel depending on pose measurement + // pos x, pos y, pos yaw, (vel) depending on pose measurement const int dim_y = enable_velocity_measurement ? 4 : 3; - double measurement_yaw = getMeasurementYaw(object); // get sign-solved yaw angle - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf_.getX(X_t); - // convert to boundingbox if input is convex shape + // convert to bounding box if input is convex shape autoware_auto_perception_msgs::msg::DetectedObject bbox_object; if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { utils::convertConvexHullToBoundingBox(object, bbox_object); @@ -361,7 +368,8 @@ bool BigVehicleTracker::measureWithPose( bbox_object = object; } - /* get offset measurement*/ + // Object extension (size) adjustment + // get offset measurement autoware_auto_perception_msgs::msg::DetectedObject offset_object; utils::calcAnchorPointOffset( last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_, @@ -543,6 +551,7 @@ bool BigVehicleTracker::getTrackedObject( pose_with_cov.pose.orientation.y = filtered_quaternion.y(); pose_with_cov.pose.orientation.z = filtered_quaternion.z(); pose_with_cov.pose.orientation.w = filtered_quaternion.w(); + // orientation availability is assumed to be unknown object.kinematics.orientation_availability = autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; } @@ -621,21 +630,3 @@ void BigVehicleTracker::setNearestCornerOrSurfaceIndex( self_transform); } -double BigVehicleTracker::getMeasurementYaw( - const autoware_auto_perception_msgs::msg::DetectedObject & object) -{ - double measurement_yaw = tier4_autoware_utils::normalizeRadian( - tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); - { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - ekf_.getX(X_t); - // Fixed measurement_yaw to be in the range of +-90 degrees of X_t(IDX::YAW) - while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) { - measurement_yaw = measurement_yaw + M_PI; - } - while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) { - measurement_yaw = measurement_yaw - M_PI; - } - } - return measurement_yaw; -} diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 18f73c8610f04..5ec9a884e5f10 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -314,16 +314,20 @@ bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const bool NormalVehicleTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + // predicted state + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); + ekf_.getX(X_t); + // measurement noise covariance float r_cov_x; float r_cov_y; + using Label = autoware_auto_perception_msgs::msg::ObjectClassification; const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); - if (label == Label::CAR) { r_cov_x = ekf_params_.r_cov_x; r_cov_y = ekf_params_.r_cov_y; } else if (utils::isLargeVehicleLabel(label)) { + // if label is changed, enlarge the measurement noise covariance constexpr float r_stddev_x = 2.0; // [m] constexpr float r_stddev_y = 2.0; // [m] r_cov_x = std::pow(r_stddev_x, 2.0); @@ -333,27 +337,30 @@ bool NormalVehicleTracker::measureWithPose( r_cov_y = ekf_params_.r_cov_y; } - // Decide dimension of measurement vector + // get measurement yaw angle to update + double measurement_yaw = 0.0; + bool is_yaw_available = utils::getMeasurementYaw(object, X_t(IDX::YAW), measurement_yaw); + if (!is_yaw_available) { + RCLCPP_WARN(logger_, "Cannot get measurement yaw"); + return false; + } + + // Decide dimension of measurement vector and matrix + // velocity capability is checked only when the object has velocity measurement + // and the predicted velocity is close to the observed velocity bool enable_velocity_measurement = false; if (object.kinematics.has_twist) { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf_.getX(X_t); - const double predicted_vel = X_t(IDX::VEL); - const double observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; - + const double & predicted_vel = X_t(IDX::VEL); + const double & observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; if (std::fabs(predicted_vel - observed_vel) < velocity_deviation_threshold_) { // Velocity deviation is small enable_velocity_measurement = true; } } - - // pos x, pos y, yaw, vel depending on pose measurement + // pos x, pos y, pos yaw, (vel) depending on pose measurement const int dim_y = enable_velocity_measurement ? 4 : 3; - double measurement_yaw = getMeasurementYaw(object); // get sign-solved yaw angle - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf_.getX(X_t); - // convert to boundingbox if input is convex shape + // convert to bounding box if input is convex shape autoware_auto_perception_msgs::msg::DetectedObject bbox_object; if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { utils::convertConvexHullToBoundingBox(object, bbox_object); @@ -361,7 +368,8 @@ bool NormalVehicleTracker::measureWithPose( bbox_object = object; } - /* get offset measurement*/ + // Object extension (size) adjustment + // get offset measurement autoware_auto_perception_msgs::msg::DetectedObject offset_object; utils::calcAnchorPointOffset( last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_, @@ -543,6 +551,7 @@ bool NormalVehicleTracker::getTrackedObject( pose_with_cov.pose.orientation.y = filtered_quaternion.y(); pose_with_cov.pose.orientation.z = filtered_quaternion.z(); pose_with_cov.pose.orientation.w = filtered_quaternion.w(); + // orientation availability is assumed to be unknown object.kinematics.orientation_availability = autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; } @@ -620,22 +629,3 @@ void NormalVehicleTracker::setNearestCornerOrSurfaceIndex( X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, self_transform); } - -double NormalVehicleTracker::getMeasurementYaw( - const autoware_auto_perception_msgs::msg::DetectedObject & object) -{ - double measurement_yaw = tier4_autoware_utils::normalizeRadian( - tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); - { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - ekf_.getX(X_t); - // Fixed measurement_yaw to be in the range of +-90 degrees of X_t(IDX::YAW) - while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) { - measurement_yaw = measurement_yaw + M_PI; - } - while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) { - measurement_yaw = measurement_yaw - M_PI; - } - } - return measurement_yaw; -} From b05e2453c9cdee39e6f727b0b1d360a89a5004eb Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 29 Feb 2024 13:06:08 +0900 Subject: [PATCH 02/42] chore: refactoring Signed-off-by: Taekjin LEE --- .../include/multi_object_tracker/utils/utils.hpp | 16 +++++++++------- .../src/tracker/model/bicycle_tracker.cpp | 10 +++++----- .../src/tracker/model/big_vehicle_tracker.cpp | 1 - 3 files changed, 14 insertions(+), 13 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp index 07d7f0f079a12..4e2de9e363f25 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp @@ -344,19 +344,21 @@ inline void convertConvexHullToBoundingBox( } inline bool getMeasurementYaw( - const autoware_auto_perception_msgs::msg::DetectedObject & object, - const double & predicted_yaw, double & measurement_yaw) + const autoware_auto_perception_msgs::msg::DetectedObject & object, const double & predicted_yaw, + double & measurement_yaw) { - if (object.kinematics.orientation_availability == - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE) { + if ( + object.kinematics.orientation_availability == + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE) { return false; } measurement_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - + // check orientation sign is known or not, and fix the limiting delta yaw double limiting_delta_yaw = M_PI; - if (object.kinematics.orientation_availability == - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN) { + if ( + object.kinematics.orientation_availability == + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN) { limiting_delta_yaw = M_PI_2; } // limiting delta yaw diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 0bd9240c9f5a0..f12d3e2a5091e 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -56,13 +56,13 @@ BicycleTracker::BicycleTracker( // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty float r_stddev_x = 0.5; // in vehicle coordinate [m] float r_stddev_y = 0.4; // in vehicle coordinate [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] + float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // in map coordinate [rad] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); // initial state covariance - float p0_stddev_x = 0.8; // [m] - float p0_stddev_y = 0.5; // [m] + float p0_stddev_x = 0.8; // in object coordinate [m] + float p0_stddev_y = 0.5; // in object coordinate [m] float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] @@ -87,7 +87,7 @@ BicycleTracker::BicycleTracker( ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle // limitations max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s] - max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s] + max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad] // initialize state vector X Eigen::MatrixXd X(ekf_params_.dim_x, 1); @@ -308,7 +308,7 @@ bool BicycleTracker::measureWithPose( bool is_yaw_available = utils::getMeasurementYaw(object, X_t(IDX::YAW), measurement_yaw); // pos x, pos y, (pos yaw) depending on pose measurement - const int dim_y = is_yaw_available ? 3 : 2; + const int dim_y = is_yaw_available ? 3 : 2; // Set measurement matrix C and observation vector Y Eigen::MatrixXd Y(dim_y, 1); diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 9ef442111c062..36182e74ae77c 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -629,4 +629,3 @@ void BigVehicleTracker::setNearestCornerOrSurfaceIndex( X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, self_transform); } - From c5b8923c218ec92e46fa677ce6a44a4ad9e6e64d Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 29 Feb 2024 14:45:25 +0900 Subject: [PATCH 03/42] feat: update tracking offset to object position Signed-off-by: Taekjin LEE --- .../src/tracker/model/big_vehicle_tracker.cpp | 16 +++++++++++----- .../src/tracker/model/normal_vehicle_tracker.cpp | 16 +++++++++++----- 2 files changed, 22 insertions(+), 10 deletions(-) diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 36182e74ae77c..72dd504700d25 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -459,7 +459,7 @@ bool BigVehicleTracker::measureWithShape( bbox_object = object; } - constexpr float gain = 0.9; + constexpr float gain = 0.8; bounding_box_.length = gain * bounding_box_.length + (1.0 - gain) * bbox_object.shape.dimensions.x; bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * bbox_object.shape.dimensions.y; @@ -468,10 +468,16 @@ bool BigVehicleTracker::measureWithShape( last_input_bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; - // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 0.3); - bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.3); + // update offset into position + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); + Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); + ekf_.getX(X_t); + ekf_.getP(P_t); + X_t(IDX::X) = X_t(IDX::X) + (1.0 - gain) * tracking_offset_.x(); + X_t(IDX::Y) = X_t(IDX::Y) + (1.0 - gain) * tracking_offset_.y(); + tracking_offset_.x() = gain * tracking_offset_.x(); + tracking_offset_.y() = gain * tracking_offset_.y(); + ekf_.init(X_t, P_t); // update lf, lr lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center, minimum of 1.5m diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 5ec9a884e5f10..edbc559877767 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -459,7 +459,7 @@ bool NormalVehicleTracker::measureWithShape( bbox_object = object; } - constexpr float gain = 0.9; + constexpr float gain = 0.8; bounding_box_.length = gain * bounding_box_.length + (1.0 - gain) * bbox_object.shape.dimensions.x; bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * bbox_object.shape.dimensions.y; @@ -468,10 +468,16 @@ bool NormalVehicleTracker::measureWithShape( last_input_bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; - // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 0.3); - bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.3); + // update offset into position + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); + Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); + ekf_.getX(X_t); + ekf_.getP(P_t); + X_t(IDX::X) = X_t(IDX::X) + (1.0 - gain) * tracking_offset_.x(); + X_t(IDX::Y) = X_t(IDX::Y) + (1.0 - gain) * tracking_offset_.y(); + tracking_offset_.x() = gain * tracking_offset_.x(); + tracking_offset_.y() = gain * tracking_offset_.y(); + ekf_.init(X_t, P_t); // update lf, lr lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center, minimum of 1.0m From 67854be6b844b007575c2c5e9457a634043998d9 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 29 Feb 2024 17:25:36 +0900 Subject: [PATCH 04/42] fix: enlarge yaw noise when the orientation is unknown Signed-off-by: Taekjin LEE --- .../multi_object_tracker/utils/utils.hpp | 17 +++++++---------- .../src/tracker/model/big_vehicle_tracker.cpp | 7 +++---- .../tracker/model/normal_vehicle_tracker.cpp | 7 +++---- 3 files changed, 13 insertions(+), 18 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp index 4e2de9e363f25..b19b0ba7ee10c 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp @@ -347,21 +347,16 @@ inline bool getMeasurementYaw( const autoware_auto_perception_msgs::msg::DetectedObject & object, const double & predicted_yaw, double & measurement_yaw) { - if ( - object.kinematics.orientation_availability == - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE) { - return false; - } measurement_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); // check orientation sign is known or not, and fix the limiting delta yaw - double limiting_delta_yaw = M_PI; + double limiting_delta_yaw = M_PI_2; if ( object.kinematics.orientation_availability == - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN) { - limiting_delta_yaw = M_PI_2; + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE) { + limiting_delta_yaw = M_PI; } - // limiting delta yaw + // limiting delta yaw, even the availability is unknown while (std::fabs(predicted_yaw - measurement_yaw) > limiting_delta_yaw) { if (measurement_yaw < predicted_yaw) { measurement_yaw += 2 * limiting_delta_yaw; @@ -369,7 +364,9 @@ inline bool getMeasurementYaw( measurement_yaw -= 2 * limiting_delta_yaw; } } - return true; + // return false if the orientation is unknown + return object.kinematics.orientation_availability != + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; } } // namespace utils diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 72dd504700d25..e99975dcf639d 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -340,10 +340,6 @@ bool BigVehicleTracker::measureWithPose( // get measurement yaw angle to update double measurement_yaw = 0.0; bool is_yaw_available = utils::getMeasurementYaw(object, X_t(IDX::YAW), measurement_yaw); - if (!is_yaw_available) { - RCLCPP_WARN(logger_, "Cannot get measurement yaw"); - return false; - } // Decide dimension of measurement vector and matrix // velocity capability is checked only when the object has velocity measurement @@ -396,6 +392,9 @@ bool BigVehicleTracker::measureWithPose( R(1, 1) = r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y R(1, 0) = R(0, 1); // y - x R(2, 2) = ekf_params_.r_cov_yaw; // yaw - yaw + if (!is_yaw_available) { + R(2, 2) *= 1e3; // yaw is not available, multiply large value + } } else { R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index edbc559877767..c4816f15813d6 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -340,10 +340,6 @@ bool NormalVehicleTracker::measureWithPose( // get measurement yaw angle to update double measurement_yaw = 0.0; bool is_yaw_available = utils::getMeasurementYaw(object, X_t(IDX::YAW), measurement_yaw); - if (!is_yaw_available) { - RCLCPP_WARN(logger_, "Cannot get measurement yaw"); - return false; - } // Decide dimension of measurement vector and matrix // velocity capability is checked only when the object has velocity measurement @@ -396,6 +392,9 @@ bool NormalVehicleTracker::measureWithPose( R(1, 1) = r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y R(1, 0) = R(0, 1); // y - x R(2, 2) = ekf_params_.r_cov_yaw; // yaw - yaw + if (!is_yaw_available) { + R(2, 2) *= 1e3; // yaw is not available, multiply large value + } } else { R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; From 78720452f16a006ae7ff882896a52ceedc5d8191 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 29 Feb 2024 18:16:45 +0900 Subject: [PATCH 05/42] fix: rearrange obj extension, noise model, observation vector length Signed-off-by: Taekjin LEE --- .../src/tracker/model/bicycle_tracker.cpp | 7 +-- .../src/tracker/model/big_vehicle_tracker.cpp | 59 +++++++++---------- .../tracker/model/normal_vehicle_tracker.cpp | 59 +++++++++---------- 3 files changed, 60 insertions(+), 65 deletions(-) diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index f12d3e2a5091e..3f54fb9a826c4 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -307,6 +307,7 @@ bool BicycleTracker::measureWithPose( double measurement_yaw = 0.0; bool is_yaw_available = utils::getMeasurementYaw(object, X_t(IDX::YAW), measurement_yaw); + // Decide dimension of measurement vector and matrix // pos x, pos y, (pos yaw) depending on pose measurement const int dim_y = is_yaw_available ? 3 : 2; @@ -331,14 +332,10 @@ bool BicycleTracker::measureWithPose( R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; } - - // if there are orientation available + // update the yaw when available if (dim_y == 3) { - // fill yaw observation and measurement matrix Y(IDX::YAW, 0) = measurement_yaw; C(2, IDX::YAW) = 1.0; - - // fill yaw covariance if (!object.kinematics.has_position_covariance) { R(2, 2) = ekf_params_.r_cov_yaw; // yaw - yaw } else { diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index e99975dcf639d..0c49279ec70e0 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -318,44 +318,23 @@ bool BigVehicleTracker::measureWithPose( Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); ekf_.getX(X_t); - // measurement noise covariance - float r_cov_x; - float r_cov_y; - using Label = autoware_auto_perception_msgs::msg::ObjectClassification; - const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); - if (utils::isLargeVehicleLabel(label)) { - r_cov_x = ekf_params_.r_cov_x; - r_cov_y = ekf_params_.r_cov_y; - } else if (label == Label::CAR) { - // if label is changed, enlarge the measurement noise covariance - constexpr float r_stddev_x = 2.0; // [m] - constexpr float r_stddev_y = 2.0; // [m] - r_cov_x = std::pow(r_stddev_x, 2.0); - r_cov_y = std::pow(r_stddev_y, 2.0); - } else { - r_cov_x = ekf_params_.r_cov_x; - r_cov_y = ekf_params_.r_cov_y; - } - // get measurement yaw angle to update double measurement_yaw = 0.0; bool is_yaw_available = utils::getMeasurementYaw(object, X_t(IDX::YAW), measurement_yaw); - // Decide dimension of measurement vector and matrix // velocity capability is checked only when the object has velocity measurement // and the predicted velocity is close to the observed velocity - bool enable_velocity_measurement = false; + bool is_velocity_available = false; if (object.kinematics.has_twist) { const double & predicted_vel = X_t(IDX::VEL); const double & observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; if (std::fabs(predicted_vel - observed_vel) < velocity_deviation_threshold_) { // Velocity deviation is small - enable_velocity_measurement = true; + is_velocity_available = true; } } - // pos x, pos y, pos yaw, (vel) depending on pose measurement - const int dim_y = enable_velocity_measurement ? 4 : 3; + // Object Shape // convert to bounding box if input is convex shape autoware_auto_perception_msgs::msg::DetectedObject bbox_object; if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { @@ -363,14 +342,36 @@ bool BigVehicleTracker::measureWithPose( } else { bbox_object = object; } - - // Object extension (size) adjustment + // extension (size) adjustment // get offset measurement autoware_auto_perception_msgs::msg::DetectedObject offset_object; utils::calcAnchorPointOffset( last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_, bbox_object, X_t(IDX::YAW), offset_object, tracking_offset_); + // measurement noise covariance + float r_cov_x; + float r_cov_y; + using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); + if (utils::isLargeVehicleLabel(label)) { + r_cov_x = ekf_params_.r_cov_x; + r_cov_y = ekf_params_.r_cov_y; + } else if (label == Label::CAR) { + // if label is changed, enlarge the measurement noise covariance + constexpr float r_stddev_x = 2.0; // [m] + constexpr float r_stddev_y = 2.0; // [m] + r_cov_x = std::pow(r_stddev_x, 2.0); + r_cov_y = std::pow(r_stddev_y, 2.0); + } else { + r_cov_x = ekf_params_.r_cov_x; + r_cov_y = ekf_params_.r_cov_y; + } + + // Decide dimension of measurement vector and matrix + // pos x, pos y, pos yaw, (vel) depending on pose measurement + const int dim_y = is_velocity_available ? 4 : 3; + // Set measurement matrix C and observation vector Y Eigen::MatrixXd Y(dim_y, 1); Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); @@ -406,12 +407,10 @@ bool BigVehicleTracker::measureWithPose( R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y]; R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; } - - // Update the velocity when necessary - if (dim_y == 4) { + // update the velocity when it is available + if (is_velocity_available) { Y(IDX::VEL, 0) = object.kinematics.twist_with_covariance.twist.linear.x; C(3, IDX::VEL) = 1.0; // for vel - if (!object.kinematics.has_twist_covariance) { R(3, 3) = ekf_params_.r_cov_vel; // vel -vel } else { diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index c4816f15813d6..3b08d280199b9 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -318,44 +318,23 @@ bool NormalVehicleTracker::measureWithPose( Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); ekf_.getX(X_t); - // measurement noise covariance - float r_cov_x; - float r_cov_y; - using Label = autoware_auto_perception_msgs::msg::ObjectClassification; - const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); - if (label == Label::CAR) { - r_cov_x = ekf_params_.r_cov_x; - r_cov_y = ekf_params_.r_cov_y; - } else if (utils::isLargeVehicleLabel(label)) { - // if label is changed, enlarge the measurement noise covariance - constexpr float r_stddev_x = 2.0; // [m] - constexpr float r_stddev_y = 2.0; // [m] - r_cov_x = std::pow(r_stddev_x, 2.0); - r_cov_y = std::pow(r_stddev_y, 2.0); - } else { - r_cov_x = ekf_params_.r_cov_x; - r_cov_y = ekf_params_.r_cov_y; - } - // get measurement yaw angle to update double measurement_yaw = 0.0; bool is_yaw_available = utils::getMeasurementYaw(object, X_t(IDX::YAW), measurement_yaw); - // Decide dimension of measurement vector and matrix // velocity capability is checked only when the object has velocity measurement // and the predicted velocity is close to the observed velocity - bool enable_velocity_measurement = false; + bool is_velocity_available = false; if (object.kinematics.has_twist) { const double & predicted_vel = X_t(IDX::VEL); const double & observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; if (std::fabs(predicted_vel - observed_vel) < velocity_deviation_threshold_) { // Velocity deviation is small - enable_velocity_measurement = true; + is_velocity_available = true; } } - // pos x, pos y, pos yaw, (vel) depending on pose measurement - const int dim_y = enable_velocity_measurement ? 4 : 3; + // Object Shape // convert to bounding box if input is convex shape autoware_auto_perception_msgs::msg::DetectedObject bbox_object; if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { @@ -363,14 +342,36 @@ bool NormalVehicleTracker::measureWithPose( } else { bbox_object = object; } - - // Object extension (size) adjustment + // extension (size) adjustment // get offset measurement autoware_auto_perception_msgs::msg::DetectedObject offset_object; utils::calcAnchorPointOffset( last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_, bbox_object, X_t(IDX::YAW), offset_object, tracking_offset_); + // measurement noise covariance + float r_cov_x; + float r_cov_y; + using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); + if (label == Label::CAR) { + r_cov_x = ekf_params_.r_cov_x; + r_cov_y = ekf_params_.r_cov_y; + } else if (utils::isLargeVehicleLabel(label)) { + // if label is changed, enlarge the measurement noise covariance + constexpr float r_stddev_x = 2.0; // [m] + constexpr float r_stddev_y = 2.0; // [m] + r_cov_x = std::pow(r_stddev_x, 2.0); + r_cov_y = std::pow(r_stddev_y, 2.0); + } else { + r_cov_x = ekf_params_.r_cov_x; + r_cov_y = ekf_params_.r_cov_y; + } + + // Decide dimension of measurement vector and matrix + // pos x, pos y, pos yaw, (vel) depending on pose measurement + const int dim_y = is_velocity_available ? 4 : 3; + // Set measurement matrix C and observation vector Y Eigen::MatrixXd Y(dim_y, 1); Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); @@ -406,12 +407,10 @@ bool NormalVehicleTracker::measureWithPose( R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y]; R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; } - - // Update the velocity when necessary - if (dim_y == 4) { + // update the velocity when it is available + if (is_velocity_available) { Y(IDX::VEL, 0) = object.kinematics.twist_with_covariance.twist.linear.x; C(3, IDX::VEL) = 1.0; // for vel - if (!object.kinematics.has_twist_covariance) { R(3, 3) = ekf_params_.r_cov_vel; // vel -vel } else { From 1fd336a0a517aee196eab3b2a08c326aecb6bf3f Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 1 Mar 2024 14:20:50 +0900 Subject: [PATCH 06/42] chore: grouping part of codes Signed-off-by: Taekjin LEE --- .../src/tracker/model/bicycle_tracker.cpp | 36 +++++---- .../src/tracker/model/big_vehicle_tracker.cpp | 75 ++++++++++--------- .../tracker/model/normal_vehicle_tracker.cpp | 75 ++++++++++--------- 3 files changed, 96 insertions(+), 90 deletions(-) diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 3f54fb9a826c4..5c361b3b29c32 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -101,6 +101,7 @@ BicycleTracker::BicycleTracker( X(IDX::VEL) = 0.0; } + // UNCERTAINTY MODEL // initialize state covariance matrix P Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); if (!object.kinematics.has_position_covariance) { @@ -134,19 +135,16 @@ BicycleTracker::BicycleTracker( P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } + // MOTION MODEL (set initial state and covariance) + ekf_.init(X, P); + + // OBJECT SHAPE MODEL if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; } else { bounding_box_ = {1.0, 0.5, 1.7}; } - // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 0.3); - bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.3); - - ekf_.init(X, P); - // Set lf, lr lf_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% front from the center, minimum of 0.3m lr_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% rear from the center, minimum of 0.3m @@ -154,6 +152,7 @@ BicycleTracker::BicycleTracker( bool BicycleTracker::predict(const rclcpp::Time & time) { + // MOTION MODEL (predict) const double dt = (time - last_update_time_).seconds(); if (dt < 0.0) { RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); @@ -203,6 +202,8 @@ bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const * */ + // MOTION MODEL (predict) + // Current state vector X t Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state ekf.getX(X_t); @@ -299,10 +300,12 @@ bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const bool BicycleTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - // predicted state + // current (predicted) state Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); ekf_.getX(X_t); + // MOTION MODEL (update) + // get measurement yaw angle to update double measurement_yaw = 0.0; bool is_yaw_available = utils::getMeasurementYaw(object, X_t(IDX::YAW), measurement_yaw); @@ -318,6 +321,10 @@ bool BicycleTracker::measureWithPose( Y(IDX::Y, 0) = object.kinematics.pose_with_covariance.pose.position.y; C(0, IDX::X) = 1.0; // for pos x C(1, IDX::Y) = 1.0; // for pos y + if (is_yaw_available) { + Y(IDX::YAW, 0) = measurement_yaw; + C(2, IDX::YAW) = 1.0; + } // Set noise covariance matrix R Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); @@ -326,19 +333,15 @@ bool BicycleTracker::measureWithPose( R(0, 1) = 0.0; // x - y R(1, 1) = ekf_params_.r_cov_y; // y - y R(1, 0) = R(0, 1); // y - x + if (is_yaw_available) { + R(2, 2) = ekf_params_.r_cov_yaw; // yaw - yaw + } } else { R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - } - // update the yaw when available - if (dim_y == 3) { - Y(IDX::YAW, 0) = measurement_yaw; - C(2, IDX::YAW) = 1.0; - if (!object.kinematics.has_position_covariance) { - R(2, 2) = ekf_params_.r_cov_yaw; // yaw - yaw - } else { + if (is_yaw_available) { R(0, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_YAW]; R(1, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_YAW]; R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X]; @@ -472,6 +475,7 @@ bool BicycleTracker::getTrackedObject( pose_with_cov.pose.orientation.y = filtered_quaternion.y(); pose_with_cov.pose.orientation.z = filtered_quaternion.z(); pose_with_cov.pose.orientation.w = filtered_quaternion.w(); + // orientation availability is assumed to be unknown object.kinematics.orientation_availability = autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; } diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 0c49279ec70e0..fd42b10b1497a 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -105,6 +105,7 @@ BigVehicleTracker::BigVehicleTracker( X(IDX::VEL) = 0.0; } + // UNCERTAINTY MODEL // initialize state covariance matrix P Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); if (!object.kinematics.has_position_covariance) { @@ -138,13 +139,15 @@ BigVehicleTracker::BigVehicleTracker( P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } + // MOTION MODEL (set initial state and covariance) + ekf_.init(X, P); + + // OBJECT SHAPE MODEL if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; last_input_bounding_box_ = bounding_box_; } else { - // past default value - // bounding_box_ = {7.0, 2.0, 2.0}; autoware_auto_perception_msgs::msg::DetectedObject bbox_object; utils::convertConvexHullToBoundingBox(object, bbox_object); bounding_box_ = { @@ -152,16 +155,8 @@ BigVehicleTracker::BigVehicleTracker( bbox_object.shape.dimensions.z}; last_input_bounding_box_ = bounding_box_; } - ekf_.init(X, P); - - /* calc nearest corner index*/ + // calc nearest corner index setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step - - // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 0.3); - bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.3); - // Set lf, lr lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center, minimum of 1.5m lr_ = std::max(bounding_box_.length * 0.25, 1.5); // 25% rear from the center, minimum of 1.5m @@ -169,6 +164,7 @@ BigVehicleTracker::BigVehicleTracker( bool BigVehicleTracker::predict(const rclcpp::Time & time) { + // MOTION MODEL (predict) const double dt = (time - last_update_time_).seconds(); if (dt < 0.0) { RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); @@ -218,6 +214,8 @@ bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const * */ + // MOTION MODEL (predict) + // Current state vector X t Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state ekf.getX(X_t); @@ -314,27 +312,11 @@ bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const bool BigVehicleTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - // predicted state + // current (predicted) state Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); ekf_.getX(X_t); - // get measurement yaw angle to update - double measurement_yaw = 0.0; - bool is_yaw_available = utils::getMeasurementYaw(object, X_t(IDX::YAW), measurement_yaw); - - // velocity capability is checked only when the object has velocity measurement - // and the predicted velocity is close to the observed velocity - bool is_velocity_available = false; - if (object.kinematics.has_twist) { - const double & predicted_vel = X_t(IDX::VEL); - const double & observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; - if (std::fabs(predicted_vel - observed_vel) < velocity_deviation_threshold_) { - // Velocity deviation is small - is_velocity_available = true; - } - } - - // Object Shape + // OBJECT SHAPE MODEL // convert to bounding box if input is convex shape autoware_auto_perception_msgs::msg::DetectedObject bbox_object; if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { @@ -349,6 +331,7 @@ bool BigVehicleTracker::measureWithPose( last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_, bbox_object, X_t(IDX::YAW), offset_object, tracking_offset_); + // UNCERTAINTY MODEL // measurement noise covariance float r_cov_x; float r_cov_y; @@ -368,6 +351,24 @@ bool BigVehicleTracker::measureWithPose( r_cov_y = ekf_params_.r_cov_y; } + // MOTION MODEL (update) + + // get measurement yaw angle to update + double measurement_yaw = 0.0; + bool is_yaw_available = utils::getMeasurementYaw(object, X_t(IDX::YAW), measurement_yaw); + + // velocity capability is checked only when the object has velocity measurement + // and the predicted velocity is close to the observed velocity + bool is_velocity_available = false; + if (object.kinematics.has_twist) { + const double & predicted_vel = X_t(IDX::VEL); + const double & observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; + if (std::fabs(predicted_vel - observed_vel) < velocity_deviation_threshold_) { + // Velocity deviation is small + is_velocity_available = true; + } + } + // Decide dimension of measurement vector and matrix // pos x, pos y, pos yaw, (vel) depending on pose measurement const int dim_y = is_velocity_available ? 4 : 3; @@ -381,6 +382,10 @@ bool BigVehicleTracker::measureWithPose( C(0, IDX::X) = 1.0; // for pos x C(1, IDX::Y) = 1.0; // for pos y C(2, IDX::YAW) = 1.0; // for yaw + if (is_velocity_available) { + Y(IDX::VEL, 0) = object.kinematics.twist_with_covariance.twist.linear.x; + C(3, IDX::VEL) = 1.0; // for vel + } // Set noise covariance matrix R Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); @@ -396,6 +401,9 @@ bool BigVehicleTracker::measureWithPose( if (!is_yaw_available) { R(2, 2) *= 1e3; // yaw is not available, multiply large value } + if (is_velocity_available) { + R(3, 3) = ekf_params_.r_cov_vel; // vel -vel + } } else { R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; @@ -406,14 +414,7 @@ bool BigVehicleTracker::measureWithPose( R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X]; R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y]; R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - } - // update the velocity when it is available - if (is_velocity_available) { - Y(IDX::VEL, 0) = object.kinematics.twist_with_covariance.twist.linear.x; - C(3, IDX::VEL) = 1.0; // for vel - if (!object.kinematics.has_twist_covariance) { - R(3, 3) = ekf_params_.r_cov_vel; // vel -vel - } else { + if (is_velocity_available) { R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } } diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 3b08d280199b9..5e9d5c696913f 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -105,6 +105,7 @@ NormalVehicleTracker::NormalVehicleTracker( X(IDX::VEL) = 0.0; } + // UNCERTAINTY MODEL // initialize state covariance matrix P Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); if (!object.kinematics.has_position_covariance) { @@ -138,13 +139,15 @@ NormalVehicleTracker::NormalVehicleTracker( P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } + // MOTION MODEL (set initial state and covariance) + ekf_.init(X, P); + + // OBJECT SHAPE MODEL if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; last_input_bounding_box_ = bounding_box_; } else { - // past default value - // bounding_box_ = {4.0, 1.7, 2.0}; autoware_auto_perception_msgs::msg::DetectedObject bbox_object; utils::convertConvexHullToBoundingBox(object, bbox_object); bounding_box_ = { @@ -152,16 +155,8 @@ NormalVehicleTracker::NormalVehicleTracker( bbox_object.shape.dimensions.z}; last_input_bounding_box_ = bounding_box_; } - ekf_.init(X, P); - - /* calc nearest corner index*/ + // calc nearest corner index setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step - - // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 0.3); - bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.3); - // Set lf, lr lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center, minimum of 1.0m lr_ = std::max(bounding_box_.length * 0.25, 1.0); // 25% rear from the center, minimum of 1.0m @@ -169,6 +164,7 @@ NormalVehicleTracker::NormalVehicleTracker( bool NormalVehicleTracker::predict(const rclcpp::Time & time) { + // MOTION MODEL (predict) const double dt = (time - last_update_time_).seconds(); if (dt < 0.0) { RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); @@ -218,6 +214,8 @@ bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const * */ + // MOTION MODEL (predict) + // Current state vector X t Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state ekf.getX(X_t); @@ -314,27 +312,11 @@ bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const bool NormalVehicleTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - // predicted state + // current (predicted) state Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); ekf_.getX(X_t); - // get measurement yaw angle to update - double measurement_yaw = 0.0; - bool is_yaw_available = utils::getMeasurementYaw(object, X_t(IDX::YAW), measurement_yaw); - - // velocity capability is checked only when the object has velocity measurement - // and the predicted velocity is close to the observed velocity - bool is_velocity_available = false; - if (object.kinematics.has_twist) { - const double & predicted_vel = X_t(IDX::VEL); - const double & observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; - if (std::fabs(predicted_vel - observed_vel) < velocity_deviation_threshold_) { - // Velocity deviation is small - is_velocity_available = true; - } - } - - // Object Shape + // OBJECT SHAPE MODEL // convert to bounding box if input is convex shape autoware_auto_perception_msgs::msg::DetectedObject bbox_object; if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { @@ -349,6 +331,7 @@ bool NormalVehicleTracker::measureWithPose( last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_, bbox_object, X_t(IDX::YAW), offset_object, tracking_offset_); + // UNCERTAINTY MODEL // measurement noise covariance float r_cov_x; float r_cov_y; @@ -368,6 +351,24 @@ bool NormalVehicleTracker::measureWithPose( r_cov_y = ekf_params_.r_cov_y; } + // MOTION MODEL (update) + + // get measurement yaw angle to update + double measurement_yaw = 0.0; + bool is_yaw_available = utils::getMeasurementYaw(object, X_t(IDX::YAW), measurement_yaw); + + // velocity capability is checked only when the object has velocity measurement + // and the predicted velocity is close to the observed velocity + bool is_velocity_available = false; + if (object.kinematics.has_twist) { + const double & predicted_vel = X_t(IDX::VEL); + const double & observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; + if (std::fabs(predicted_vel - observed_vel) < velocity_deviation_threshold_) { + // Velocity deviation is small + is_velocity_available = true; + } + } + // Decide dimension of measurement vector and matrix // pos x, pos y, pos yaw, (vel) depending on pose measurement const int dim_y = is_velocity_available ? 4 : 3; @@ -381,6 +382,10 @@ bool NormalVehicleTracker::measureWithPose( C(0, IDX::X) = 1.0; // for pos x C(1, IDX::Y) = 1.0; // for pos y C(2, IDX::YAW) = 1.0; // for yaw + if (is_velocity_available) { + Y(IDX::VEL, 0) = object.kinematics.twist_with_covariance.twist.linear.x; + C(3, IDX::VEL) = 1.0; // for vel + } // Set noise covariance matrix R Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); @@ -396,6 +401,9 @@ bool NormalVehicleTracker::measureWithPose( if (!is_yaw_available) { R(2, 2) *= 1e3; // yaw is not available, multiply large value } + if (is_velocity_available) { + R(3, 3) = ekf_params_.r_cov_vel; // vel -vel + } } else { R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; @@ -406,14 +414,7 @@ bool NormalVehicleTracker::measureWithPose( R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X]; R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y]; R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - } - // update the velocity when it is available - if (is_velocity_available) { - Y(IDX::VEL, 0) = object.kinematics.twist_with_covariance.twist.linear.x; - C(3, IDX::VEL) = 1.0; // for vel - if (!object.kinematics.has_twist_covariance) { - R(3, 3) = ekf_params_.r_cov_vel; // vel -vel - } else { + if (is_velocity_available) { R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } } From e7d99dff62189a4977a84d338700705b6b5147b0 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 1 Mar 2024 17:35:29 +0900 Subject: [PATCH 07/42] fix: separate object pre-process getUpdatingObject Signed-off-by: Taekjin LEE --- .../tracker/model/big_vehicle_tracker.hpp | 5 +- .../tracker/model/normal_vehicle_tracker.hpp | 5 +- .../src/tracker/model/big_vehicle_tracker.cpp | 186 +++++++++--------- .../tracker/model/normal_vehicle_tracker.cpp | 186 +++++++++--------- 4 files changed, 192 insertions(+), 190 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index b41850c832baf..de33ca7e37afb 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -27,7 +27,6 @@ class BigVehicleTracker : public Tracker private: autoware_auto_perception_msgs::msg::DetectedObject object_; rclcpp::Logger logger_; - int last_nearest_corner_index_; private: KalmanFilter ekf_; @@ -81,12 +80,14 @@ class BigVehicleTracker : public Tracker bool measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; + autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const override; - void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform); virtual ~BigVehicleTracker() {} }; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 5577636944fb3..ce63a9ef05aa5 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -28,7 +28,6 @@ class NormalVehicleTracker : public Tracker private: autoware_auto_perception_msgs::msg::DetectedObject object_; rclcpp::Logger logger_; - int last_nearest_corner_index_; private: KalmanFilter ekf_; @@ -84,12 +83,14 @@ class NormalVehicleTracker : public Tracker bool measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; + autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const override; - void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform); virtual ~NormalVehicleTracker() {} }; diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index fd42b10b1497a..7c42b4219339e 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -44,7 +44,7 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; BigVehicleTracker::BigVehicleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform) + const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("BigVehicleTracker")), last_update_time_(time), @@ -155,8 +155,6 @@ BigVehicleTracker::BigVehicleTracker( bbox_object.shape.dimensions.z}; last_input_bounding_box_ = bounding_box_; } - // calc nearest corner index - setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step // Set lf, lr lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center, minimum of 1.5m lr_ = std::max(bounding_box_.length * 0.25, 1.5); // 25% rear from the center, minimum of 1.5m @@ -309,9 +307,12 @@ bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const return true; } -bool BigVehicleTracker::measureWithPose( - const autoware_auto_perception_msgs::msg::DetectedObject & object) +autoware_auto_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform) { + autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; + // current (predicted) state Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); ekf_.getX(X_t); @@ -324,38 +325,80 @@ bool BigVehicleTracker::measureWithPose( } else { bbox_object = object; } - // extension (size) adjustment + // get offset measurement - autoware_auto_perception_msgs::msg::DetectedObject offset_object; + int nearest_corner_index = utils::getNearestCornerOrSurface( + X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, + self_transform); utils::calcAnchorPointOffset( - last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_, - bbox_object, X_t(IDX::YAW), offset_object, tracking_offset_); + last_input_bounding_box_.width, last_input_bounding_box_.length, nearest_corner_index, + bbox_object, X_t(IDX::YAW), updating_object, tracking_offset_); // UNCERTAINTY MODEL - // measurement noise covariance - float r_cov_x; - float r_cov_y; - using Label = autoware_auto_perception_msgs::msg::ObjectClassification; - const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); - if (utils::isLargeVehicleLabel(label)) { - r_cov_x = ekf_params_.r_cov_x; - r_cov_y = ekf_params_.r_cov_y; - } else if (label == Label::CAR) { - // if label is changed, enlarge the measurement noise covariance - constexpr float r_stddev_x = 2.0; // [m] - constexpr float r_stddev_y = 2.0; // [m] - r_cov_x = std::pow(r_stddev_x, 2.0); - r_cov_y = std::pow(r_stddev_y, 2.0); - } else { - r_cov_x = ekf_params_.r_cov_x; - r_cov_y = ekf_params_.r_cov_y; + if (!object.kinematics.has_position_covariance) { + // measurement noise covariance + float r_cov_x; + float r_cov_y; + using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); + if (label == Label::CAR) { + r_cov_x = ekf_params_.r_cov_x; + r_cov_y = ekf_params_.r_cov_y; + } else if (utils::isLargeVehicleLabel(label)) { + // if label is changed, enlarge the measurement noise covariance + constexpr float r_stddev_x = 2.0; // [m] + constexpr float r_stddev_y = 2.0; // [m] + r_cov_x = std::pow(r_stddev_x, 2.0); + r_cov_y = std::pow(r_stddev_y, 2.0); + } else { + r_cov_x = ekf_params_.r_cov_x; + r_cov_y = ekf_params_.r_cov_y; + } + + // yaw angle fix + double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + bool is_yaw_available = + object.kinematics.orientation_availability != + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + + // fill covariance matrix + auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; + const double cos_yaw = std::cos(pose_yaw); + const double sin_yaw = std::sin(pose_yaw); + const double sin_2yaw = std::sin(2.0f * pose_yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + pose_cov[utils::MSG_COV_IDX::Y_Y] = + r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; // y - x + pose_cov[utils::MSG_COV_IDX::X_YAW] = 0.0; // x - yaw + pose_cov[utils::MSG_COV_IDX::Y_YAW] = 0.0; // y - yaw + pose_cov[utils::MSG_COV_IDX::YAW_X] = 0.0; // yaw - x + pose_cov[utils::MSG_COV_IDX::YAW_Y] = 0.0; // yaw - y + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = ekf_params_.r_cov_yaw; // yaw - yaw + if (!is_yaw_available) { + pose_cov[utils::MSG_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value + } + auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance; + twist_cov[utils::MSG_COV_IDX::X_X] = ekf_params_.r_cov_vel; // vel - vel } + return updating_object; +} + +bool BigVehicleTracker::measureWithPose( + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + // current (predicted) state + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); + ekf_.getX(X_t); + // MOTION MODEL (update) // get measurement yaw angle to update double measurement_yaw = 0.0; - bool is_yaw_available = utils::getMeasurementYaw(object, X_t(IDX::YAW), measurement_yaw); + utils::getMeasurementYaw(object, X_t(IDX::YAW), measurement_yaw); // velocity capability is checked only when the object has velocity measurement // and the predicted velocity is close to the observed velocity @@ -376,8 +419,8 @@ bool BigVehicleTracker::measureWithPose( // Set measurement matrix C and observation vector Y Eigen::MatrixXd Y(dim_y, 1); Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); - Y(IDX::X, 0) = offset_object.kinematics.pose_with_covariance.pose.position.x; - Y(IDX::Y, 0) = offset_object.kinematics.pose_with_covariance.pose.position.y; + Y(IDX::X, 0) = object.kinematics.pose_with_covariance.pose.position.x; + Y(IDX::Y, 0) = object.kinematics.pose_with_covariance.pose.position.y; Y(IDX::YAW, 0) = measurement_yaw; C(0, IDX::X) = 1.0; // for pos x C(1, IDX::Y) = 1.0; // for pos y @@ -389,34 +432,17 @@ bool BigVehicleTracker::measureWithPose( // Set noise covariance matrix R Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - if (!object.kinematics.has_position_covariance) { - const double cos_yaw = std::cos(measurement_yaw); - const double sin_yaw = std::sin(measurement_yaw); - const double sin_2yaw = std::sin(2.0f * measurement_yaw); - R(0, 0) = r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - R(0, 1) = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y - R(1, 1) = r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y - R(1, 0) = R(0, 1); // y - x - R(2, 2) = ekf_params_.r_cov_yaw; // yaw - yaw - if (!is_yaw_available) { - R(2, 2) *= 1e3; // yaw is not available, multiply large value - } - if (is_velocity_available) { - R(3, 3) = ekf_params_.r_cov_vel; // vel -vel - } - } else { - R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - R(0, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_YAW]; - R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - R(1, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_YAW]; - R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X]; - R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y]; - R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - if (is_velocity_available) { - R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - } + R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + R(0, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_YAW]; + R(1, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_YAW]; + R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X]; + R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y]; + R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; + if (is_velocity_available) { + R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } // ekf update @@ -450,22 +476,14 @@ bool BigVehicleTracker::measureWithPose( bool BigVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - // if the input shape is convex type, convert it to bbox type - autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - utils::convertConvexHullToBoundingBox(object, bbox_object); - } else { - bbox_object = object; - } - constexpr float gain = 0.8; - bounding_box_.length = - gain * bounding_box_.length + (1.0 - gain) * bbox_object.shape.dimensions.x; - bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * bbox_object.shape.dimensions.y; - bounding_box_.height = - gain * bounding_box_.height + (1.0 - gain) * bbox_object.shape.dimensions.z; + + // update object size + bounding_box_.length = gain * bounding_box_.length + (1.0 - gain) * object.shape.dimensions.x; + bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * object.shape.dimensions.y; + bounding_box_.height = gain * bounding_box_.height + (1.0 - gain) * object.shape.dimensions.z; last_input_bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; // update offset into position Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); @@ -501,11 +519,11 @@ bool BigVehicleTracker::measure( (time - last_update_time_).seconds()); } - measureWithPose(object); - measureWithShape(object); + const autoware_auto_perception_msgs::msg::DetectedObject updating_object = + getUpdatingObject(object, self_transform); - /* calc nearest corner index*/ - setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step + measureWithPose(updating_object); + measureWithShape(updating_object); return true; } @@ -532,14 +550,6 @@ bool BigVehicleTracker::getTrackedObject( auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; - // recover bounding box from tracking point - const double dl = bounding_box_.length - last_input_bounding_box_.length; - const double dw = bounding_box_.width - last_input_bounding_box_.width; - const Eigen::Vector2d recovered_pose = utils::recoverFromTrackingPoint( - X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), dw, dl, last_nearest_corner_index_, tracking_offset_); - X_t(IDX::X) = recovered_pose.x(); - X_t(IDX::Y) = recovered_pose.y(); - // position pose_with_cov.pose.position.x = X_t(IDX::X); pose_with_cov.pose.position.y = X_t(IDX::Y); @@ -624,13 +634,3 @@ bool BigVehicleTracker::getTrackedObject( return true; } - -void BigVehicleTracker::setNearestCornerOrSurfaceIndex( - const geometry_msgs::msg::Transform & self_transform) -{ - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - ekf_.getX(X_t); - last_nearest_corner_index_ = utils::getNearestCornerOrSurface( - X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, - self_transform); -} diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 5e9d5c696913f..bdab2547f09f1 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -44,7 +44,7 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; NormalVehicleTracker::NormalVehicleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform) + const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("NormalVehicleTracker")), last_update_time_(time), @@ -155,8 +155,6 @@ NormalVehicleTracker::NormalVehicleTracker( bbox_object.shape.dimensions.z}; last_input_bounding_box_ = bounding_box_; } - // calc nearest corner index - setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step // Set lf, lr lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center, minimum of 1.0m lr_ = std::max(bounding_box_.length * 0.25, 1.0); // 25% rear from the center, minimum of 1.0m @@ -309,9 +307,12 @@ bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const return true; } -bool NormalVehicleTracker::measureWithPose( - const autoware_auto_perception_msgs::msg::DetectedObject & object) +autoware_auto_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform) { + autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; + // current (predicted) state Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); ekf_.getX(X_t); @@ -324,38 +325,80 @@ bool NormalVehicleTracker::measureWithPose( } else { bbox_object = object; } - // extension (size) adjustment + // get offset measurement - autoware_auto_perception_msgs::msg::DetectedObject offset_object; + int nearest_corner_index = utils::getNearestCornerOrSurface( + X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, + self_transform); utils::calcAnchorPointOffset( - last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_, - bbox_object, X_t(IDX::YAW), offset_object, tracking_offset_); + last_input_bounding_box_.width, last_input_bounding_box_.length, nearest_corner_index, + bbox_object, X_t(IDX::YAW), updating_object, tracking_offset_); // UNCERTAINTY MODEL - // measurement noise covariance - float r_cov_x; - float r_cov_y; - using Label = autoware_auto_perception_msgs::msg::ObjectClassification; - const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); - if (label == Label::CAR) { - r_cov_x = ekf_params_.r_cov_x; - r_cov_y = ekf_params_.r_cov_y; - } else if (utils::isLargeVehicleLabel(label)) { - // if label is changed, enlarge the measurement noise covariance - constexpr float r_stddev_x = 2.0; // [m] - constexpr float r_stddev_y = 2.0; // [m] - r_cov_x = std::pow(r_stddev_x, 2.0); - r_cov_y = std::pow(r_stddev_y, 2.0); - } else { - r_cov_x = ekf_params_.r_cov_x; - r_cov_y = ekf_params_.r_cov_y; + if (!object.kinematics.has_position_covariance) { + // measurement noise covariance + float r_cov_x; + float r_cov_y; + using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); + if (label == Label::CAR) { + r_cov_x = ekf_params_.r_cov_x; + r_cov_y = ekf_params_.r_cov_y; + } else if (utils::isLargeVehicleLabel(label)) { + // if label is changed, enlarge the measurement noise covariance + constexpr float r_stddev_x = 2.0; // [m] + constexpr float r_stddev_y = 2.0; // [m] + r_cov_x = std::pow(r_stddev_x, 2.0); + r_cov_y = std::pow(r_stddev_y, 2.0); + } else { + r_cov_x = ekf_params_.r_cov_x; + r_cov_y = ekf_params_.r_cov_y; + } + + // yaw angle fix + double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + bool is_yaw_available = + object.kinematics.orientation_availability != + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + + // fill covariance matrix + auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; + const double cos_yaw = std::cos(pose_yaw); + const double sin_yaw = std::sin(pose_yaw); + const double sin_2yaw = std::sin(2.0f * pose_yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + pose_cov[utils::MSG_COV_IDX::Y_Y] = + r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; // y - x + pose_cov[utils::MSG_COV_IDX::X_YAW] = 0.0; // x - yaw + pose_cov[utils::MSG_COV_IDX::Y_YAW] = 0.0; // y - yaw + pose_cov[utils::MSG_COV_IDX::YAW_X] = 0.0; // yaw - x + pose_cov[utils::MSG_COV_IDX::YAW_Y] = 0.0; // yaw - y + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = ekf_params_.r_cov_yaw; // yaw - yaw + if (!is_yaw_available) { + pose_cov[utils::MSG_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value + } + auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance; + twist_cov[utils::MSG_COV_IDX::X_X] = ekf_params_.r_cov_vel; // vel - vel } + return updating_object; +} + +bool NormalVehicleTracker::measureWithPose( + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + // current (predicted) state + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); + ekf_.getX(X_t); + // MOTION MODEL (update) // get measurement yaw angle to update double measurement_yaw = 0.0; - bool is_yaw_available = utils::getMeasurementYaw(object, X_t(IDX::YAW), measurement_yaw); + utils::getMeasurementYaw(object, X_t(IDX::YAW), measurement_yaw); // velocity capability is checked only when the object has velocity measurement // and the predicted velocity is close to the observed velocity @@ -376,8 +419,8 @@ bool NormalVehicleTracker::measureWithPose( // Set measurement matrix C and observation vector Y Eigen::MatrixXd Y(dim_y, 1); Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); - Y(IDX::X, 0) = offset_object.kinematics.pose_with_covariance.pose.position.x; - Y(IDX::Y, 0) = offset_object.kinematics.pose_with_covariance.pose.position.y; + Y(IDX::X, 0) = object.kinematics.pose_with_covariance.pose.position.x; + Y(IDX::Y, 0) = object.kinematics.pose_with_covariance.pose.position.y; Y(IDX::YAW, 0) = measurement_yaw; C(0, IDX::X) = 1.0; // for pos x C(1, IDX::Y) = 1.0; // for pos y @@ -389,34 +432,17 @@ bool NormalVehicleTracker::measureWithPose( // Set noise covariance matrix R Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - if (!object.kinematics.has_position_covariance) { - const double cos_yaw = std::cos(measurement_yaw); - const double sin_yaw = std::sin(measurement_yaw); - const double sin_2yaw = std::sin(2.0f * measurement_yaw); - R(0, 0) = r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - R(0, 1) = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y - R(1, 1) = r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y - R(1, 0) = R(0, 1); // y - x - R(2, 2) = ekf_params_.r_cov_yaw; // yaw - yaw - if (!is_yaw_available) { - R(2, 2) *= 1e3; // yaw is not available, multiply large value - } - if (is_velocity_available) { - R(3, 3) = ekf_params_.r_cov_vel; // vel -vel - } - } else { - R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - R(0, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_YAW]; - R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - R(1, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_YAW]; - R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X]; - R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y]; - R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - if (is_velocity_available) { - R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - } + R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + R(0, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_YAW]; + R(1, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_YAW]; + R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X]; + R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y]; + R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; + if (is_velocity_available) { + R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } // ekf update @@ -450,22 +476,14 @@ bool NormalVehicleTracker::measureWithPose( bool NormalVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - // if the input shape is convex type, convert it to bbox type - autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - utils::convertConvexHullToBoundingBox(object, bbox_object); - } else { - bbox_object = object; - } - constexpr float gain = 0.8; - bounding_box_.length = - gain * bounding_box_.length + (1.0 - gain) * bbox_object.shape.dimensions.x; - bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * bbox_object.shape.dimensions.y; - bounding_box_.height = - gain * bounding_box_.height + (1.0 - gain) * bbox_object.shape.dimensions.z; + + // update object size + bounding_box_.length = gain * bounding_box_.length + (1.0 - gain) * object.shape.dimensions.x; + bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * object.shape.dimensions.y; + bounding_box_.height = gain * bounding_box_.height + (1.0 - gain) * object.shape.dimensions.z; last_input_bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; // update offset into position Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); @@ -501,11 +519,11 @@ bool NormalVehicleTracker::measure( (time - last_update_time_).seconds()); } - measureWithPose(object); - measureWithShape(object); + const autoware_auto_perception_msgs::msg::DetectedObject updating_object = + getUpdatingObject(object, self_transform); - /* calc nearest corner index*/ - setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step + measureWithPose(updating_object); + measureWithShape(updating_object); return true; } @@ -532,14 +550,6 @@ bool NormalVehicleTracker::getTrackedObject( auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; - // recover bounding box from tracking point - const double dl = bounding_box_.length - last_input_bounding_box_.length; - const double dw = bounding_box_.width - last_input_bounding_box_.width; - const Eigen::Vector2d recovered_pose = utils::recoverFromTrackingPoint( - X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), dw, dl, last_nearest_corner_index_, tracking_offset_); - X_t(IDX::X) = recovered_pose.x(); - X_t(IDX::Y) = recovered_pose.y(); - // position pose_with_cov.pose.position.x = X_t(IDX::X); pose_with_cov.pose.position.y = X_t(IDX::Y); @@ -624,13 +634,3 @@ bool NormalVehicleTracker::getTrackedObject( return true; } - -void NormalVehicleTracker::setNearestCornerOrSurfaceIndex( - const geometry_msgs::msg::Transform & self_transform) -{ - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - ekf_.getX(X_t); - last_nearest_corner_index_ = utils::getNearestCornerOrSurface( - X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, - self_transform); -} From 1116da2ead3975addccc87dfb2d981ca285bd619 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 1 Mar 2024 18:10:53 +0900 Subject: [PATCH 08/42] fix: bicycle tracker object preprocessing Signed-off-by: Taekjin LEE --- .../tracker/model/bicycle_tracker.hpp | 3 + .../src/tracker/model/bicycle_tracker.cpp | 92 ++++++++++--------- .../src/tracker/model/big_vehicle_tracker.cpp | 40 ++++---- .../tracker/model/normal_vehicle_tracker.cpp | 36 ++++---- 4 files changed, 93 insertions(+), 78 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp index ba50933eddaec..8199e0b89290d 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -79,6 +79,9 @@ class BicycleTracker : public Tracker bool measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; + autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 5c361b3b29c32..efc9396b0d71c 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -297,6 +297,33 @@ bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const return true; } +autoware_auto_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & /*self_transform*/) +{ + autoware_auto_perception_msgs::msg::DetectedObject updating_object; + + // OBJECT SHAPE MODEL + // convert to bounding box if input is convex shape + if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + utils::convertConvexHullToBoundingBox(object, updating_object); + } else { + updating_object = object; + } + + // UNCERTAINTY MODEL + if (!object.kinematics.has_position_covariance) { + auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; + pose_cov[utils::MSG_COV_IDX::X_X] = ekf_params_.r_cov_x; // x - x + pose_cov[utils::MSG_COV_IDX::X_Y] = 0; // x - y + pose_cov[utils::MSG_COV_IDX::Y_X] = 0; // y - x + pose_cov[utils::MSG_COV_IDX::Y_Y] = ekf_params_.r_cov_y; // y - y + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = ekf_params_.r_cov_yaw; // yaw - yaw + } + + return updating_object; +} + bool BicycleTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { @@ -328,26 +355,17 @@ bool BicycleTracker::measureWithPose( // Set noise covariance matrix R Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - if (!object.kinematics.has_position_covariance) { - R(0, 0) = ekf_params_.r_cov_x; // x - x - R(0, 1) = 0.0; // x - y - R(1, 1) = ekf_params_.r_cov_y; // y - y - R(1, 0) = R(0, 1); // y - x - if (is_yaw_available) { - R(2, 2) = ekf_params_.r_cov_yaw; // yaw - yaw - } - } else { - R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - if (is_yaw_available) { - R(0, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_YAW]; - R(1, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_YAW]; - R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X]; - R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y]; - R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - } + const auto & pose_cov = object.kinematics.pose_with_covariance.covariance; + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + if (is_yaw_available) { + R(0, 2) = pose_cov[utils::MSG_COV_IDX::X_YAW]; + R(1, 2) = pose_cov[utils::MSG_COV_IDX::Y_YAW]; + R(2, 0) = pose_cov[utils::MSG_COV_IDX::YAW_X]; + R(2, 1) = pose_cov[utils::MSG_COV_IDX::YAW_Y]; + R(2, 2) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; } // ekf update @@ -381,27 +399,14 @@ bool BicycleTracker::measureWithPose( bool BicycleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - // if the input shape is convex type, convert it to bbox type - autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - bbox_object = object; - } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { - // convert cylinder to bbox - bbox_object.shape.dimensions.x = object.shape.dimensions.x; - bbox_object.shape.dimensions.y = object.shape.dimensions.x; - bbox_object.shape.dimensions.z = object.shape.dimensions.z; - } else { - utils::convertConvexHullToBoundingBox(object, bbox_object); - } - constexpr float gain = 0.9; - bounding_box_.length = - gain * bounding_box_.length + (1.0 - gain) * bbox_object.shape.dimensions.x; - bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * bbox_object.shape.dimensions.y; - bounding_box_.height = - gain * bounding_box_.height + (1.0 - gain) * bbox_object.shape.dimensions.z; + + // update object size + bounding_box_.length = gain * bounding_box_.length + (1.0 - gain) * object.shape.dimensions.x; + bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * object.shape.dimensions.y; + bounding_box_.height = gain * bounding_box_.height + (1.0 - gain) * object.shape.dimensions.z; last_input_bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; // set minimum size bounding_box_.length = std::max(bounding_box_.length, 0.3); @@ -417,7 +422,7 @@ bool BicycleTracker::measureWithShape( bool BicycleTracker::measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & /*self_transform*/) + const geometry_msgs::msg::Transform & self_transform) { const auto & current_classification = getClassification(); object_ = object; @@ -431,8 +436,11 @@ bool BicycleTracker::measure( (time - last_update_time_).seconds()); } - measureWithPose(object); - measureWithShape(object); + const autoware_auto_perception_msgs::msg::DetectedObject updating_object = + getUpdatingObject(object, self_transform); + + measureWithPose(updating_object); + measureWithShape(updating_object); return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 7c42b4219339e..ec93e6e81323d 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -432,15 +432,16 @@ bool BigVehicleTracker::measureWithPose( // Set noise covariance matrix R Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - R(0, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_YAW]; - R(1, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_YAW]; - R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X]; - R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y]; - R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; + const auto & pose_cov = object.kinematics.pose_with_covariance.covariance; + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + R(0, 2) = pose_cov[utils::MSG_COV_IDX::X_YAW]; + R(1, 2) = pose_cov[utils::MSG_COV_IDX::Y_YAW]; + R(2, 0) = pose_cov[utils::MSG_COV_IDX::YAW_X]; + R(2, 1) = pose_cov[utils::MSG_COV_IDX::YAW_Y]; + R(2, 2) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; if (is_velocity_available) { R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } @@ -476,12 +477,13 @@ bool BigVehicleTracker::measureWithPose( bool BigVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - constexpr float gain = 0.8; + constexpr float gain = 0.2; + constexpr float gain_inv = 1.0 - gain; // update object size - bounding_box_.length = gain * bounding_box_.length + (1.0 - gain) * object.shape.dimensions.x; - bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * object.shape.dimensions.y; - bounding_box_.height = gain * bounding_box_.height + (1.0 - gain) * object.shape.dimensions.z; + bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x; + bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y; + bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; last_input_bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; @@ -490,15 +492,15 @@ bool BigVehicleTracker::measureWithShape( Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); ekf_.getX(X_t); ekf_.getP(P_t); - X_t(IDX::X) = X_t(IDX::X) + (1.0 - gain) * tracking_offset_.x(); - X_t(IDX::Y) = X_t(IDX::Y) + (1.0 - gain) * tracking_offset_.y(); - tracking_offset_.x() = gain * tracking_offset_.x(); - tracking_offset_.y() = gain * tracking_offset_.y(); + X_t(IDX::X) = X_t(IDX::X) + gain * tracking_offset_.x(); + X_t(IDX::Y) = X_t(IDX::Y) + gain * tracking_offset_.y(); + tracking_offset_.x() = gain_inv * tracking_offset_.x(); + tracking_offset_.y() = gain_inv * tracking_offset_.y(); ekf_.init(X_t, P_t); // update lf, lr - lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center, minimum of 1.5m - lr_ = std::max(bounding_box_.length * 0.25, 1.5); // 25% rear from the center, minimum of 1.5m + lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center, minimum of 1.0m + lr_ = std::max(bounding_box_.length * 0.25, 1.0); // 25% rear from the center, minimum of 1.0m return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index bdab2547f09f1..f487772e2ab41 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -432,15 +432,16 @@ bool NormalVehicleTracker::measureWithPose( // Set noise covariance matrix R Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - R(0, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_YAW]; - R(1, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_YAW]; - R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X]; - R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y]; - R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; + const auto & pose_cov = object.kinematics.pose_with_covariance.covariance; + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + R(0, 2) = pose_cov[utils::MSG_COV_IDX::X_YAW]; + R(1, 2) = pose_cov[utils::MSG_COV_IDX::Y_YAW]; + R(2, 0) = pose_cov[utils::MSG_COV_IDX::YAW_X]; + R(2, 1) = pose_cov[utils::MSG_COV_IDX::YAW_Y]; + R(2, 2) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; if (is_velocity_available) { R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } @@ -476,12 +477,13 @@ bool NormalVehicleTracker::measureWithPose( bool NormalVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - constexpr float gain = 0.8; + constexpr float gain = 0.2; + constexpr float gain_inv = 1.0 - gain; // update object size - bounding_box_.length = gain * bounding_box_.length + (1.0 - gain) * object.shape.dimensions.x; - bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * object.shape.dimensions.y; - bounding_box_.height = gain * bounding_box_.height + (1.0 - gain) * object.shape.dimensions.z; + bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x; + bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y; + bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; last_input_bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; @@ -490,10 +492,10 @@ bool NormalVehicleTracker::measureWithShape( Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); ekf_.getX(X_t); ekf_.getP(P_t); - X_t(IDX::X) = X_t(IDX::X) + (1.0 - gain) * tracking_offset_.x(); - X_t(IDX::Y) = X_t(IDX::Y) + (1.0 - gain) * tracking_offset_.y(); - tracking_offset_.x() = gain * tracking_offset_.x(); - tracking_offset_.y() = gain * tracking_offset_.y(); + X_t(IDX::X) = X_t(IDX::X) + gain * tracking_offset_.x(); + X_t(IDX::Y) = X_t(IDX::Y) + gain * tracking_offset_.y(); + tracking_offset_.x() = gain_inv * tracking_offset_.x(); + tracking_offset_.y() = gain_inv * tracking_offset_.y(); ekf_.init(X_t, P_t); // update lf, lr From 31c69fef96640bea0394cedd404fe46422e6843c Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 4 Mar 2024 15:13:13 +0900 Subject: [PATCH 09/42] feat: separated motion model class, not tested Signed-off-by: Taekjin LEE --- .../multi_object_tracker/CMakeLists.txt | 1 + .../motion_model/bicycle_motion_model.hpp | 115 +++++ .../src/motion_model/bicycle_motion_model.cpp | 412 ++++++++++++++++++ 3 files changed, 528 insertions(+) create mode 100644 perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp create mode 100644 perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt index f5fbc8ff950e8..dfe34c1e6a74f 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/multi_object_tracker/CMakeLists.txt @@ -33,6 +33,7 @@ set(MULTI_OBJECT_TRACKER_SRC src/tracker/model/pass_through_tracker.cpp src/data_association/data_association.cpp src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp + src/motion_model/bicycle_motion_model.cpp ) ament_auto_add_library(multi_object_tracker_node SHARED diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp new file mode 100644 index 0000000000000..462d4da3b21c2 --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp @@ -0,0 +1,115 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Taekjin Lee +// + +#ifndef MULTI_OBJECT_TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ +#define MULTI_OBJECT_TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ + +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include +#include + +#include "autoware_auto_perception_msgs/msg/detected_object.hpp" + +class BicycleMotionModel +{ +private: + // attributes + rclcpp::Logger logger_; + rclcpp::Time last_update_time_; + +private: + // state + KalmanFilter ekf_; + + // extended state + double lf_; + double lr_; + + // motion parameters + struct MotionParams + { + double q_stddev_acc_long; + double q_stddev_acc_lat; + double q_stddev_yaw_rate_min; + double q_stddev_yaw_rate_max; + double q_cov_slip_rate_min; + double q_cov_slip_rate_max; + double q_max_slip_angle; + double p0_cov_vel; + double p0_cov_slip; + double lf_ratio; + double lr_ratio; + double lf_min; + double lr_min; + double max_vel; + double max_slip; + double dt_max; + } motion_params_; + +protected: + enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; + const char DIM = 5; + +public: + BicycleMotionModel( + const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P, + const double & length); + + void setDefaultParams(); + + void setMotionParams( + const double & q_stddev_acc_long, const double & q_stddev_acc_lat, + const double & q_stddev_yaw_rate_min, const double & q_stddev_yaw_rate_max, + const double & q_stddev_slip_rate_min, const double & q_stddev_slip_rate_max, + const double & q_max_slip_angle, const double & lf_ratio, const double & lf_min, + const double & lr_ratio, const double & lr_min); + + void setMotionLimits(const double & max_vel, const double & max_slip); + + Eigen::MatrixXd getStateVector() const + { + Eigen::MatrixXd X_t(DIM, 1); + ekf_.getX(X_t); + return X_t; + } + + double getStateElement(unsigned int idx) const { return ekf_.getXelement(idx); } + + bool updateStatePose(const double & x, const double & y, const double (&pose_cov)[36]); + + bool updateStatePoseHead( + const double & x, const double & y, const double & yaw, const double (&pose_cov)[36]); + + bool updateStatePoseHeadVel( + const double & x, const double & y, const double & yaw, const double & vel, + const double (&pose_cov)[36], const double (&twist_cov)[36]); + + bool limitStates(); + + bool updateExtendedState(const double & length); + + bool predictState(const rclcpp::Time & time); + + bool predictState(const double dt, KalmanFilter & ekf) const; + + bool getPredictedState(const rclcpp::Time & time, Eigen::MatrixXd & X, Eigen::MatrixXd & P) const; +}; + +#endif // MULTI_OBJECT_TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp new file mode 100644 index 0000000000000..ae135f76bc3bc --- /dev/null +++ b/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp @@ -0,0 +1,412 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Taekjin Lee +// + +#include "multi_object_tracker/motion_model/bicycle_motion_model.hpp" + +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +BicycleMotionModel::BicycleMotionModel( + const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P, + const double & length) +: logger_(rclcpp::get_logger("BicycleMotionModel")), last_update_time_(time) +{ + // Initialize motion parameters + setDefaultParams(); + + // initialize Kalman filter + ekf_.init(X, P); + + // set initial extended state + updateExtendedState(length); +} + +void BicycleMotionModel::setDefaultParams() +{ + // set default motion parameters + constexpr double q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + constexpr double q_stddev_yaw_rate_min = 1.5; // [deg/s] uncertain yaw change rate + constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate + constexpr double q_stddev_slip_rate_min = 0.3; // [deg/s] uncertain slip angle change rate + constexpr double q_stddev_slip_rate_max = 10.0; // [deg/s] uncertain slip angle change rate + constexpr double q_max_slip_angle = 30.0; // [deg] max slip angle + // extended state parameters + constexpr double lf_ratio = 0.3; // 30% front from the center + constexpr double lf_min = 1.0; // minimum of 1.0m + constexpr double lr_ratio = 0.25; // 25% rear from the center + constexpr double lr_min = 1.0; // minimum of 1.0m + setMotionParams( + q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max, + q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio, + lr_min); + + // set motion limitations + constexpr double max_vel = tier4_autoware_utils::kmph2mps(100); // [m/s] + constexpr double max_slip = 30.0; // [deg] + setMotionLimits(max_vel, max_slip); + + // set prediction parameters + constexpr double dt_max = 0.11; // [s] + motion_params_.dt_max = dt_max; + + // set initial covariance for velocity and slip angle + constexpr double p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); + constexpr double p0_stddev_slip = tier4_autoware_utils::deg2rad(5); + motion_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0); + motion_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); +} + +void BicycleMotionModel::setMotionParams( + const double & q_stddev_acc_long, const double & q_stddev_acc_lat, + const double & q_stddev_yaw_rate_min, const double & q_stddev_yaw_rate_max, + const double & q_stddev_slip_rate_min, const double & q_stddev_slip_rate_max, + const double & q_max_slip_angle, const double & lf_ratio, const double & lf_min, + const double & lr_ratio, const double & lr_min) +{ + // set process noise covariance parameters + motion_params_.q_stddev_acc_long = q_stddev_acc_long; + motion_params_.q_stddev_acc_lat = q_stddev_acc_lat; + motion_params_.q_stddev_yaw_rate_min = tier4_autoware_utils::deg2rad(q_stddev_yaw_rate_min); + motion_params_.q_stddev_yaw_rate_max = tier4_autoware_utils::deg2rad(q_stddev_yaw_rate_max); + motion_params_.q_cov_slip_rate_min = + std::pow(tier4_autoware_utils::deg2rad(q_stddev_slip_rate_min), 2.0); + motion_params_.q_cov_slip_rate_max = + std::pow(tier4_autoware_utils::deg2rad(q_stddev_slip_rate_max), 2.0); + motion_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(q_max_slip_angle); + + // set extended state parameters + motion_params_.lf_ratio = lf_ratio; + motion_params_.lf_min = lf_min; + motion_params_.lr_ratio = lr_ratio; + motion_params_.lr_min = lr_min; +} + +void BicycleMotionModel::setMotionLimits(const double & max_vel, const double & max_slip) +{ + // set motion limitations + motion_params_.max_vel = max_vel; + motion_params_.max_slip = tier4_autoware_utils::deg2rad(max_slip); +} + +bool BicycleMotionModel::updateStatePose( + const double & x, const double & y, const double (&pose_cov)[36]) +{ + // update state, without velocity + constexpr int DIM_Y = 2; + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + + return ekf_.update(Y, C, R); +} + +bool BicycleMotionModel::updateStatePoseHead( + const double & x, const double & y, const double & yaw, const double (&pose_cov)[36]) +{ + // update state, without velocity + constexpr int DIM_Y = 3; + + // fix yaw + double estimated_yaw = getStateElement(IDX::YAW); + double fixed_yaw = yaw; + double limiting_delta_yaw = M_PI_2; + while (std::fabs(estimated_yaw - fixed_yaw) > limiting_delta_yaw) { + if (fixed_yaw < estimated_yaw) { + fixed_yaw += 2 * limiting_delta_yaw; + } else { + fixed_yaw -= 2 * limiting_delta_yaw; + } + } + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y, fixed_yaw; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + C(2, IDX::YAW) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + R(0, 2) = pose_cov[utils::MSG_COV_IDX::X_YAW]; + R(1, 2) = pose_cov[utils::MSG_COV_IDX::Y_YAW]; + R(2, 0) = pose_cov[utils::MSG_COV_IDX::YAW_X]; + R(2, 1) = pose_cov[utils::MSG_COV_IDX::YAW_Y]; + R(2, 2) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; + + return ekf_.update(Y, C, R); +} + +bool BicycleMotionModel::updateStatePoseHeadVel( + const double & x, const double & y, const double & yaw, const double & vel, + const double (&pose_cov)[36], const double (&twist_cov)[36]) +{ + // update state, with velocity + constexpr int DIM_Y = 4; + + // fix yaw + double estimated_yaw = getStateElement(IDX::YAW); + double fixed_yaw = yaw; + double limiting_delta_yaw = M_PI_2; + while (std::fabs(estimated_yaw - fixed_yaw) > limiting_delta_yaw) { + if (fixed_yaw < estimated_yaw) { + fixed_yaw += 2 * limiting_delta_yaw; + } else { + fixed_yaw -= 2 * limiting_delta_yaw; + } + } + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y, yaw, vel; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + C(2, IDX::YAW) = 1.0; + C(3, IDX::VEL) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + R(0, 2) = pose_cov[utils::MSG_COV_IDX::X_YAW]; + R(1, 2) = pose_cov[utils::MSG_COV_IDX::Y_YAW]; + R(2, 0) = pose_cov[utils::MSG_COV_IDX::YAW_X]; + R(2, 1) = pose_cov[utils::MSG_COV_IDX::YAW_Y]; + R(2, 2) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; + R(3, 3) = twist_cov[utils::MSG_COV_IDX::X_X]; + + return ekf_.update(Y, C, R); +} + +bool BicycleMotionModel::limitStates() +{ + Eigen::MatrixXd X_t(DIM, 1); + Eigen::MatrixXd P_t(DIM, DIM); + ekf_.getX(X_t); + ekf_.getP(P_t); + X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); + if (!(-motion_params_.max_vel <= X_t(IDX::VEL) && X_t(IDX::VEL) <= motion_params_.max_vel)) { + X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -motion_params_.max_vel : motion_params_.max_vel; + } + if (!(-motion_params_.max_slip <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= motion_params_.max_slip)) { + X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -motion_params_.max_slip : motion_params_.max_slip; + } + ekf_.init(X_t, P_t); + + return true; +} + +bool BicycleMotionModel::updateExtendedState(const double & length) +{ + lf_ = std::max(length * motion_params_.lf_ratio, motion_params_.lf_min); + lr_ = std::max(length * motion_params_.lr_ratio, motion_params_.lr_min); + return true; +} + +bool BicycleMotionModel::predictState(const rclcpp::Time & time) +{ + const double dt = (time - last_update_time_).seconds(); + if (dt < 0.0) { + RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); + return false; + } + // if dt is too large, shorten dt and repeat prediction + const uint32_t repeat = std::ceil(dt / motion_params_.dt_max); + const double dt_ = dt / repeat; + bool ret = false; + for (uint32_t i = 0; i < repeat; ++i) { + ret = predictState(dt_, ekf_); + if (!ret) { + return false; + } + } + if (ret) { + last_update_time_ = time; + } + return ret; +} + +bool BicycleMotionModel::predictState(const double dt, KalmanFilter & ekf) const +{ + /* Motion model: static bicycle model (constant slip angle, constant velocity) + * + * w_k = vel_k * sin(slip_k) / l_r + * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt + * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt + * yaw_{k+1} = yaw_k + w_k * dt + * vel_{k+1} = vel_k + * slip_{k+1} = slip_k + * + */ + + /* Jacobian Matrix + * + * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, + * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, + * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] + * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, + * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, + * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] + * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/l_r*cos(slip)*dt] + * [ 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 1] + * + */ + + // MOTION MODEL (predict) + + // Current state vector X t + Eigen::MatrixXd X_t = getStateVector(); + + 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)); + const double vel = X_t(IDX::VEL); + const double cos_slip = std::cos(X_t(IDX::SLIP)); + const double sin_slip = std::sin(X_t(IDX::SLIP)); + + double w = vel * sin_slip / lr_; + const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); + const double w_dtdt = w * dt * dt; + const double vv_dtdt__lr = vel * vel * dt * dt / lr_; + + // Predict state vector X t+1 + Eigen::MatrixXd X_next_t(DIM, 1); // predicted state + X_next_t(IDX::X) = + X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt + X_next_t(IDX::Y) = + X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt + X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt + X_next_t(IDX::VEL) = X_t(IDX::VEL); + X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) + + // State transition matrix A + Eigen::MatrixXd A = Eigen::MatrixXd::Identity(DIM, DIM); + A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; + A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; + A(IDX::X, IDX::SLIP) = + -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; + A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; + A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; + A(IDX::Y, IDX::SLIP) = + vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; + A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; + A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; + + // Process noise covariance Q + double q_stddev_yaw_rate{0.0}; + if (vel <= 0.01) { + q_stddev_yaw_rate = motion_params_.q_stddev_yaw_rate_min; + } else { + // uncertainty of the yaw rate is limited by + // centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v + // or maximum slip angle slip_max : w = v*sin(slip_max)/l_r + q_stddev_yaw_rate = std::min( + motion_params_.q_stddev_acc_lat / vel, + vel * std::sin(motion_params_.q_max_slip_angle) / lr_); // [rad/s] + q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, motion_params_.q_stddev_yaw_rate_max); + q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, motion_params_.q_stddev_yaw_rate_min); + } + double q_cov_slip_rate{0.0}; + if (vel <= 0.01) { + q_cov_slip_rate = motion_params_.q_cov_slip_rate_min; + } else { + // The slip angle rate uncertainty is modeled as follows: + // d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt + // where sin(slip) = w * l_r / v + // d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) + // d(v)/dt and d(w)/t are considered to be uncorrelated + q_cov_slip_rate = + std::pow(motion_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); + q_cov_slip_rate = std::min(q_cov_slip_rate, motion_params_.q_cov_slip_rate_max); + q_cov_slip_rate = std::max(q_cov_slip_rate, motion_params_.q_cov_slip_rate_min); + } + const double q_cov_x = std::pow(0.5 * motion_params_.q_stddev_acc_long * dt * dt, 2); + const double q_cov_y = std::pow(0.5 * motion_params_.q_stddev_acc_lat * dt * dt, 2); + const double q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); + const double q_cov_vel = std::pow(motion_params_.q_stddev_acc_long * dt, 2); + const double q_cov_slip = q_cov_slip_rate * dt * dt; + + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(DIM, DIM); + // Rotate the covariance matrix according to the vehicle yaw + // because q_cov_x and y are in the vehicle coordinate system. + Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); + Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); + Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); + Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); + Q(IDX::YAW, IDX::YAW) = q_cov_yaw; + Q(IDX::VEL, IDX::VEL) = q_cov_vel; + Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; + + // control-input model B and control-input u are not used + // Eigen::MatrixXd B = Eigen::MatrixXd::Zero(DIM, DIM); + // Eigen::MatrixXd u = Eigen::MatrixXd::Zero(DIM, 1); + + // predict state + return ekf.predict(X_next_t, A, Q); +} + +bool BicycleMotionModel::getPredictedState( + const rclcpp::Time & time, Eigen::MatrixXd & X, Eigen::MatrixXd & P) const +{ + // copy the predicted state and covariance + KalmanFilter tmp_ekf_for_no_update = ekf_; + + const double dt = (time - last_update_time_).seconds(); + if (dt < 0.0) { + RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); + return false; + } + // if dt is too large, shorten dt and repeat prediction + const uint32_t repeat = std::ceil(dt / motion_params_.dt_max); + const double dt_ = dt / repeat; + bool ret = false; + for (uint32_t i = 0; i < repeat; ++i) { + ret = predictState(dt_, tmp_ekf_for_no_update); + if (!ret) { + return false; + } + } + tmp_ekf_for_no_update.getX(X); + tmp_ekf_for_no_update.getP(P); + return ret; +} From 80e3a7efc29b41a3c56af0c1826163d17378f649 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 4 Mar 2024 17:28:50 +0900 Subject: [PATCH 10/42] feat: add motion model Signed-off-by: Taekjin LEE --- .../motion_model/bicycle_motion_model.hpp | 24 +++++++-- .../tracker/model/normal_vehicle_tracker.hpp | 4 ++ .../src/motion_model/bicycle_motion_model.cpp | 40 ++++++++++++--- .../tracker/model/normal_vehicle_tracker.cpp | 51 +++++++++++++++++++ 4 files changed, 108 insertions(+), 11 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp index 462d4da3b21c2..c27cb1628c7f7 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp @@ -36,6 +36,7 @@ class BicycleMotionModel private: // state + bool is_initialized_{false}; KalmanFilter ekf_; // extended state @@ -68,10 +69,22 @@ class BicycleMotionModel const char DIM = 5; public: - BicycleMotionModel( + BicycleMotionModel(); + + bool init( const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P, const double & length); + bool checkInitialized() const + { + // if the state is not initialized, return false + if (!is_initialized_) { + RCLCPP_WARN(logger_, "BicycleMotionModel is not initialized."); + return false; + } + return true; + } + void setDefaultParams(); void setMotionParams( @@ -92,14 +105,17 @@ class BicycleMotionModel double getStateElement(unsigned int idx) const { return ekf_.getXelement(idx); } - bool updateStatePose(const double & x, const double & y, const double (&pose_cov)[36]); + bool updateStatePose(const double & x, const double & y, const std::array & pose_cov); bool updateStatePoseHead( - const double & x, const double & y, const double & yaw, const double (&pose_cov)[36]); + const double & x, const double & y, const double & yaw, + const std::array & pose_cov); bool updateStatePoseHeadVel( const double & x, const double & y, const double & yaw, const double & vel, - const double (&pose_cov)[36], const double (&twist_cov)[36]); + const std::array & pose_cov, const std::array & twist_cov); + + bool adjustPosition(const double & x, const double & y, const double & yaw); bool limitStates(); diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index ce63a9ef05aa5..0e5c0a8742031 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -19,6 +19,7 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ +#include "multi_object_tracker/motion_model/bicycle_motion_model.hpp" #include "tracker_base.hpp" #include @@ -73,6 +74,9 @@ class NormalVehicleTracker : public Tracker BoundingBox last_input_bounding_box_; Eigen::Vector2d tracking_offset_; +private: + BicycleMotionModel motion_model_; + public: NormalVehicleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, diff --git a/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp index ae135f76bc3bc..ed43dce367e83 100644 --- a/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp @@ -27,19 +27,30 @@ #include #include -BicycleMotionModel::BicycleMotionModel( - const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P, - const double & length) -: logger_(rclcpp::get_logger("BicycleMotionModel")), last_update_time_(time) +BicycleMotionModel::BicycleMotionModel() +: logger_(rclcpp::get_logger("BicycleMotionModel")), last_update_time_(rclcpp::Time(0, 0)) { // Initialize motion parameters setDefaultParams(); +} + +bool BicycleMotionModel::init( + const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P, + const double & length) +{ + // set last update time + last_update_time_ = time; // initialize Kalman filter ekf_.init(X, P); // set initial extended state updateExtendedState(length); + + // set initialized flag + is_initialized_ = true; + + return true; } void BicycleMotionModel::setDefaultParams() @@ -111,8 +122,11 @@ void BicycleMotionModel::setMotionLimits(const double & max_vel, const double & } bool BicycleMotionModel::updateStatePose( - const double & x, const double & y, const double (&pose_cov)[36]) + const double & x, const double & y, const std::array & pose_cov) { + // check if the state is initialized + if (!checkInitialized()) return false; + // update state, without velocity constexpr int DIM_Y = 2; @@ -134,8 +148,11 @@ bool BicycleMotionModel::updateStatePose( } bool BicycleMotionModel::updateStatePoseHead( - const double & x, const double & y, const double & yaw, const double (&pose_cov)[36]) + const double & x, const double & y, const double & yaw, const std::array & pose_cov) { + // check if the state is initialized + if (!checkInitialized()) return false; + // update state, without velocity constexpr int DIM_Y = 3; @@ -176,8 +193,11 @@ bool BicycleMotionModel::updateStatePoseHead( bool BicycleMotionModel::updateStatePoseHeadVel( const double & x, const double & y, const double & yaw, const double & vel, - const double (&pose_cov)[36], const double (&twist_cov)[36]) + const std::array & pose_cov, const std::array & twist_cov) { + // check if the state is initialized + if (!checkInitialized()) return false; + // update state, with velocity constexpr int DIM_Y = 4; @@ -245,6 +265,9 @@ bool BicycleMotionModel::updateExtendedState(const double & length) bool BicycleMotionModel::predictState(const rclcpp::Time & time) { + // check if the state is initialized + if (!checkInitialized()) return false; + const double dt = (time - last_update_time_).seconds(); if (dt < 0.0) { RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); @@ -388,6 +411,9 @@ bool BicycleMotionModel::predictState(const double dt, KalmanFilter & ekf) const bool BicycleMotionModel::getPredictedState( const rclcpp::Time & time, Eigen::MatrixXd & X, Eigen::MatrixXd & P) const { + // check if the state is initialized + if (!checkInitialized()) return false; + // copy the predicted state and covariance KalmanFilter tmp_ekf_for_no_update = ekf_; diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index f487772e2ab41..20857f3a7b7f0 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -158,6 +158,30 @@ NormalVehicleTracker::NormalVehicleTracker( // Set lf, lr lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center, minimum of 1.0m lr_ = std::max(bounding_box_.length * 0.25, 1.0); // 25% rear from the center, minimum of 1.0m + + // Set motion model + motion_model_.init(time, X, P, bounding_box_.length); + + // Set motion model parameters + motion_model_.setMotionParams( + 9.8 * 0.35, /* [m/(s*s)] uncertain longitudinal acceleration */ + 9.8 * 0.15, /* [m/(s*s)] uncertain lateral acceleration */ + 1.5, /* [deg/s] uncertain yaw change rate, minimum */ + 15.0, /* [deg/s] uncertain yaw change rate, maximum */ + 0.3, /* [deg/s] uncertain slip angle change rate, minimum */ + 10.0, /* [deg/s] uncertain slip angle change rate, maximum */ + 30, /* [deg] max slip angle */ + 0.3, /* [-] ratio of front wheel*/ + 1.0, /* [m] minimum front wheel position*/ + 0.25, /* [-] ratio of rear wheel*/ + 1.0 /* [m] minimum rear wheel position*/ + ); + + // Set motion limits + motion_model_.setMotionLimits( + tier4_autoware_utils::kmph2mps(100), /* [m/s] maximum velocity */ + 30 /* [deg] maximum slip angle */ + ); } bool NormalVehicleTracker::predict(const rclcpp::Time & time) @@ -182,6 +206,9 @@ bool NormalVehicleTracker::predict(const rclcpp::Time & time) if (ret) { last_update_time_ = time; } + + motion_model_.predictState(time); + return ret; } @@ -471,6 +498,24 @@ bool NormalVehicleTracker::measureWithPose( constexpr float gain = 0.9; z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; + // motion model + { + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double vel = object.kinematics.twist_with_covariance.twist.linear.x; + + if (is_velocity_available) { + motion_model_.updateStatePoseHeadVel( + x, y, yaw, vel, object.kinematics.pose_with_covariance.covariance, + object.kinematics.twist_with_covariance.covariance); + } else { + motion_model_.updateStatePoseHead( + x, y, yaw, object.kinematics.pose_with_covariance.covariance); + } + motion_model_.limitStates(); + } + return true; } @@ -502,6 +547,9 @@ bool NormalVehicleTracker::measureWithShape( lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center, minimum of 1.0m lr_ = std::max(bounding_box_.length * 0.25, 1.0); // 25% rear from the center, minimum of 1.0m + // update motion model + motion_model_.updateExtendedState(bounding_box_.length); + return true; } @@ -548,6 +596,9 @@ bool NormalVehicleTracker::getTrackedObject( tmp_ekf_for_no_update.getX(X_t); tmp_ekf_for_no_update.getP(P); + // predict from motion model + motion_model_.getPredictedState(time, X_t, P); + /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; From ff16d47ba52cfba36ee36f373691f61168cd92fa Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 4 Mar 2024 19:00:38 +0900 Subject: [PATCH 11/42] feat: replace tracker to motion model class Signed-off-by: Taekjin LEE --- .../motion_model/bicycle_motion_model.hpp | 5 +- .../tracker/model/normal_vehicle_tracker.hpp | 2 +- .../src/motion_model/bicycle_motion_model.cpp | 54 +++- .../tracker/model/normal_vehicle_tracker.cpp | 252 ++---------------- 4 files changed, 64 insertions(+), 249 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp index c27cb1628c7f7..70d3fcc1d1b45 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp @@ -115,7 +115,7 @@ class BicycleMotionModel const double & x, const double & y, const double & yaw, const double & vel, const std::array & pose_cov, const std::array & twist_cov); - bool adjustPosition(const double & x, const double & y, const double & yaw); + bool adjustPosition(const double & x, const double & y); bool limitStates(); @@ -126,6 +126,9 @@ class BicycleMotionModel bool predictState(const double dt, KalmanFilter & ekf) const; bool getPredictedState(const rclcpp::Time & time, Eigen::MatrixXd & X, Eigen::MatrixXd & P) const; + + bool getPredictedState( + const rclcpp::Time & time, Eigen::MatrixXd & X, Eigen::MatrixXd & P, double & lr) const; }; #endif // MULTI_OBJECT_TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 0e5c0a8742031..ce6613d3a249a 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -83,7 +83,7 @@ class NormalVehicleTracker : public Tracker const geometry_msgs::msg::Transform & self_transform); bool predict(const rclcpp::Time & time) override; - bool predict(const double dt, KalmanFilter & ekf) const; + // bool predict(const double dt, KalmanFilter & ekf) const; bool measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; diff --git a/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp index ed43dce367e83..effafb1d7ae6c 100644 --- a/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp @@ -263,6 +263,23 @@ bool BicycleMotionModel::updateExtendedState(const double & length) return true; } +bool BicycleMotionModel::adjustPosition(const double & x, const double & y) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // adjust position + Eigen::MatrixXd X_t(DIM, 1); + Eigen::MatrixXd P_t(DIM, DIM); + ekf_.getX(X_t); + ekf_.getP(P_t); + X_t(IDX::X) += x; + X_t(IDX::Y) += y; + ekf_.init(X_t, P_t); + + return true; +} + bool BicycleMotionModel::predictState(const rclcpp::Time & time) { // check if the state is initialized @@ -270,7 +287,7 @@ bool BicycleMotionModel::predictState(const rclcpp::Time & time) const double dt = (time - last_update_time_).seconds(); if (dt < 0.0) { - RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); + RCLCPP_WARN(logger_, "predictState: dt is negative. (%f)", dt); return false; } // if dt is too large, shorten dt and repeat prediction @@ -418,21 +435,34 @@ bool BicycleMotionModel::getPredictedState( KalmanFilter tmp_ekf_for_no_update = ekf_; const double dt = (time - last_update_time_).seconds(); - if (dt < 0.0) { - RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); - return false; - } - // if dt is too large, shorten dt and repeat prediction - const uint32_t repeat = std::ceil(dt / motion_params_.dt_max); - const double dt_ = dt / repeat; + // if (dt < 0.0) { + // RCLCPP_WARN(logger_, "getPredictedState: dt is negative. (%f)", dt); + // return false; + // } + + // naive conditioning, since dt of negative value was not considered + bool ret = false; - for (uint32_t i = 0; i < repeat; ++i) { - ret = predictState(dt_, tmp_ekf_for_no_update); - if (!ret) { - return false; + // predeict 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 / motion_params_.dt_max); + const double dt_ = dt / repeat; + for (uint32_t i = 0; i < repeat; ++i) { + ret = predictState(dt_, tmp_ekf_for_no_update); + if (!ret) { + return false; + } } } tmp_ekf_for_no_update.getX(X); tmp_ekf_for_no_update.getP(P); return ret; } + +bool BicycleMotionModel::getPredictedState( + const rclcpp::Time & time, Eigen::MatrixXd & X, Eigen::MatrixXd & P, double & lr) const +{ + lr = lr_; + return getPredictedState(time, X, P); +} diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 20857f3a7b7f0..c56173f861c02 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -139,9 +139,6 @@ NormalVehicleTracker::NormalVehicleTracker( P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } - // MOTION MODEL (set initial state and covariance) - ekf_.init(X, P); - // OBJECT SHAPE MODEL if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { @@ -155,9 +152,6 @@ NormalVehicleTracker::NormalVehicleTracker( bbox_object.shape.dimensions.z}; last_input_bounding_box_ = bounding_box_; } - // Set lf, lr - lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center, minimum of 1.0m - lr_ = std::max(bounding_box_.length * 0.25, 1.0); // 25% rear from the center, minimum of 1.0m // Set motion model motion_model_.init(time, X, P, bounding_box_.length); @@ -186,151 +180,10 @@ NormalVehicleTracker::NormalVehicleTracker( bool NormalVehicleTracker::predict(const rclcpp::Time & time) { - // MOTION MODEL (predict) - const double dt = (time - last_update_time_).seconds(); - if (dt < 0.0) { - RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); - return false; - } - // if dt is too large, shorten dt and repeat prediction - const double dt_max = 0.11; - const uint32_t repeat = std::ceil(dt / dt_max); - const double dt_ = dt / repeat; - bool ret = false; - for (uint32_t i = 0; i < repeat; ++i) { - ret = predict(dt_, ekf_); - if (!ret) { - return false; - } - } - if (ret) { + // predict state vector X t+1 + if (motion_model_.predictState(time)) { last_update_time_ = time; } - - motion_model_.predictState(time); - - return ret; -} - -bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const -{ - /* Motion model: static bicycle model (constant slip angle, constant velocity) - * - * w_k = vel_k * sin(slip_k) / l_r - * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt - * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt - * yaw_{k+1} = yaw_k + w_k * dt - * vel_{k+1} = vel_k - * slip_{k+1} = slip_k - * - */ - - /* Jacobian Matrix - * - * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, - * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, - * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] - * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, - * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, - * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] - * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/l_r*cos(slip)*dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] - * - */ - - // MOTION MODEL (predict) - - // Current state vector X t - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - 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)); - const double vel = X_t(IDX::VEL); - const double cos_slip = std::cos(X_t(IDX::SLIP)); - const double sin_slip = std::sin(X_t(IDX::SLIP)); - - double w = vel * sin_slip / lr_; - const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - const double w_dtdt = w * dt * dt; - const double vv_dtdt__lr = vel * vel * dt * dt / lr_; - - // Predict state vector X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = - X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt - X_next_t(IDX::Y) = - X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt - X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt - X_next_t(IDX::VEL) = X_t(IDX::VEL); - X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) - - // State transition matrix A - Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; - A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; - A(IDX::X, IDX::SLIP) = - -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; - A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; - A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; - A(IDX::Y, IDX::SLIP) = - vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; - A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; - A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; - - // Process noise covariance Q - float q_stddev_yaw_rate{0.0}; - if (vel <= 0.01) { - q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min; - } else { - // uncertainty of the yaw rate is limited by - // centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v - // or maximum slip angle slip_max : w = v*sin(slip_max)/l_r - q_stddev_yaw_rate = std::min( - ekf_params_.q_stddev_acc_lat / vel, - vel * std::sin(ekf_params_.q_max_slip_angle) / lr_); // [rad/s] - q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max); - q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min); - } - float q_cov_slip_rate{0.0f}; - if (vel <= 0.01) { - q_cov_slip_rate = ekf_params_.q_cov_slip_rate_min; - } else { - // The slip angle rate uncertainty is modeled as follows: - // d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt - // where sin(slip) = w * l_r / v - // d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) - // d(v)/dt and d(w)/t are considered to be uncorrelated - q_cov_slip_rate = - std::pow(ekf_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); - q_cov_slip_rate = std::min(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_max); - q_cov_slip_rate = std::max(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_min); - } - const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2); - const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2); - const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); - const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2); - const float q_cov_slip = q_cov_slip_rate * dt * dt; - - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - // Rotate the covariance matrix according to the vehicle yaw - // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); - Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); - Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); - Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::YAW, IDX::YAW) = q_cov_yaw; - Q(IDX::VEL, IDX::VEL) = q_cov_vel; - Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; - - // control-input model B and control-input u are not used - // Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - // Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); - - if (!ekf.predict(X_next_t, A, Q)) { - RCLCPP_WARN(logger_, "Cannot predict"); - } - return true; } @@ -341,8 +194,9 @@ autoware_auto_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpda autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; // current (predicted) state - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - ekf_.getX(X_t); + // Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); + // ekf_.getX(X_t); + Eigen::MatrixXd X_t = motion_model_.getStateVector(); // OBJECT SHAPE MODEL // convert to bounding box if input is convex shape @@ -418,8 +272,10 @@ bool NormalVehicleTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { // current (predicted) state - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - ekf_.getX(X_t); + // Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); + // ekf_.getX(X_t); + + Eigen::MatrixXd X_t = motion_model_.getStateVector(); // MOTION MODEL (update) @@ -439,61 +295,6 @@ bool NormalVehicleTracker::measureWithPose( } } - // Decide dimension of measurement vector and matrix - // pos x, pos y, pos yaw, (vel) depending on pose measurement - const int dim_y = is_velocity_available ? 4 : 3; - - // Set measurement matrix C and observation vector Y - Eigen::MatrixXd Y(dim_y, 1); - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); - Y(IDX::X, 0) = object.kinematics.pose_with_covariance.pose.position.x; - Y(IDX::Y, 0) = object.kinematics.pose_with_covariance.pose.position.y; - Y(IDX::YAW, 0) = measurement_yaw; - C(0, IDX::X) = 1.0; // for pos x - C(1, IDX::Y) = 1.0; // for pos y - C(2, IDX::YAW) = 1.0; // for yaw - if (is_velocity_available) { - Y(IDX::VEL, 0) = object.kinematics.twist_with_covariance.twist.linear.x; - C(3, IDX::VEL) = 1.0; // for vel - } - - // Set noise covariance matrix R - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - const auto & pose_cov = object.kinematics.pose_with_covariance.covariance; - R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; - R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; - R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; - R(0, 2) = pose_cov[utils::MSG_COV_IDX::X_YAW]; - R(1, 2) = pose_cov[utils::MSG_COV_IDX::Y_YAW]; - R(2, 0) = pose_cov[utils::MSG_COV_IDX::YAW_X]; - R(2, 1) = pose_cov[utils::MSG_COV_IDX::YAW_Y]; - R(2, 2) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; - if (is_velocity_available) { - R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - } - - // ekf update - if (!ekf_.update(Y, C, R)) { - RCLCPP_WARN(logger_, "Cannot update"); - } - - // normalize yaw and limit vel, slip - { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); - ekf_.getX(X_t); - ekf_.getP(P_t); - X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - if (!(-max_vel_ <= X_t(IDX::VEL) && X_t(IDX::VEL) <= max_vel_)) { - X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -max_vel_ : max_vel_; - } - if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { - X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; - } - ekf_.init(X_t, P_t); - } - // position z constexpr float gain = 0.9; z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; @@ -532,23 +333,12 @@ bool NormalVehicleTracker::measureWithShape( last_input_bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; + // update motion model + motion_model_.updateExtendedState(bounding_box_.length); // update offset into position - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); - ekf_.getX(X_t); - ekf_.getP(P_t); - X_t(IDX::X) = X_t(IDX::X) + gain * tracking_offset_.x(); - X_t(IDX::Y) = X_t(IDX::Y) + gain * tracking_offset_.y(); + motion_model_.adjustPosition(gain * tracking_offset_.x(), gain * tracking_offset_.y()); tracking_offset_.x() = gain_inv * tracking_offset_.x(); tracking_offset_.y() = gain_inv * tracking_offset_.y(); - ekf_.init(X_t, P_t); - - // update lf, lr - lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center, minimum of 1.0m - lr_ = std::max(bounding_box_.length * 0.25, 1.0); // 25% rear from the center, minimum of 1.0m - - // update motion model - motion_model_.updateExtendedState(bounding_box_.length); return true; } @@ -585,19 +375,11 @@ bool NormalVehicleTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict state - KalmanFilter tmp_ekf_for_no_update = ekf_; - const double dt = (time - last_update_time_).seconds(); - if (0.001 /*1msec*/ < dt) { - predict(dt, tmp_ekf_for_no_update); - } + // predict from motion model Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state Eigen::MatrixXd P(ekf_params_.dim_x, ekf_params_.dim_x); // predicted state - tmp_ekf_for_no_update.getX(X_t); - tmp_ekf_for_no_update.getP(P); - - // predict from motion model - motion_model_.getPredictedState(time, X_t, P); + double lr = lr_; + motion_model_.getPredictedState(time, X_t, P, lr); /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; @@ -640,7 +422,7 @@ bool NormalVehicleTracker::getTrackedObject( twist_with_cov.twist.linear.x = X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)); twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)); twist_with_cov.twist.angular.z = - X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r + X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr; // yaw_rate = vel_k * sin(slip_k) / l_r /* twist covariance * convert covariance from velocity, slip angle to vx, vy, and yaw angle * @@ -657,7 +439,7 @@ bool NormalVehicleTracker::getTrackedObject( Eigen::MatrixXd cov_jacob(3, 2); cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)), std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)), - std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_; + std::sin(X_t(IDX::SLIP)) / lr, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr; Eigen::MatrixXd cov_twist(2, 2); cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), P(IDX::SLIP, IDX::SLIP); From 749c57564df1c8de3885ffec87995171abe9eacf Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 5 Mar 2024 13:53:00 +0900 Subject: [PATCH 12/42] feat: normal vehicle tracker replace to motion model Signed-off-by: Taekjin LEE --- .../motion_model/bicycle_motion_model.hpp | 3 +- .../src/motion_model/bicycle_motion_model.cpp | 92 +++++++++++--- .../tracker/model/normal_vehicle_tracker.cpp | 119 +++--------------- 3 files changed, 94 insertions(+), 120 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp index 70d3fcc1d1b45..2340a5fd7391b 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp @@ -128,7 +128,8 @@ class BicycleMotionModel bool getPredictedState(const rclcpp::Time & time, Eigen::MatrixXd & X, Eigen::MatrixXd & P) const; bool getPredictedState( - const rclcpp::Time & time, Eigen::MatrixXd & X, Eigen::MatrixXd & P, double & lr) const; + const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, + geometry_msgs::msg::Twist & twist, std::array & twist_cov) const; }; #endif // MULTI_OBJECT_TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp index effafb1d7ae6c..21849ce085a94 100644 --- a/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp @@ -287,7 +287,7 @@ bool BicycleMotionModel::predictState(const rclcpp::Time & time) const double dt = (time - last_update_time_).seconds(); if (dt < 0.0) { - RCLCPP_WARN(logger_, "predictState: dt is negative. (%f)", dt); + RCLCPP_WARN(logger_, "BicycleMotionModel::predictState: dt is negative. (%f)", dt); return false; } // if dt is too large, shorten dt and repeat prediction @@ -434,16 +434,14 @@ bool BicycleMotionModel::getPredictedState( // copy the predicted state and covariance KalmanFilter tmp_ekf_for_no_update = ekf_; - const double dt = (time - last_update_time_).seconds(); - // if (dt < 0.0) { - // RCLCPP_WARN(logger_, "getPredictedState: dt is negative. (%f)", dt); - // return false; - // } - - // naive conditioning, since dt of negative value was not considered + double dt = (time - last_update_time_).seconds(); + if (dt < 0.0) { + RCLCPP_WARN(logger_, "BicycleMotionModel::getPredictedState: dt is negative. (%f)", dt); + dt = 0.0; + } - bool ret = false; - // predeict only when dt is small enough + bool ret = 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 / motion_params_.dt_max); @@ -457,12 +455,78 @@ bool BicycleMotionModel::getPredictedState( } tmp_ekf_for_no_update.getX(X); tmp_ekf_for_no_update.getP(P); - return ret; + return true; } bool BicycleMotionModel::getPredictedState( - const rclcpp::Time & time, Eigen::MatrixXd & X, Eigen::MatrixXd & P, double & lr) const + const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, + geometry_msgs::msg::Twist & twist, std::array & twist_cov) const { - lr = lr_; - return getPredictedState(time, X, P); + // get predicted state + Eigen::MatrixXd X(DIM, 1); + Eigen::MatrixXd P(DIM, DIM); + if (!getPredictedState(time, X, P)) { + return false; + } + + // set position + pose.position.x = X(IDX::X); + pose.position.y = X(IDX::Y); + pose.position.z = 0.0; + + // set orientation + tf2::Quaternion quaternion; + quaternion.setRPY(0.0, 0.0, X(IDX::YAW)); + pose.orientation.x = quaternion.x(); + pose.orientation.y = quaternion.y(); + pose.orientation.z = quaternion.z(); + pose.orientation.w = quaternion.w(); + + // set twist + twist.linear.x = X(IDX::VEL) * std::cos(X(IDX::SLIP)); + twist.linear.y = X(IDX::VEL) * std::sin(X(IDX::SLIP)); + twist.linear.z = 0.0; + twist.angular.x = 0.0; + twist.angular.y = 0.0; + twist.angular.z = twist.linear.y / lr_; + + // set pose covariance + constexpr double zz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double rr_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double pp_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + pose_cov[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); + pose_cov[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); + pose_cov[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); + pose_cov[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); + pose_cov[utils::MSG_COV_IDX::Z_Z] = zz_cov; + pose_cov[utils::MSG_COV_IDX::ROLL_ROLL] = rr_cov; + pose_cov[utils::MSG_COV_IDX::PITCH_PITCH] = pp_cov; + + // set twist covariance + Eigen::MatrixXd cov_jacob(3, 2); + cov_jacob << std::cos(X(IDX::SLIP)), -X(IDX::VEL) * std::sin(X(IDX::SLIP)), + std::sin(X(IDX::SLIP)), X(IDX::VEL) * std::cos(X(IDX::SLIP)), std::sin(X(IDX::SLIP)) / lr_, + X(IDX::VEL) * std::cos(X(IDX::SLIP)) / lr_; + Eigen::MatrixXd cov_twist(2, 2); + cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), + P(IDX::SLIP, IDX::SLIP); + Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); + constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + twist_cov[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); + twist_cov[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); + twist_cov[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); + twist_cov[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); + twist_cov[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); + twist_cov[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); + twist_cov[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); + twist_cov[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); + twist_cov[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); + twist_cov[utils::MSG_COV_IDX::Z_Z] = vz_cov; + twist_cov[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; + twist_cov[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; + + return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index c56173f861c02..81d5df1537b0a 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -74,24 +74,6 @@ NormalVehicleTracker::NormalVehicleTracker( ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); ekf_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0); ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); - // process noise covariance - ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration - ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration - ekf_params_.q_stddev_yaw_rate_min = - tier4_autoware_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate - ekf_params_.q_stddev_yaw_rate_max = - tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate - float q_stddev_slip_rate_min = - tier4_autoware_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate - float q_stddev_slip_rate_max = - tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate - ekf_params_.q_cov_slip_rate_min = std::pow(q_stddev_slip_rate_min, 2.0); - ekf_params_.q_cov_slip_rate_max = std::pow(q_stddev_slip_rate_max, 2.0); - ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle - // limitations - max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s] - max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad] - velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] // initialize state vector X Eigen::MatrixXd X(ekf_params_.dim_x, 1); @@ -194,8 +176,6 @@ autoware_auto_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpda autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; // current (predicted) state - // Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - // ekf_.getX(X_t); Eigen::MatrixXd X_t = motion_model_.getStateVector(); // OBJECT SHAPE MODEL @@ -272,17 +252,10 @@ bool NormalVehicleTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { // current (predicted) state - // Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - // ekf_.getX(X_t); - Eigen::MatrixXd X_t = motion_model_.getStateVector(); // MOTION MODEL (update) - // get measurement yaw angle to update - double measurement_yaw = 0.0; - utils::getMeasurementYaw(object, X_t(IDX::YAW), measurement_yaw); - // velocity capability is checked only when the object has velocity measurement // and the predicted velocity is close to the observed velocity bool is_velocity_available = false; @@ -347,21 +320,25 @@ bool NormalVehicleTracker::measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { - const auto & current_classification = getClassification(); + // keep the latest input object object_ = object; + + // update classification + const auto & current_classification = getClassification(); if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { setClassification(current_classification); } + // check time gap if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { RCLCPP_WARN( logger_, "There is a large gap between predicted time and measurement time. (%f)", (time - last_update_time_).seconds()); } + // update object const autoware_auto_perception_msgs::msg::DetectedObject updating_object = getUpdatingObject(object, self_transform); - measureWithPose(updating_object); measureWithShape(updating_object); @@ -375,88 +352,20 @@ bool NormalVehicleTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict from motion model - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - Eigen::MatrixXd P(ekf_params_.dim_x, ekf_params_.dim_x); // predicted state - double lr = lr_; - motion_model_.getPredictedState(time, X_t, P, lr); - /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; + // predict from motion model + if (!motion_model_.getPredictedState( + time, pose_with_cov.pose, pose_with_cov.covariance, twist_with_cov.twist, + twist_with_cov.covariance)) { + RCLCPP_WARN(logger_, "NormalVehicleTracker::getTrackedObject: Failed to get predicted state."); + return false; + } + // position - pose_with_cov.pose.position.x = X_t(IDX::X); - pose_with_cov.pose.position.y = X_t(IDX::Y); pose_with_cov.pose.position.z = z_; - // quaternion - { - double roll, pitch, yaw; - tf2::Quaternion original_quaternion; - tf2::fromMsg(object_.kinematics.pose_with_covariance.pose.orientation, original_quaternion); - tf2::Matrix3x3(original_quaternion).getRPY(roll, pitch, yaw); - tf2::Quaternion filtered_quaternion; - filtered_quaternion.setRPY(roll, pitch, X_t(IDX::YAW)); - pose_with_cov.pose.orientation.x = filtered_quaternion.x(); - pose_with_cov.pose.orientation.y = filtered_quaternion.y(); - pose_with_cov.pose.orientation.z = filtered_quaternion.z(); - pose_with_cov.pose.orientation.w = filtered_quaternion.w(); - // orientation availability is assumed to be unknown - object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; - } - // position covariance - constexpr double z_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double r_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double p_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = r_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = p_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); - - // twist - twist_with_cov.twist.linear.x = X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)); - twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)); - twist_with_cov.twist.angular.z = - X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr; // yaw_rate = vel_k * sin(slip_k) / l_r - /* twist covariance - * convert covariance from velocity, slip angle to vx, vy, and yaw angle - * - * vx = vel * cos(slip) - * vy = vel * sin(slip) - * wz = vel * sin(slip) / l_r = vy / l_r - * - */ - - constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - - Eigen::MatrixXd cov_jacob(3, 2); - cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)), - std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)), - std::sin(X_t(IDX::SLIP)) / lr, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr; - Eigen::MatrixXd cov_twist(2, 2); - cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), - P(IDX::SLIP, IDX::SLIP); - Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); - - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; // set shape object.shape.dimensions.x = bounding_box_.length; From f0bf549de454b7f40e053fa0c52a1b64b558d8a0 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 5 Mar 2024 14:16:55 +0900 Subject: [PATCH 13/42] feat: big vehicle tracker is replaced by motion model Signed-off-by: Taekjin LEE --- .../tracker/model/big_vehicle_tracker.hpp | 24 +- .../tracker/model/normal_vehicle_tracker.hpp | 22 +- .../src/tracker/model/big_vehicle_tracker.cpp | 389 +++--------------- .../tracker/model/normal_vehicle_tracker.cpp | 7 +- 4 files changed, 82 insertions(+), 360 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index de33ca7e37afb..ef314bcd4619b 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -19,9 +19,11 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ +#include "multi_object_tracker/motion_model/bicycle_motion_model.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" #include + class BigVehicleTracker : public Tracker { private: @@ -29,33 +31,21 @@ class BigVehicleTracker : public Tracker rclcpp::Logger logger_; private: - KalmanFilter ekf_; rclcpp::Time last_update_time_; enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; struct EkfParams { char dim_x = 5; - float q_stddev_acc_long; - float q_stddev_acc_lat; - float q_stddev_yaw_rate_min; - float q_stddev_yaw_rate_max; - float q_cov_slip_rate_min; - float q_cov_slip_rate_max; - float q_max_slip_angle; float p0_cov_vel; float p0_cov_slip; + float p0_cov_x; + float p0_cov_y; + float p0_cov_yaw; float r_cov_x; float r_cov_y; float r_cov_yaw; float r_cov_vel; - float p0_cov_x; - float p0_cov_y; - float p0_cov_yaw; } ekf_params_; - double max_vel_; - double max_slip_; - double lf_; - double lr_; float z_; double velocity_deviation_threshold_; @@ -70,13 +60,15 @@ class BigVehicleTracker : public Tracker BoundingBox last_input_bounding_box_; Eigen::Vector2d tracking_offset_; +private: + BicycleMotionModel motion_model_; + public: BigVehicleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & self_transform); bool predict(const rclcpp::Time & time) override; - bool predict(const double dt, KalmanFilter & ekf) const; bool measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index ce6613d3a249a..0935ed7643b81 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -20,7 +20,7 @@ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ #include "multi_object_tracker/motion_model/bicycle_motion_model.hpp" -#include "tracker_base.hpp" +#include "multi_object_tracker/tracker/model/tracker_base.hpp" #include @@ -31,35 +31,22 @@ class NormalVehicleTracker : public Tracker rclcpp::Logger logger_; private: - KalmanFilter ekf_; rclcpp::Time last_update_time_; enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; struct EkfParams { char dim_x = 5; - float q_stddev_acc_long; - float q_stddev_acc_lat; - float q_stddev_yaw_rate_min; - float q_stddev_yaw_rate_max; - float q_cov_slip_rate_min; - float q_cov_slip_rate_max; - float q_max_slip_angle; float p0_cov_vel; float p0_cov_slip; + float p0_cov_x; + float p0_cov_y; + float p0_cov_yaw; float r_cov_x; float r_cov_y; float r_cov_yaw; float r_cov_vel; - float p0_cov_x; - float p0_cov_y; - float p0_cov_yaw; } ekf_params_; - - double max_vel_; - double max_slip_; - double lf_; - double lr_; float z_; double velocity_deviation_threshold_; @@ -83,7 +70,6 @@ class NormalVehicleTracker : public Tracker const geometry_msgs::msg::Transform & self_transform); bool predict(const rclcpp::Time & time) override; - // bool predict(const double dt, KalmanFilter & ekf) const; bool measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index ec93e6e81323d..79965d4470512 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -74,24 +74,6 @@ BigVehicleTracker::BigVehicleTracker( ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); ekf_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0); ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); - // process noise covariance - ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration - ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration - ekf_params_.q_stddev_yaw_rate_min = - tier4_autoware_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate - ekf_params_.q_stddev_yaw_rate_max = - tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate - float q_stddev_slip_rate_min = - tier4_autoware_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate - float q_stddev_slip_rate_max = - tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate - ekf_params_.q_cov_slip_rate_min = std::pow(q_stddev_slip_rate_min, 2.0); - ekf_params_.q_cov_slip_rate_max = std::pow(q_stddev_slip_rate_max, 2.0); - ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle - // limitations - max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s] - max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad] - velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] // initialize state vector X Eigen::MatrixXd X(ekf_params_.dim_x, 1); @@ -139,9 +121,6 @@ BigVehicleTracker::BigVehicleTracker( P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } - // MOTION MODEL (set initial state and covariance) - ekf_.init(X, P); - // OBJECT SHAPE MODEL if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { @@ -155,155 +134,38 @@ BigVehicleTracker::BigVehicleTracker( bbox_object.shape.dimensions.z}; last_input_bounding_box_ = bounding_box_; } - // Set lf, lr - lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center, minimum of 1.5m - lr_ = std::max(bounding_box_.length * 0.25, 1.5); // 25% rear from the center, minimum of 1.5m + + // Set motion model + motion_model_.init(time, X, P, bounding_box_.length); + + // Set motion model parameters + motion_model_.setMotionParams( + 9.8 * 0.35, /* [m/(s*s)] uncertain longitudinal acceleration */ + 9.8 * 0.15, /* [m/(s*s)] uncertain lateral acceleration */ + 1.5, /* [deg/s] uncertain yaw change rate, minimum */ + 15.0, /* [deg/s] uncertain yaw change rate, maximum */ + 0.3, /* [deg/s] uncertain slip angle change rate, minimum */ + 10.0, /* [deg/s] uncertain slip angle change rate, maximum */ + 30, /* [deg] max slip angle */ + 0.3, /* [-] ratio of front wheel position*/ + 1.5, /* [m] minimum front wheel position*/ + 0.25, /* [-] ratio of rear wheel position*/ + 1.5 /* [m] minimum rear wheel position*/ + ); + + // Set motion limits + motion_model_.setMotionLimits( + tier4_autoware_utils::kmph2mps(100), /* [m/s] maximum velocity */ + 30 /* [deg] maximum slip angle */ + ); } bool BigVehicleTracker::predict(const rclcpp::Time & time) { - // MOTION MODEL (predict) - const double dt = (time - last_update_time_).seconds(); - if (dt < 0.0) { - RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); - return false; - } - // if dt is too large, shorten dt and repeat prediction - const double dt_max = 0.11; - const uint32_t repeat = std::ceil(dt / dt_max); - const double dt_ = dt / repeat; - bool ret = false; - for (uint32_t i = 0; i < repeat; ++i) { - ret = predict(dt_, ekf_); - if (!ret) { - return false; - } - } - if (ret) { + // predict state vector X t+1 + if (motion_model_.predictState(time)) { last_update_time_ = time; } - return ret; -} - -bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const -{ - /* Motion model: static bicycle model (constant slip angle, constant velocity) - * - * w_k = vel_k * sin(slip_k) / l_r - * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt - * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt - * yaw_{k+1} = yaw_k + w_k * dt - * vel_{k+1} = vel_k - * slip_{k+1} = slip_k - * - */ - - /* Jacobian Matrix - * - * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, - * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, - * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] - * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, - * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, - * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] - * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/l_r*cos(slip)*dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] - * - */ - - // MOTION MODEL (predict) - - // Current state vector X t - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - 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)); - const double vel = X_t(IDX::VEL); - const double cos_slip = std::cos(X_t(IDX::SLIP)); - const double sin_slip = std::sin(X_t(IDX::SLIP)); - - double w = vel * sin_slip / lr_; - const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - const double w_dtdt = w * dt * dt; - const double vv_dtdt__lr = vel * vel * dt * dt / lr_; - - // Predict state vector X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = - X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt - X_next_t(IDX::Y) = - X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt - X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt - X_next_t(IDX::VEL) = X_t(IDX::VEL); - X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) - - // State transition matrix A - Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; - A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; - A(IDX::X, IDX::SLIP) = - -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; - A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; - A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; - A(IDX::Y, IDX::SLIP) = - vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; - A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; - A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; - - // Process noise covariance Q - float q_stddev_yaw_rate{0.0}; - if (vel <= 0.01) { - q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min; - } else { - // uncertainty of the yaw rate is limited by - // centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v - // or maximum slip angle slip_max : w = v*sin(slip_max)/l_r - q_stddev_yaw_rate = std::min( - ekf_params_.q_stddev_acc_lat / vel, - vel * std::sin(ekf_params_.q_max_slip_angle) / lr_); // [rad/s] - q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max); - q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min); - } - float q_cov_slip_rate{0.0f}; - if (vel <= 0.01) { - q_cov_slip_rate = ekf_params_.q_cov_slip_rate_min; - } else { - // The slip angle rate uncertainty is modeled as follows: - // d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt - // where sin(slip) = w * l_r / v - // d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) - // d(v)/dt and d(w)/t are considered to be uncorrelated - q_cov_slip_rate = - std::pow(ekf_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); - q_cov_slip_rate = std::min(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_max); - q_cov_slip_rate = std::max(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_min); - } - const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2); - const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2); - const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); - const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2); - const float q_cov_slip = q_cov_slip_rate * dt * dt; - - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - // Rotate the covariance matrix according to the vehicle yaw - // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); - Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); - Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); - Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::YAW, IDX::YAW) = q_cov_yaw; - Q(IDX::VEL, IDX::VEL) = q_cov_vel; - Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; - - // control-input model B and control-input u are not used - // Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - // Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); - - if (!ekf.predict(X_next_t, A, Q)) { - RCLCPP_WARN(logger_, "Cannot predict"); - } - return true; } @@ -314,8 +176,7 @@ autoware_auto_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatin autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; // current (predicted) state - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - ekf_.getX(X_t); + Eigen::MatrixXd X_t = motion_model_.getStateVector(); // OBJECT SHAPE MODEL // convert to bounding box if input is convex shape @@ -341,10 +202,10 @@ autoware_auto_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatin float r_cov_y; using Label = autoware_auto_perception_msgs::msg::ObjectClassification; const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); - if (label == Label::CAR) { + if (utils::isLargeVehicleLabel(label)) { r_cov_x = ekf_params_.r_cov_x; r_cov_y = ekf_params_.r_cov_y; - } else if (utils::isLargeVehicleLabel(label)) { + } else if (label == Label::CAR) { // if label is changed, enlarge the measurement noise covariance constexpr float r_stddev_x = 2.0; // [m] constexpr float r_stddev_y = 2.0; // [m] @@ -391,15 +252,10 @@ bool BigVehicleTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { // current (predicted) state - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - ekf_.getX(X_t); + Eigen::MatrixXd X_t = motion_model_.getStateVector(); // MOTION MODEL (update) - // get measurement yaw angle to update - double measurement_yaw = 0.0; - utils::getMeasurementYaw(object, X_t(IDX::YAW), measurement_yaw); - // velocity capability is checked only when the object has velocity measurement // and the predicted velocity is close to the observed velocity bool is_velocity_available = false; @@ -412,65 +268,28 @@ bool BigVehicleTracker::measureWithPose( } } - // Decide dimension of measurement vector and matrix - // pos x, pos y, pos yaw, (vel) depending on pose measurement - const int dim_y = is_velocity_available ? 4 : 3; - - // Set measurement matrix C and observation vector Y - Eigen::MatrixXd Y(dim_y, 1); - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); - Y(IDX::X, 0) = object.kinematics.pose_with_covariance.pose.position.x; - Y(IDX::Y, 0) = object.kinematics.pose_with_covariance.pose.position.y; - Y(IDX::YAW, 0) = measurement_yaw; - C(0, IDX::X) = 1.0; // for pos x - C(1, IDX::Y) = 1.0; // for pos y - C(2, IDX::YAW) = 1.0; // for yaw - if (is_velocity_available) { - Y(IDX::VEL, 0) = object.kinematics.twist_with_covariance.twist.linear.x; - C(3, IDX::VEL) = 1.0; // for vel - } - - // Set noise covariance matrix R - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - const auto & pose_cov = object.kinematics.pose_with_covariance.covariance; - R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; - R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; - R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; - R(0, 2) = pose_cov[utils::MSG_COV_IDX::X_YAW]; - R(1, 2) = pose_cov[utils::MSG_COV_IDX::Y_YAW]; - R(2, 0) = pose_cov[utils::MSG_COV_IDX::YAW_X]; - R(2, 1) = pose_cov[utils::MSG_COV_IDX::YAW_Y]; - R(2, 2) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; - if (is_velocity_available) { - R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - } - - // ekf update - if (!ekf_.update(Y, C, R)) { - RCLCPP_WARN(logger_, "Cannot update"); - } + // position z + constexpr float gain = 0.9; + z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; - // normalize yaw and limit vel, slip + // motion model { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); - ekf_.getX(X_t); - ekf_.getP(P_t); - X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - if (!(-max_vel_ <= X_t(IDX::VEL) && X_t(IDX::VEL) <= max_vel_)) { - X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -max_vel_ : max_vel_; - } - if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { - X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double vel = object.kinematics.twist_with_covariance.twist.linear.x; + + if (is_velocity_available) { + motion_model_.updateStatePoseHeadVel( + x, y, yaw, vel, object.kinematics.pose_with_covariance.covariance, + object.kinematics.twist_with_covariance.covariance); + } else { + motion_model_.updateStatePoseHead( + x, y, yaw, object.kinematics.pose_with_covariance.covariance); } - ekf_.init(X_t, P_t); + motion_model_.limitStates(); } - // position z - constexpr float gain = 0.9; - z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; - return true; } @@ -487,20 +306,13 @@ bool BigVehicleTracker::measureWithShape( last_input_bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; - // update offset into position - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); - ekf_.getX(X_t); - ekf_.getP(P_t); - X_t(IDX::X) = X_t(IDX::X) + gain * tracking_offset_.x(); - X_t(IDX::Y) = X_t(IDX::Y) + gain * tracking_offset_.y(); + // update motion model + motion_model_.updateExtendedState(bounding_box_.length); + // update offset into object position + motion_model_.adjustPosition(gain * tracking_offset_.x(), gain * tracking_offset_.y()); + // update offset tracking_offset_.x() = gain_inv * tracking_offset_.x(); tracking_offset_.y() = gain_inv * tracking_offset_.y(); - ekf_.init(X_t, P_t); - - // update lf, lr - lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center, minimum of 1.0m - lr_ = std::max(bounding_box_.length * 0.25, 1.0); // 25% rear from the center, minimum of 1.0m return true; } @@ -509,21 +321,25 @@ bool BigVehicleTracker::measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { - const auto & current_classification = getClassification(); + // keep the latest input object object_ = object; + + // update classification + const auto & current_classification = getClassification(); if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { setClassification(current_classification); } + // check time gap if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { RCLCPP_WARN( logger_, "There is a large gap between predicted time and measurement time. (%f)", (time - last_update_time_).seconds()); } + // update object const autoware_auto_perception_msgs::msg::DetectedObject updating_object = getUpdatingObject(object, self_transform); - measureWithPose(updating_object); measureWithShape(updating_object); @@ -537,93 +353,20 @@ bool BigVehicleTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict state - KalmanFilter tmp_ekf_for_no_update = ekf_; - const double dt = (time - last_update_time_).seconds(); - if (0.001 /*1msec*/ < dt) { - predict(dt, tmp_ekf_for_no_update); - } - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - Eigen::MatrixXd P(ekf_params_.dim_x, ekf_params_.dim_x); // predicted state - tmp_ekf_for_no_update.getX(X_t); - tmp_ekf_for_no_update.getP(P); - /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; + // predict from motion model + if (!motion_model_.getPredictedState( + time, pose_with_cov.pose, pose_with_cov.covariance, twist_with_cov.twist, + twist_with_cov.covariance)) { + RCLCPP_WARN(logger_, "BigVehicleTracker::getTrackedObject: Failed to get predicted state."); + return false; + } + // position - pose_with_cov.pose.position.x = X_t(IDX::X); - pose_with_cov.pose.position.y = X_t(IDX::Y); pose_with_cov.pose.position.z = z_; - // quaternion - { - double roll, pitch, yaw; - tf2::Quaternion original_quaternion; - tf2::fromMsg(object_.kinematics.pose_with_covariance.pose.orientation, original_quaternion); - tf2::Matrix3x3(original_quaternion).getRPY(roll, pitch, yaw); - tf2::Quaternion filtered_quaternion; - filtered_quaternion.setRPY(roll, pitch, X_t(IDX::YAW)); - pose_with_cov.pose.orientation.x = filtered_quaternion.x(); - pose_with_cov.pose.orientation.y = filtered_quaternion.y(); - pose_with_cov.pose.orientation.z = filtered_quaternion.z(); - pose_with_cov.pose.orientation.w = filtered_quaternion.w(); - // orientation availability is assumed to be unknown - object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; - } - // position covariance - constexpr double z_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double r_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double p_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = r_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = p_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); - - // twist - twist_with_cov.twist.linear.x = X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)); - twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)); - twist_with_cov.twist.angular.z = - X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r - /* twist covariance - * convert covariance from velocity, slip angle to vx, vy, and yaw angle - * - * vx = vel * cos(slip) - * vy = vel * sin(slip) - * wz = vel * sin(slip) / l_r = vy / l_r - * - */ - - constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - - Eigen::MatrixXd cov_jacob(3, 2); - cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)), - std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)), - std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_; - Eigen::MatrixXd cov_twist(2, 2); - cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), - P(IDX::SLIP, IDX::SLIP); - Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); - - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; // set shape object.shape.dimensions.x = bounding_box_.length; diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 81d5df1537b0a..c8556a115d4fd 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -147,9 +147,9 @@ NormalVehicleTracker::NormalVehicleTracker( 0.3, /* [deg/s] uncertain slip angle change rate, minimum */ 10.0, /* [deg/s] uncertain slip angle change rate, maximum */ 30, /* [deg] max slip angle */ - 0.3, /* [-] ratio of front wheel*/ + 0.3, /* [-] ratio of front wheel position*/ 1.0, /* [m] minimum front wheel position*/ - 0.25, /* [-] ratio of rear wheel*/ + 0.25, /* [-] ratio of rear wheel position*/ 1.0 /* [m] minimum rear wheel position*/ ); @@ -308,8 +308,9 @@ bool NormalVehicleTracker::measureWithShape( // update motion model motion_model_.updateExtendedState(bounding_box_.length); - // update offset into position + // update offset into object position motion_model_.adjustPosition(gain * tracking_offset_.x(), gain * tracking_offset_.y()); + // update offset tracking_offset_.x() = gain_inv * tracking_offset_.x(); tracking_offset_.y() = gain_inv * tracking_offset_.y(); From 990118b2f8e051a5f611e0374be2eb1fe7a88719 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 5 Mar 2024 14:55:57 +0900 Subject: [PATCH 14/42] feat: bicycle tracker replace by motion model Signed-off-by: Taekjin LEE --- .../tracker/model/bicycle_tracker.hpp | 25 +- .../src/tracker/model/bicycle_tracker.cpp | 380 +++--------------- .../src/tracker/model/big_vehicle_tracker.cpp | 13 +- .../tracker/model/normal_vehicle_tracker.cpp | 13 +- 4 files changed, 91 insertions(+), 340 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 8199e0b89290d..7a62b3ccb8c0f 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -19,6 +19,7 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ +#include "multi_object_tracker/motion_model/bicycle_motion_model.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" #include @@ -30,33 +31,21 @@ class BicycleTracker : public Tracker rclcpp::Logger logger_; private: - KalmanFilter ekf_; rclcpp::Time last_update_time_; enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; + struct EkfParams { char dim_x = 5; - float q_stddev_acc_long; - float q_stddev_acc_lat; - float q_stddev_yaw_rate_min; - float q_stddev_yaw_rate_max; - float q_cov_slip_rate_min; - float q_cov_slip_rate_max; - float q_max_slip_angle; float p0_cov_vel; float p0_cov_slip; - float r_cov_x; - float r_cov_y; - float r_cov_yaw; float p0_cov_x; float p0_cov_y; float p0_cov_yaw; + float r_cov_x; + float r_cov_y; + float r_cov_yaw; } ekf_params_; - - double max_vel_; - double max_slip_; - double lf_; - double lr_; float z_; private: @@ -67,7 +56,9 @@ class BicycleTracker : public Tracker double height; }; BoundingBox bounding_box_; - BoundingBox last_input_bounding_box_; + +private: + BicycleMotionModel motion_model_; public: BicycleTracker( diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index efc9396b0d71c..261076e2886fb 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -71,23 +71,6 @@ BicycleTracker::BicycleTracker( ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); ekf_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0); ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); - // process noise covariance - ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration - ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration - ekf_params_.q_stddev_yaw_rate_min = - tier4_autoware_utils::deg2rad(5.0); // [rad/s] uncertain yaw change rate - ekf_params_.q_stddev_yaw_rate_max = - tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate - float q_stddev_slip_rate_min = - tier4_autoware_utils::deg2rad(1); // [rad/s] uncertain slip angle change rate - float q_stddev_slip_rate_max = - tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate - ekf_params_.q_cov_slip_rate_min = std::pow(q_stddev_slip_rate_min, 2.0); - ekf_params_.q_cov_slip_rate_max = std::pow(q_stddev_slip_rate_max, 2.0); - ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle - // limitations - max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s] - max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad] // initialize state vector X Eigen::MatrixXd X(ekf_params_.dim_x, 1); @@ -135,9 +118,6 @@ BicycleTracker::BicycleTracker( P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } - // MOTION MODEL (set initial state and covariance) - ekf_.init(X, P); - // OBJECT SHAPE MODEL if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { @@ -145,155 +125,38 @@ BicycleTracker::BicycleTracker( } else { bounding_box_ = {1.0, 0.5, 1.7}; } - // Set lf, lr - lf_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% front from the center, minimum of 0.3m - lr_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% rear from the center, minimum of 0.3m + + // Set motion model + motion_model_.init(time, X, P, bounding_box_.length); + + // Set motion model parameters + motion_model_.setMotionParams( + 9.8 * 0.35, /* [m/(s*s)] uncertain longitudinal acceleration */ + 9.8 * 0.15, /* [m/(s*s)] uncertain lateral acceleration */ + 5.0, /* [deg/s] uncertain yaw change rate, minimum */ + 15.0, /* [deg/s] uncertain yaw change rate, maximum */ + 1.0, /* [deg/s] uncertain slip angle change rate, minimum */ + 10.0, /* [deg/s] uncertain slip angle change rate, maximum */ + 30, /* [deg] max slip angle */ + 0.3, /* [-] ratio of front wheel position*/ + 0.3, /* [m] minimum front wheel position*/ + 0.3, /* [-] ratio of rear wheel position*/ + 0.3 /* [m] minimum rear wheel position*/ + ); + + // Set motion limits + motion_model_.setMotionLimits( + tier4_autoware_utils::kmph2mps(100), /* [m/s] maximum velocity */ + 30 /* [deg] maximum slip angle */ + ); } bool BicycleTracker::predict(const rclcpp::Time & time) { - // MOTION MODEL (predict) - const double dt = (time - last_update_time_).seconds(); - if (dt < 0.0) { - RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); - return false; - } - // if dt is too large, shorten dt and repeat prediction - const double dt_max = 0.11; - const uint32_t repeat = std::ceil(dt / dt_max); - const double dt_ = dt / repeat; - bool ret = false; - for (uint32_t i = 0; i < repeat; ++i) { - ret = predict(dt_, ekf_); - if (!ret) { - return false; - } - } - if (ret) { + // predict state vector X t+1 + if (motion_model_.predictState(time)) { last_update_time_ = time; } - return ret; -} - -bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const -{ - /* Motion model: static bicycle model (constant slip angle, constant velocity) - * - * w_k = vel_k * sin(slip_k) / l_r - * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt - * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt - * yaw_{k+1} = yaw_k + w_k * dt - * vel_{k+1} = vel_k - * slip_{k+1} = slip_k - * - */ - - /* Jacobian Matrix - * - * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, - * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, - * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] - * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, - * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, - * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] - * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/l_r*cos(slip)*dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] - * - */ - - // MOTION MODEL (predict) - - // Current state vector X t - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - 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)); - const double vel = X_t(IDX::VEL); - const double cos_slip = std::cos(X_t(IDX::SLIP)); - const double sin_slip = std::sin(X_t(IDX::SLIP)); - - double w = vel * sin_slip / lr_; - const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - const double w_dtdt = w * dt * dt; - const double vv_dtdt__lr = vel * vel * dt * dt / lr_; - - // Predict state vector X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = - X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt - X_next_t(IDX::Y) = - X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt - X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt - X_next_t(IDX::VEL) = X_t(IDX::VEL); - X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) - - // State transition matrix A - Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; - A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; - A(IDX::X, IDX::SLIP) = - -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; - A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; - A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; - A(IDX::Y, IDX::SLIP) = - vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; - A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; - A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; - - // Process noise covariance Q - float q_stddev_yaw_rate{0.0}; - if (vel <= 0.01) { - q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min; - } else { - // uncertainty of the yaw rate is limited by - // centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v - // or maximum slip angle slip_max : w = v*sin(slip_max)/l_r - q_stddev_yaw_rate = std::min( - ekf_params_.q_stddev_acc_lat / vel, - vel * std::sin(ekf_params_.q_max_slip_angle) / lr_); // [rad/s] - q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max); - q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min); - } - float q_cov_slip_rate{0.0f}; - if (vel <= 0.01) { - q_cov_slip_rate = ekf_params_.q_cov_slip_rate_min; - } else { - // The slip angle rate uncertainty is modeled as follows: - // d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt - // where sin(slip) = w * l_r / v - // d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) - // d(v)/dt and d(w)/t are considered to be uncorrelated - q_cov_slip_rate = - std::pow(ekf_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); - q_cov_slip_rate = std::min(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_max); - q_cov_slip_rate = std::max(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_min); - } - const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2); - const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2); - const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); - const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2); - const float q_cov_slip = q_cov_slip_rate * dt * dt; - - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - // Rotate the covariance matrix according to the vehicle yaw - // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); - Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); - Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); - Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::YAW, IDX::YAW) = q_cov_yaw; - Q(IDX::VEL, IDX::VEL) = q_cov_vel; - Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; - - // control-input model B and control-input u are not used - // Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - // Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); - - if (!ekf.predict(X_next_t, A, Q)) { - RCLCPP_WARN(logger_, "Cannot predict"); - } - return true; } @@ -328,8 +191,7 @@ bool BicycleTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { // current (predicted) state - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - ekf_.getX(X_t); + Eigen::MatrixXd X_t = motion_model_.getStateVector(); // MOTION MODEL (update) @@ -337,85 +199,43 @@ bool BicycleTracker::measureWithPose( double measurement_yaw = 0.0; bool is_yaw_available = utils::getMeasurementYaw(object, X_t(IDX::YAW), measurement_yaw); - // Decide dimension of measurement vector and matrix - // pos x, pos y, (pos yaw) depending on pose measurement - const int dim_y = is_yaw_available ? 3 : 2; - - // Set measurement matrix C and observation vector Y - Eigen::MatrixXd Y(dim_y, 1); - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); - Y(IDX::X, 0) = object.kinematics.pose_with_covariance.pose.position.x; - Y(IDX::Y, 0) = object.kinematics.pose_with_covariance.pose.position.y; - C(0, IDX::X) = 1.0; // for pos x - C(1, IDX::Y) = 1.0; // for pos y - if (is_yaw_available) { - Y(IDX::YAW, 0) = measurement_yaw; - C(2, IDX::YAW) = 1.0; - } - - // Set noise covariance matrix R - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - const auto & pose_cov = object.kinematics.pose_with_covariance.covariance; - R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; - R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; - R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; - if (is_yaw_available) { - R(0, 2) = pose_cov[utils::MSG_COV_IDX::X_YAW]; - R(1, 2) = pose_cov[utils::MSG_COV_IDX::Y_YAW]; - R(2, 0) = pose_cov[utils::MSG_COV_IDX::YAW_X]; - R(2, 1) = pose_cov[utils::MSG_COV_IDX::YAW_Y]; - R(2, 2) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; - } - - // ekf update - if (!ekf_.update(Y, C, R)) { - RCLCPP_WARN(logger_, "Cannot update"); - } + // position z + constexpr float gain = 0.9; + z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; - // normalize yaw and limit vel, slip + // motion model + bool is_updated = false; { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); - ekf_.getX(X_t); - ekf_.getP(P_t); - X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - if (!(-max_vel_ <= X_t(IDX::VEL) && X_t(IDX::VEL) <= max_vel_)) { - X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -max_vel_ : max_vel_; - } - if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { - X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double yaw = measurement_yaw; + + if (is_yaw_available) { + is_updated = motion_model_.updateStatePoseHead( + x, y, yaw, object.kinematics.pose_with_covariance.covariance); + } else { + is_updated = + motion_model_.updateStatePose(x, y, object.kinematics.pose_with_covariance.covariance); } - ekf_.init(X_t, P_t); + motion_model_.limitStates(); } - // position z - constexpr float gain = 0.9; - z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; - - return true; + return is_updated; } bool BicycleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - constexpr float gain = 0.9; + constexpr float gain = 0.2; + constexpr float gain_inv = 1.0 - gain; // update object size - bounding_box_.length = gain * bounding_box_.length + (1.0 - gain) * object.shape.dimensions.x; - bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * object.shape.dimensions.y; - bounding_box_.height = gain * bounding_box_.height + (1.0 - gain) * object.shape.dimensions.z; - last_input_bounding_box_ = { - object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; - - // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 0.3); - bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.3); + bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x; + bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y; + bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; - // update lf, lr - lf_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% front from the center, minimum of 0.3m - lr_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% rear from the center, minimum of 0.3m + // update motion model + motion_model_.updateExtendedState(bounding_box_.length); return true; } @@ -424,21 +244,25 @@ bool BicycleTracker::measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { - const auto & current_classification = getClassification(); + // keep the latest input object object_ = object; + + // update classification + const auto & current_classification = getClassification(); if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { setClassification(current_classification); } + // check time gap if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { RCLCPP_WARN( logger_, "There is a large gap between predicted time and measurement time. (%f)", (time - last_update_time_).seconds()); } + // update object const autoware_auto_perception_msgs::msg::DetectedObject updating_object = getUpdatingObject(object, self_transform); - measureWithPose(updating_object); measureWithShape(updating_object); @@ -452,93 +276,19 @@ bool BicycleTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict state - KalmanFilter tmp_ekf_for_no_update = ekf_; - const double dt = (time - last_update_time_).seconds(); - if (0.001 /*1msec*/ < dt) { - predict(dt, tmp_ekf_for_no_update); - } - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - Eigen::MatrixXd P(ekf_params_.dim_x, ekf_params_.dim_x); // predicted state - tmp_ekf_for_no_update.getX(X_t); - tmp_ekf_for_no_update.getP(P); - - /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; + // predict from motion model + if (!motion_model_.getPredictedState( + time, pose_with_cov.pose, pose_with_cov.covariance, twist_with_cov.twist, + twist_with_cov.covariance)) { + RCLCPP_WARN(logger_, "BicycleTracker::getTrackedObject: Failed to get predicted state."); + return false; + } + // position - pose_with_cov.pose.position.x = X_t(IDX::X); - pose_with_cov.pose.position.y = X_t(IDX::Y); pose_with_cov.pose.position.z = z_; - // quaternion - { - double roll, pitch, yaw; - tf2::Quaternion original_quaternion; - tf2::fromMsg(object_.kinematics.pose_with_covariance.pose.orientation, original_quaternion); - tf2::Matrix3x3(original_quaternion).getRPY(roll, pitch, yaw); - tf2::Quaternion filtered_quaternion; - filtered_quaternion.setRPY(roll, pitch, X_t(IDX::YAW)); - pose_with_cov.pose.orientation.x = filtered_quaternion.x(); - pose_with_cov.pose.orientation.y = filtered_quaternion.y(); - pose_with_cov.pose.orientation.z = filtered_quaternion.z(); - pose_with_cov.pose.orientation.w = filtered_quaternion.w(); - // orientation availability is assumed to be unknown - object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; - } - // position covariance - constexpr double z_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double r_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double p_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = r_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = p_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); - - // twist - twist_with_cov.twist.linear.x = X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)); - twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)); - twist_with_cov.twist.angular.z = - X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r - /* twist covariance - * convert covariance from velocity, slip angle to vx, vy, and yaw angle - * - * vx = vel * cos(slip) - * vy = vel * sin(slip) - * wz = vel * sin(slip) / l_r = vy / l_r - * - */ - - constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - - Eigen::MatrixXd cov_jacob(3, 2); - cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)), - std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)), - std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_; - Eigen::MatrixXd cov_twist(2, 2); - cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), - P(IDX::SLIP, IDX::SLIP); - Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); - - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; // set shape object.shape.dimensions.x = bounding_box_.length; diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 79965d4470512..c56a556d29704 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -75,6 +75,11 @@ BigVehicleTracker::BigVehicleTracker( ekf_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0); ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); + // velocity deviation threshold + // if the predicted velocity is close to the observed velocity, + // the observed velocity is used as the measurement. + velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] + // initialize state vector X Eigen::MatrixXd X(ekf_params_.dim_x, 1); X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; @@ -273,6 +278,7 @@ bool BigVehicleTracker::measureWithPose( z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; // motion model + bool is_updated = false; { const double x = object.kinematics.pose_with_covariance.pose.position.x; const double y = object.kinematics.pose_with_covariance.pose.position.y; @@ -280,17 +286,17 @@ bool BigVehicleTracker::measureWithPose( const double vel = object.kinematics.twist_with_covariance.twist.linear.x; if (is_velocity_available) { - motion_model_.updateStatePoseHeadVel( + is_updated = motion_model_.updateStatePoseHeadVel( x, y, yaw, vel, object.kinematics.pose_with_covariance.covariance, object.kinematics.twist_with_covariance.covariance); } else { - motion_model_.updateStatePoseHead( + is_updated = motion_model_.updateStatePoseHead( x, y, yaw, object.kinematics.pose_with_covariance.covariance); } motion_model_.limitStates(); } - return true; + return is_updated; } bool BigVehicleTracker::measureWithShape( @@ -353,7 +359,6 @@ bool BigVehicleTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index c8556a115d4fd..9a2ed7fb66cfb 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -75,6 +75,11 @@ NormalVehicleTracker::NormalVehicleTracker( ekf_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0); ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); + // velocity deviation threshold + // if the predicted velocity is close to the observed velocity, + // the observed velocity is used as the measurement. + velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] + // initialize state vector X Eigen::MatrixXd X(ekf_params_.dim_x, 1); X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; @@ -273,6 +278,7 @@ bool NormalVehicleTracker::measureWithPose( z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; // motion model + bool is_updated = false; { const double x = object.kinematics.pose_with_covariance.pose.position.x; const double y = object.kinematics.pose_with_covariance.pose.position.y; @@ -280,17 +286,17 @@ bool NormalVehicleTracker::measureWithPose( const double vel = object.kinematics.twist_with_covariance.twist.linear.x; if (is_velocity_available) { - motion_model_.updateStatePoseHeadVel( + is_updated = motion_model_.updateStatePoseHeadVel( x, y, yaw, vel, object.kinematics.pose_with_covariance.covariance, object.kinematics.twist_with_covariance.covariance); } else { - motion_model_.updateStatePoseHead( + is_updated = motion_model_.updateStatePoseHead( x, y, yaw, object.kinematics.pose_with_covariance.covariance); } motion_model_.limitStates(); } - return true; + return is_updated; } bool NormalVehicleTracker::measureWithShape( @@ -353,7 +359,6 @@ bool NormalVehicleTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; From cbfbc1610b7d740dbf8dcd8f8d72137f282e92a5 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 5 Mar 2024 16:32:32 +0900 Subject: [PATCH 15/42] chore: remove non-used members Signed-off-by: Taekjin LEE --- .../motion_model/bicycle_motion_model.hpp | 7 ++-- .../tracker/model/bicycle_tracker.hpp | 4 +-- .../tracker/model/big_vehicle_tracker.hpp | 4 +-- .../tracker/model/normal_vehicle_tracker.hpp | 4 +-- .../src/motion_model/bicycle_motion_model.cpp | 15 ++++----- .../src/tracker/model/bicycle_tracker.cpp | 9 ++--- .../src/tracker/model/big_vehicle_tracker.cpp | 9 ++--- .../tracker/model/normal_vehicle_tracker.cpp | 9 ++--- .../src/tracker/model/pedestrian_tracker.cpp | 33 +++---------------- .../src/tracker/model/unknown_tracker.cpp | 4 +-- 10 files changed, 36 insertions(+), 62 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp index 2340a5fd7391b..01bca6356e8fd 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp @@ -64,13 +64,12 @@ class BicycleMotionModel double dt_max; } motion_params_; -protected: - enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; - const char DIM = 5; - public: BicycleMotionModel(); + enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; + const char DIM = 5; + bool init( const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P, const double & length); diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 7a62b3ccb8c0f..fbe29228f0009 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -32,11 +32,9 @@ class BicycleTracker : public Tracker private: rclcpp::Time last_update_time_; - enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; struct EkfParams { - char dim_x = 5; float p0_cov_vel; float p0_cov_slip; float p0_cov_x; @@ -59,6 +57,8 @@ class BicycleTracker : public Tracker private: BicycleMotionModel motion_model_; + const char DIM = motion_model_.DIM; + using IDX = BicycleMotionModel::IDX; public: BicycleTracker( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index ef314bcd4619b..2682e13f5e57c 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -32,10 +32,8 @@ class BigVehicleTracker : public Tracker private: rclcpp::Time last_update_time_; - enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; struct EkfParams { - char dim_x = 5; float p0_cov_vel; float p0_cov_slip; float p0_cov_x; @@ -62,6 +60,8 @@ class BigVehicleTracker : public Tracker private: BicycleMotionModel motion_model_; + const char DIM = motion_model_.DIM; + using IDX = BicycleMotionModel::IDX; public: BigVehicleTracker( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 0935ed7643b81..c237ef5dc21c5 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -32,11 +32,9 @@ class NormalVehicleTracker : public Tracker private: rclcpp::Time last_update_time_; - enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; struct EkfParams { - char dim_x = 5; float p0_cov_vel; float p0_cov_slip; float p0_cov_x; @@ -63,6 +61,8 @@ class NormalVehicleTracker : public Tracker private: BicycleMotionModel motion_model_; + const char DIM = motion_model_.DIM; + using IDX = BicycleMotionModel::IDX; public: NormalVehicleTracker( diff --git a/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp index 21849ce085a94..31bc7ad69bf21 100644 --- a/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp @@ -27,6 +27,9 @@ #include #include +// Bicycle CTRV motion model +// CTRV : Constant turn-rate and velocity + BicycleMotionModel::BicycleMotionModel() : logger_(rclcpp::get_logger("BicycleMotionModel")), last_update_time_(rclcpp::Time(0, 0)) { @@ -293,17 +296,13 @@ bool BicycleMotionModel::predictState(const rclcpp::Time & time) // if dt is too large, shorten dt and repeat prediction const uint32_t repeat = std::ceil(dt / motion_params_.dt_max); const double dt_ = dt / repeat; - bool ret = false; for (uint32_t i = 0; i < repeat; ++i) { - ret = predictState(dt_, ekf_); - if (!ret) { + if (!predictState(dt_, ekf_)) { return false; } - } - if (ret) { last_update_time_ = time; } - return ret; + return true; } bool BicycleMotionModel::predictState(const double dt, KalmanFilter & ekf) const @@ -440,15 +439,13 @@ bool BicycleMotionModel::getPredictedState( dt = 0.0; } - bool ret = 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 / motion_params_.dt_max); const double dt_ = dt / repeat; for (uint32_t i = 0; i < repeat; ++i) { - ret = predictState(dt_, tmp_ekf_for_no_update); - if (!ret) { + if (!predictState(dt_, tmp_ekf_for_no_update)) { return false; } } diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 261076e2886fb..9db8510539c21 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -73,7 +73,7 @@ BicycleTracker::BicycleTracker( ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); // initialize state vector X - Eigen::MatrixXd X(ekf_params_.dim_x, 1); + Eigen::MatrixXd X(DIM, 1); X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); @@ -86,7 +86,7 @@ BicycleTracker::BicycleTracker( // UNCERTAINTY MODEL // initialize state covariance matrix P - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(DIM, DIM); if (!object.kinematics.has_position_covariance) { const double cos_yaw = std::cos(X(IDX::YAW)); const double sin_yaw = std::sin(X(IDX::YAW)); @@ -154,10 +154,11 @@ BicycleTracker::BicycleTracker( bool BicycleTracker::predict(const rclcpp::Time & time) { // predict state vector X t+1 - if (motion_model_.predictState(time)) { + bool is_predicted = motion_model_.predictState(time); + if (is_predicted) { last_update_time_ = time; } - return true; + return is_predicted; } autoware_auto_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index c56a556d29704..a17c3a1cc36fc 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -81,7 +81,7 @@ BigVehicleTracker::BigVehicleTracker( velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] // initialize state vector X - Eigen::MatrixXd X(ekf_params_.dim_x, 1); + Eigen::MatrixXd X(DIM, 1); X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); @@ -94,7 +94,7 @@ BigVehicleTracker::BigVehicleTracker( // UNCERTAINTY MODEL // initialize state covariance matrix P - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(DIM, DIM); if (!object.kinematics.has_position_covariance) { const double cos_yaw = std::cos(X(IDX::YAW)); const double sin_yaw = std::sin(X(IDX::YAW)); @@ -168,10 +168,11 @@ BigVehicleTracker::BigVehicleTracker( bool BigVehicleTracker::predict(const rclcpp::Time & time) { // predict state vector X t+1 - if (motion_model_.predictState(time)) { + bool is_predicted = motion_model_.predictState(time); + if (is_predicted) { last_update_time_ = time; } - return true; + return is_predicted; } autoware_auto_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObject( diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 9a2ed7fb66cfb..1fd8c2410d7fe 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -81,7 +81,7 @@ NormalVehicleTracker::NormalVehicleTracker( velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] // initialize state vector X - Eigen::MatrixXd X(ekf_params_.dim_x, 1); + Eigen::MatrixXd X(DIM, 1); X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); @@ -94,7 +94,7 @@ NormalVehicleTracker::NormalVehicleTracker( // UNCERTAINTY MODEL // initialize state covariance matrix P - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(DIM, DIM); if (!object.kinematics.has_position_covariance) { const double cos_yaw = std::cos(X(IDX::YAW)); const double sin_yaw = std::sin(X(IDX::YAW)); @@ -168,10 +168,11 @@ NormalVehicleTracker::NormalVehicleTracker( bool NormalVehicleTracker::predict(const rclcpp::Time & time) { // predict state vector X t+1 - if (motion_model_.predictState(time)) { + bool is_predicted = motion_model_.predictState(time); + if (is_predicted) { last_update_time_ = time; } - return true; + return is_predicted; } autoware_auto_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingObject( diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index b5b3bd5826e69..49ef421ec3a03 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -99,6 +99,7 @@ PedestrianTracker::PedestrianTracker( X(IDX::WZ) = 0.0; } + // UNCERTAINTY MODEL // initialize state covariance matrix P Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); if (!object.kinematics.has_position_covariance) { @@ -134,6 +135,7 @@ PedestrianTracker::PedestrianTracker( } } + // OBJECT SHAPE MODEL bounding_box_ = {0.5, 0.5, 1.7}; cylinder_ = {0.3, 1.7}; if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { @@ -143,18 +145,13 @@ PedestrianTracker::PedestrianTracker( cylinder_ = {object.shape.dimensions.x, object.shape.dimensions.z}; } - // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 0.3); - bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.3); - cylinder_.width = std::max(cylinder_.width, 0.3); - cylinder_.height = std::max(cylinder_.height, 0.3); - + // Set motion model ekf_.init(X, P); } bool PedestrianTracker::predict(const rclcpp::Time & time) { + // Set motion model const double dt = (time - last_update_time_).seconds(); bool ret = predict(dt, ekf_); if (ret) { @@ -235,22 +232,6 @@ bool PedestrianTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { constexpr int dim_y = 2; // pos x, pos y depending on Pose output - // double measurement_yaw = - // tier4_autoware_utils::normalizeRadian(tf2::getYaw(object.state.pose_covariance.pose.orientation)); - // { - // Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - // ekf_.getX(X_t); - // while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) { - // measurement_yaw = measurement_yaw + M_PI; - // } - // while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) { - // measurement_yaw = measurement_yaw - M_PI; - // } - // float theta = std::acos( - // std::cos(X_t(IDX::YAW)) * std::cos(measurement_yaw) + - // std::sin(X_t(IDX::YAW)) * std::sin(measurement_yaw)); - // if (tier4_autoware_utils::deg2rad(60) < std::fabs(theta)) return false; - // } // Set measurement matrix C and observation vector Y Eigen::MatrixXd Y(dim_y, 1); @@ -268,17 +249,11 @@ bool PedestrianTracker::measureWithPose( R(0, 1) = 0.0; // x - y R(1, 1) = ekf_params_.r_cov_y; // y - y R(1, 0) = R(0, 1); // y - x - // R(2, 2) = ekf_params_.r_cov_yaw; // yaw - yaw } else { R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - // R(0, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_YAW]; R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - // R(1, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_YAW]; - // R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X]; - // R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y]; - // R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; } // ekf update diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp index 897b858a6aabe..77cf51d3d505f 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -122,7 +122,7 @@ bool UnknownTracker::predict(const rclcpp::Time & time) bool UnknownTracker::predict(const double dt, KalmanFilter & ekf) const { - /* == Nonlinear model == + /* Motion model: Constant velocity model * * x_{k+1} = x_k + vx_k * dt * y_{k+1} = y_k + vx_k * dt @@ -131,7 +131,7 @@ bool UnknownTracker::predict(const double dt, KalmanFilter & ekf) const * */ - /* == Linearized model == + /* Jacobian Matrix * * A = [ 1, 0, dt, 0] * [ 0, 1, 0, dt] From 10b1ba6e1f9ee430493b04d659bae32be0d0d3d0 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 5 Mar 2024 17:18:37 +0900 Subject: [PATCH 16/42] feat: add CTRV motion model and implement it to pedestrian tracker Signed-off-by: Taekjin LEE --- .../multi_object_tracker/CMakeLists.txt | 1 + .../motion_model/bicycle_motion_model.hpp | 2 - .../motion_model/ctrv_motion_model.hpp | 115 +++++ .../tracker/model/pedestrian_tracker.hpp | 30 +- .../src/motion_model/bicycle_motion_model.cpp | 8 +- .../src/motion_model/ctrv_motion_model.cpp | 437 ++++++++++++++++++ .../src/tracker/model/pedestrian_tracker.cpp | 332 +++++-------- 7 files changed, 687 insertions(+), 238 deletions(-) create mode 100644 perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp create mode 100644 perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt index dfe34c1e6a74f..605ab4ccfff1d 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/multi_object_tracker/CMakeLists.txt @@ -34,6 +34,7 @@ set(MULTI_OBJECT_TRACKER_SRC src/data_association/data_association.cpp src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp src/motion_model/bicycle_motion_model.cpp + src/motion_model/ctrv_motion_model.cpp ) ament_auto_add_library(multi_object_tracker_node SHARED diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp index 01bca6356e8fd..2251a74db8a77 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp @@ -53,8 +53,6 @@ class BicycleMotionModel double q_cov_slip_rate_min; double q_cov_slip_rate_max; double q_max_slip_angle; - double p0_cov_vel; - double p0_cov_slip; double lf_ratio; double lr_ratio; double lf_min; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp new file mode 100644 index 0000000000000..a15093795b272 --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp @@ -0,0 +1,115 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Taekjin Lee +// + +#ifndef MULTI_OBJECT_TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ +#define MULTI_OBJECT_TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ + +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include +#include + +#include "autoware_auto_perception_msgs/msg/detected_object.hpp" + +class CTRVMotionModel +{ +private: + // attributes + rclcpp::Logger logger_; + rclcpp::Time last_update_time_; + +private: + // state + bool is_initialized_{false}; + KalmanFilter ekf_; + + // motion parameters + struct MotionParams + { + double q_cov_x; + double q_cov_y; + double q_cov_yaw; + double q_cov_vx; + double q_cov_wz; + double max_vel; + double max_wz; + double dt_max; + } motion_params_; + +public: + CTRVMotionModel(); + + enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, WZ = 4 }; + const char DIM = 5; + + bool init(const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P); + + bool checkInitialized() const + { + // if the state is not initialized, return false + if (!is_initialized_) { + RCLCPP_WARN(logger_, "CTRVMotionModel is not initialized."); + return false; + } + return true; + } + + void setDefaultParams(); + + void setMotionParams( + const double & q_stddev_x, const double & q_stddev_y, const double & q_stddev_yaw, + const double & q_stddev_vx, const double & q_stddev_wz); + + void setMotionLimits(const double & max_vel, const double & max_wz); + + Eigen::MatrixXd getStateVector() const + { + Eigen::MatrixXd X_t(DIM, 1); + ekf_.getX(X_t); + return X_t; + } + + double getStateElement(unsigned int idx) const { return ekf_.getXelement(idx); } + + bool updateStatePose(const double & x, const double & y, const std::array & pose_cov); + + bool updateStatePoseHead( + const double & x, const double & y, const double & yaw, + const std::array & pose_cov); + + bool updateStatePoseHeadVel( + const double & x, const double & y, const double & yaw, const double & vel, + const std::array & pose_cov, const std::array & twist_cov); + + bool adjustPosition(const double & x, const double & y); + + bool limitStates(); + + bool predictState(const rclcpp::Time & time); + + bool predictState(const double dt, KalmanFilter & ekf) const; + + bool getPredictedState(const rclcpp::Time & time, Eigen::MatrixXd & X, Eigen::MatrixXd & P) const; + + bool getPredictedState( + const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, + geometry_msgs::msg::Twist & twist, std::array & twist_cov) const; +}; + +#endif // MULTI_OBJECT_TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index 0219d3b227044..0de31edec660f 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -19,6 +19,7 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ +#include "multi_object_tracker/motion_model/ctrv_motion_model.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" #include @@ -30,35 +31,18 @@ class PedestrianTracker : public Tracker rclcpp::Logger logger_; private: - KalmanFilter ekf_; rclcpp::Time last_update_time_; - enum IDX { - X = 0, - Y = 1, - YAW = 2, - VX = 3, - WZ = 4, - }; struct EkfParams { - char dim_x = 5; - float q_cov_x; - float q_cov_y; - float q_cov_yaw; - float q_cov_wz; - float q_cov_vx; float p0_cov_vx; float p0_cov_wz; - float r_cov_x; - float r_cov_y; - float r_cov_yaw; float p0_cov_x; float p0_cov_y; float p0_cov_yaw; + float r_cov_x; + float r_cov_y; + float r_cov_yaw; } ekf_params_; - - double max_vx_; - double max_wz_; float z_; private: @@ -76,13 +60,17 @@ class PedestrianTracker : public Tracker BoundingBox bounding_box_; Cylinder cylinder_; +private: + CTRVMotionModel motion_model_; + const char DIM = motion_model_.DIM; + using IDX = CTRVMotionModel::IDX; + public: PedestrianTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & self_transform); bool predict(const rclcpp::Time & time) override; - bool predict(const double dt, KalmanFilter & ekf) const; bool measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; diff --git a/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp index 31bc7ad69bf21..c868ec51d10be 100644 --- a/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp @@ -28,7 +28,7 @@ #include // Bicycle CTRV motion model -// CTRV : Constant turn-rate and velocity +// CTRV : Constant Turn Rate and constant Velocity BicycleMotionModel::BicycleMotionModel() : logger_(rclcpp::get_logger("BicycleMotionModel")), last_update_time_(rclcpp::Time(0, 0)) @@ -84,12 +84,6 @@ void BicycleMotionModel::setDefaultParams() // set prediction parameters constexpr double dt_max = 0.11; // [s] motion_params_.dt_max = dt_max; - - // set initial covariance for velocity and slip angle - constexpr double p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); - constexpr double p0_stddev_slip = tier4_autoware_utils::deg2rad(5); - motion_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0); - motion_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); } void BicycleMotionModel::setMotionParams( diff --git a/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp new file mode 100644 index 0000000000000..151b33eb532be --- /dev/null +++ b/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp @@ -0,0 +1,437 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Taekjin Lee +// + +#include "multi_object_tracker/motion_model/ctrv_motion_model.hpp" + +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +// Constant Turn Rate and constant Velocity (CTRV) motion model + +CTRVMotionModel::CTRVMotionModel() +: logger_(rclcpp::get_logger("CTRVMotionModel")), last_update_time_(rclcpp::Time(0, 0)) +{ + // Initialize motion parameters + setDefaultParams(); +} + +bool CTRVMotionModel::init( + const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P) +{ + // set last update time + last_update_time_ = time; + + // initialize Kalman filter + ekf_.init(X, P); + + // // set initial extended state + // updateExtendedState(length); + + // set initialized flag + is_initialized_ = true; + + return true; +} + +void CTRVMotionModel::setDefaultParams() +{ + // process noise covariance + constexpr double q_stddev_x = 0.4; // [m/s] + constexpr double q_stddev_y = 0.4; // [m/s] + constexpr double q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] + constexpr double q_stddev_vx = tier4_autoware_utils::kmph2mps(5); // [m/(s*s)] + constexpr double q_stddev_wz = tier4_autoware_utils::deg2rad(30); // [rad/(s*s)] + + setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vx, q_stddev_wz); + + // set motion limitations + constexpr double max_vel = tier4_autoware_utils::kmph2mps(10); // [m/s] + constexpr double max_wz = 30.0; // [deg] + setMotionLimits(max_vel, max_wz); + + // set prediction parameters + constexpr double dt_max = 0.11; // [s] + motion_params_.dt_max = dt_max; +} + +void CTRVMotionModel::setMotionParams( + const double & q_stddev_x, const double & q_stddev_y, const double & q_stddev_yaw, + const double & q_stddev_vx, const double & q_stddev_wz) +{ + // set process noise covariance parameters + motion_params_.q_cov_x = std::pow(q_stddev_x, 2.0); + motion_params_.q_cov_y = std::pow(q_stddev_y, 2.0); + motion_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); + motion_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); + motion_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0); +} + +void CTRVMotionModel::setMotionLimits(const double & max_vel, const double & max_wz) +{ + // set motion limitations + motion_params_.max_vel = max_vel; + motion_params_.max_wz = tier4_autoware_utils::deg2rad(max_wz); +} + +bool CTRVMotionModel::updateStatePose( + const double & x, const double & y, const std::array & pose_cov) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // update state, without velocity + constexpr int DIM_Y = 2; + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + + return ekf_.update(Y, C, R); +} + +bool CTRVMotionModel::updateStatePoseHead( + const double & x, const double & y, const double & yaw, const std::array & pose_cov) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // update state, without velocity + constexpr int DIM_Y = 3; + + // fix yaw + double estimated_yaw = getStateElement(IDX::YAW); + double fixed_yaw = yaw; + double limiting_delta_yaw = M_PI_2; + while (std::fabs(estimated_yaw - fixed_yaw) > limiting_delta_yaw) { + if (fixed_yaw < estimated_yaw) { + fixed_yaw += 2 * limiting_delta_yaw; + } else { + fixed_yaw -= 2 * limiting_delta_yaw; + } + } + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y, fixed_yaw; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + C(2, IDX::YAW) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + R(0, 2) = pose_cov[utils::MSG_COV_IDX::X_YAW]; + R(1, 2) = pose_cov[utils::MSG_COV_IDX::Y_YAW]; + R(2, 0) = pose_cov[utils::MSG_COV_IDX::YAW_X]; + R(2, 1) = pose_cov[utils::MSG_COV_IDX::YAW_Y]; + R(2, 2) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; + + return ekf_.update(Y, C, R); +} + +bool CTRVMotionModel::updateStatePoseHeadVel( + const double & x, const double & y, const double & yaw, const double & vel, + const std::array & pose_cov, const std::array & twist_cov) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // update state, with velocity + constexpr int DIM_Y = 4; + + // fix yaw + double estimated_yaw = getStateElement(IDX::YAW); + double fixed_yaw = yaw; + double limiting_delta_yaw = M_PI_2; + while (std::fabs(estimated_yaw - fixed_yaw) > limiting_delta_yaw) { + if (fixed_yaw < estimated_yaw) { + fixed_yaw += 2 * limiting_delta_yaw; + } else { + fixed_yaw -= 2 * limiting_delta_yaw; + } + } + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y, yaw, vel; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + C(2, IDX::YAW) = 1.0; + C(3, IDX::VEL) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + R(0, 2) = pose_cov[utils::MSG_COV_IDX::X_YAW]; + R(1, 2) = pose_cov[utils::MSG_COV_IDX::Y_YAW]; + R(2, 0) = pose_cov[utils::MSG_COV_IDX::YAW_X]; + R(2, 1) = pose_cov[utils::MSG_COV_IDX::YAW_Y]; + R(2, 2) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; + R(3, 3) = twist_cov[utils::MSG_COV_IDX::X_X]; + + return ekf_.update(Y, C, R); +} + +bool CTRVMotionModel::limitStates() +{ + Eigen::MatrixXd X_t(DIM, 1); + Eigen::MatrixXd P_t(DIM, DIM); + ekf_.getX(X_t); + ekf_.getP(P_t); + X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); + if (!(-motion_params_.max_vel <= X_t(IDX::VEL) && X_t(IDX::VEL) <= motion_params_.max_vel)) { + X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -motion_params_.max_vel : motion_params_.max_vel; + } + if (!(-motion_params_.max_wz <= X_t(IDX::WZ) && X_t(IDX::WZ) <= motion_params_.max_wz)) { + X_t(IDX::WZ) = X_t(IDX::WZ) < 0 ? -motion_params_.max_wz : motion_params_.max_wz; + } + ekf_.init(X_t, P_t); + + return true; +} + +bool CTRVMotionModel::adjustPosition(const double & x, const double & y) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // adjust position + Eigen::MatrixXd X_t(DIM, 1); + Eigen::MatrixXd P_t(DIM, DIM); + ekf_.getX(X_t); + ekf_.getP(P_t); + X_t(IDX::X) += x; + X_t(IDX::Y) += y; + ekf_.init(X_t, P_t); + + return true; +} + +bool CTRVMotionModel::predictState(const rclcpp::Time & time) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + const double dt = (time - last_update_time_).seconds(); + if (dt < 0.0) { + RCLCPP_WARN(logger_, "CTRVMotionModel::predictState: dt is negative. (%f)", dt); + return false; + } + // if dt is too large, shorten dt and repeat prediction + const uint32_t repeat = std::ceil(dt / motion_params_.dt_max); + const double dt_ = dt / repeat; + for (uint32_t i = 0; i < repeat; ++i) { + if (!predictState(dt_, ekf_)) { + return false; + } + last_update_time_ = time; + } + return true; +} + +bool CTRVMotionModel::predictState(const double dt, KalmanFilter & ekf) const +{ + /* Motion model: Constant Turn Rate and constant Velocity model (CTRV) + * + * x_{k+1} = x_k + vx_k * cos(yaw_k) * dt + * y_{k+1} = y_k + vx_k * sin(yaw_k) * dt + * yaw_{k+1} = yaw_k + (wz_k) * dt + * vx_{k+1} = vx_k + * wz_{k+1} = wz_k + * + */ + + /* Jacobian Matrix + * + * A = [ 1, 0, -vx*sin(yaw)*dt, cos(yaw)*dt, 0] + * [ 0, 1, vx*cos(yaw)*dt, sin(yaw)*dt, 0] + * [ 0, 0, 1, 0, dt] + * [ 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 1] + */ + + // MOTION MODEL (predict) + + // Current state vector X t + Eigen::MatrixXd X_t = getStateVector(); + + const double cos_yaw = std::cos(X_t(IDX::YAW)); + const double sin_yaw = std::sin(X_t(IDX::YAW)); + const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); + + // Predict state vector X t+1 + Eigen::MatrixXd X_next_t(DIM, 1); // predicted state + X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VEL) * cos_yaw * dt; // dx = v * cos(yaw) + X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VEL) * sin_yaw * dt; // dy = v * sin(yaw) + X_next_t(IDX::YAW) = X_t(IDX::YAW) + (X_t(IDX::WZ)) * dt; // dyaw = omega + X_next_t(IDX::VEL) = X_t(IDX::VEL); + X_next_t(IDX::WZ) = X_t(IDX::WZ); + + // State transition matrix A + Eigen::MatrixXd A = Eigen::MatrixXd::Identity(DIM, DIM); + A(IDX::X, IDX::YAW) = -X_t(IDX::VEL) * sin_yaw * dt; + A(IDX::X, IDX::VEL) = cos_yaw * dt; + A(IDX::Y, IDX::YAW) = X_t(IDX::VEL) * cos_yaw * dt; + A(IDX::Y, IDX::VEL) = sin_yaw * dt; + A(IDX::YAW, IDX::WZ) = dt; + + // Process noise covariance Q + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(DIM, DIM); + // Rotate the covariance matrix according to the vehicle yaw + // because q_cov_x and y are in the vehicle coordinate system. + Q(IDX::X, IDX::X) = + (motion_params_.q_cov_x * cos_yaw * cos_yaw + motion_params_.q_cov_y * sin_yaw * sin_yaw) * dt * + dt; + Q(IDX::X, IDX::Y) = + (0.5f * (motion_params_.q_cov_x - motion_params_.q_cov_y) * sin_2yaw) * dt * dt; + Q(IDX::Y, IDX::Y) = + (motion_params_.q_cov_x * sin_yaw * sin_yaw + motion_params_.q_cov_y * cos_yaw * cos_yaw) * dt * + dt; + Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); + Q(IDX::YAW, IDX::YAW) = motion_params_.q_cov_yaw * dt * dt; + Q(IDX::VEL, IDX::VEL) = motion_params_.q_cov_vx * dt * dt; + Q(IDX::WZ, IDX::WZ) = motion_params_.q_cov_wz * dt * dt; + + // control-input model B and control-input u are not used + // Eigen::MatrixXd B = Eigen::MatrixXd::Zero(DIM, DIM); + // Eigen::MatrixXd u = Eigen::MatrixXd::Zero(DIM, 1); + + // predict state + return ekf.predict(X_next_t, A, Q); +} + +bool CTRVMotionModel::getPredictedState( + const rclcpp::Time & time, Eigen::MatrixXd & X, Eigen::MatrixXd & P) const +{ + // 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 = (time - last_update_time_).seconds(); + if (dt < 0.0) { + RCLCPP_WARN(logger_, "CTRVMotionModel::getPredictedState: dt is negative. (%f)", dt); + 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 / motion_params_.dt_max); + const double dt_ = dt / repeat; + for (uint32_t i = 0; i < repeat; ++i) { + if (!predictState(dt_, tmp_ekf_for_no_update)) { + return false; + } + } + } + tmp_ekf_for_no_update.getX(X); + tmp_ekf_for_no_update.getP(P); + return true; +} + +bool CTRVMotionModel::getPredictedState( + const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, + geometry_msgs::msg::Twist & twist, std::array & twist_cov) const +{ + // get predicted state + Eigen::MatrixXd X(DIM, 1); + Eigen::MatrixXd P(DIM, DIM); + if (!getPredictedState(time, X, P)) { + return false; + } + + // set position + pose.position.x = X(IDX::X); + pose.position.y = X(IDX::Y); + pose.position.z = 0.0; + + // set orientation + tf2::Quaternion quaternion; + quaternion.setRPY(0.0, 0.0, X(IDX::YAW)); + pose.orientation.x = quaternion.x(); + pose.orientation.y = quaternion.y(); + pose.orientation.z = quaternion.z(); + pose.orientation.w = quaternion.w(); + + // set twist + twist.linear.x = X(IDX::VEL); + twist.linear.y = 0.0; + twist.linear.z = 0.0; + twist.angular.x = 0.0; + twist.angular.y = 0.0; + twist.angular.z = X(IDX::WZ); + + // set pose covariance + constexpr double zz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double rr_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double pp_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + pose_cov[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); + pose_cov[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); + pose_cov[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); + pose_cov[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); + pose_cov[utils::MSG_COV_IDX::Z_Z] = zz_cov; + pose_cov[utils::MSG_COV_IDX::ROLL_ROLL] = rr_cov; + pose_cov[utils::MSG_COV_IDX::PITCH_PITCH] = pp_cov; + + // set twist covariance + constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + twist_cov[utils::MSG_COV_IDX::X_X] = P(IDX::VEL, IDX::VEL); + twist_cov[utils::MSG_COV_IDX::X_Y] = 0.0; + twist_cov[utils::MSG_COV_IDX::X_YAW] = P(IDX::VEL, IDX::WZ); + twist_cov[utils::MSG_COV_IDX::Y_X] = 0.0; + twist_cov[utils::MSG_COV_IDX::Y_Y] = vy_cov; + twist_cov[utils::MSG_COV_IDX::Y_YAW] = 0.0; + twist_cov[utils::MSG_COV_IDX::YAW_X] = 0.0; + twist_cov[utils::MSG_COV_IDX::YAW_Y] = 0.0; + twist_cov[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ); + twist_cov[utils::MSG_COV_IDX::Z_Z] = vz_cov; + twist_cov[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; + twist_cov[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; + + return true; +} diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index 49ef421ec3a03..0d153fad127ab 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -71,37 +71,23 @@ PedestrianTracker::PedestrianTracker( ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); ekf_params_.p0_cov_wz = std::pow(p0_stddev_wz, 2.0); - // process noise covariance - float q_stddev_x = 0.4; // [m/s] - float q_stddev_y = 0.4; // [m/s] - float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] - float q_stddev_vx = tier4_autoware_utils::kmph2mps(5); // [m/(s*s)] - float q_stddev_wz = tier4_autoware_utils::deg2rad(30); // [rad/(s*s)] - ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); - ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); - ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); - ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); - ekf_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0); - // limitations - max_vx_ = tier4_autoware_utils::kmph2mps(10); // [m/s] - max_wz_ = tier4_autoware_utils::deg2rad(30); // [rad/s] // initialize state vector X - Eigen::MatrixXd X(ekf_params_.dim_x, 1); + Eigen::MatrixXd X(DIM, 1); X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); if (object.kinematics.has_twist) { - X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x; + X(IDX::VEL) = object.kinematics.twist_with_covariance.twist.linear.x; X(IDX::WZ) = object.kinematics.twist_with_covariance.twist.angular.z; } else { - X(IDX::VX) = 0.0; + X(IDX::VEL) = 0.0; X(IDX::WZ) = 0.0; } // UNCERTAINTY MODEL // initialize state covariance matrix P - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(DIM, DIM); if (!object.kinematics.has_position_covariance) { const double cos_yaw = std::cos(X(IDX::YAW)); const double sin_yaw = std::sin(X(IDX::YAW)); @@ -115,7 +101,7 @@ PedestrianTracker::PedestrianTracker( ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vx; P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz; } else { P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; @@ -125,12 +111,12 @@ PedestrianTracker::PedestrianTracker( P(IDX::YAW, IDX::YAW) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; if (object.kinematics.has_twist_covariance) { - P(IDX::VX, IDX::VX) = + P(IDX::VEL, IDX::VEL) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; P(IDX::WZ, IDX::WZ) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; } else { - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vx; P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz; } } @@ -146,155 +132,68 @@ PedestrianTracker::PedestrianTracker( } // Set motion model - ekf_.init(X, P); + motion_model_.init(time, X, P); + + // Set motion model parameters + constexpr double q_stddev_x = 0.4; // [m/s] + constexpr double q_stddev_y = 0.4; // [m/s] + constexpr double q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] + constexpr double q_stddev_vx = tier4_autoware_utils::kmph2mps(5); // [m/(s*s)] + constexpr double q_stddev_wz = tier4_autoware_utils::deg2rad(30); // [rad/(s*s)] + motion_model_.setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vx, q_stddev_wz); + + // Set motion limits + motion_model_.setMotionLimits( + tier4_autoware_utils::kmph2mps(100), /* [m/s] maximum velocity */ + 30.0 /* [deg/s] maximum turn rate */ + ); } bool PedestrianTracker::predict(const rclcpp::Time & time) { - // Set motion model - const double dt = (time - last_update_time_).seconds(); - bool ret = predict(dt, ekf_); - if (ret) { + // predict state vector X t+1 + bool is_predicted = motion_model_.predictState(time); + if (is_predicted) { last_update_time_ = time; } - return ret; -} - -bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const -{ - // cspell: ignore CTRV - /* Motion model: Constant turn-rate motion model (CTRV) - * - * x_{k+1} = x_k + vx_k * cos(yaw_k) * dt - * y_{k+1} = y_k + vx_k * sin(yaw_k) * dt - * yaw_{k+1} = yaw_k + (wz_k) * dt - * vx_{k+1} = vx_k - * wz_{k+1} = wz_k - * - */ - - /* Jacobian Matrix - * - * A = [ 1, 0, -vx*sin(yaw)*dt, cos(yaw)*dt, 0] - * [ 0, 1, vx*cos(yaw)*dt, sin(yaw)*dt, 0] - * [ 0, 0, 1, 0, dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] - */ - - // Current state vector X t - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf.getX(X_t); - const double cos_yaw = std::cos(X_t(IDX::YAW)); - const double sin_yaw = std::sin(X_t(IDX::YAW)); - const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - - // Predict state vector X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VX) * cos_yaw * dt; // dx = v * cos(yaw) - X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VX) * sin_yaw * dt; // dy = v * sin(yaw) - X_next_t(IDX::YAW) = X_t(IDX::YAW) + (X_t(IDX::WZ)) * dt; // dyaw = omega - X_next_t(IDX::VX) = X_t(IDX::VX); - X_next_t(IDX::WZ) = X_t(IDX::WZ); - - // State transition matrix A - Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -X_t(IDX::VX) * sin_yaw * dt; - A(IDX::X, IDX::VX) = cos_yaw * dt; - A(IDX::Y, IDX::YAW) = X_t(IDX::VX) * cos_yaw * dt; - A(IDX::Y, IDX::VX) = sin_yaw * dt; - A(IDX::YAW, IDX::WZ) = dt; - - // Process noise covariance Q - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - // Rotate the covariance matrix according to the vehicle yaw - // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = - (ekf_params_.q_cov_x * cos_yaw * cos_yaw + ekf_params_.q_cov_y * sin_yaw * sin_yaw) * dt * dt; - Q(IDX::X, IDX::Y) = (0.5f * (ekf_params_.q_cov_x - ekf_params_.q_cov_y) * sin_2yaw) * dt * dt; - Q(IDX::Y, IDX::Y) = - (ekf_params_.q_cov_x * sin_yaw * sin_yaw + ekf_params_.q_cov_y * cos_yaw * cos_yaw) * dt * dt; - Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::YAW, IDX::YAW) = ekf_params_.q_cov_yaw * dt * dt; - Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt; - Q(IDX::WZ, IDX::WZ) = ekf_params_.q_cov_wz * dt * dt; - Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); - - if (!ekf.predict(X_next_t, A, Q)) { - RCLCPP_WARN(logger_, "Cannot predict"); - } - - return true; + return is_predicted; } bool PedestrianTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - constexpr int dim_y = 2; // pos x, pos y depending on Pose output - - // Set measurement matrix C and observation vector Y - Eigen::MatrixXd Y(dim_y, 1); - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); - Y << object.kinematics.pose_with_covariance.pose.position.x, - object.kinematics.pose_with_covariance.pose.position.y; - C(0, IDX::X) = 1.0; // for pos x - C(1, IDX::Y) = 1.0; // for pos y - // C(2, IDX::YAW) = 1.0; // for yaw - - // Set noise covariance matrix R - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - if (!object.kinematics.has_position_covariance) { - R(0, 0) = ekf_params_.r_cov_x; // x - x - R(0, 1) = 0.0; // x - y - R(1, 1) = ekf_params_.r_cov_y; // y - y - R(1, 0) = R(0, 1); // y - x - } else { - R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - } - - // ekf update - if (!ekf_.update(Y, C, R)) { - RCLCPP_WARN(logger_, "Cannot update"); - } - - // normalize yaw and limit vx, wz - { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); - ekf_.getX(X_t); - ekf_.getP(P_t); - X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { - X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; - } - if (!(-max_wz_ <= X_t(IDX::WZ) && X_t(IDX::WZ) <= max_wz_)) { - X_t(IDX::WZ) = X_t(IDX::WZ) < 0 ? -max_wz_ : max_wz_; - } - ekf_.init(X_t, P_t); - } - // position z constexpr float gain = 0.9; z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; - return true; + // motion model + bool is_updated = false; + { + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + + is_updated = + motion_model_.updateStatePose(x, y, object.kinematics.pose_with_covariance.covariance); + motion_model_.limitStates(); + } + + return is_updated; } bool PedestrianTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - constexpr float gain = 0.9; + constexpr float gain = 0.2; + constexpr float gain_inv = 1.0 - gain; + + // constexpr float gain = 0.9; if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - bounding_box_.length = gain * bounding_box_.length + (1.0 - gain) * object.shape.dimensions.x; - bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * object.shape.dimensions.y; - bounding_box_.height = gain * bounding_box_.height + (1.0 - gain) * object.shape.dimensions.z; + bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x; + bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y; + bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { - cylinder_.width = gain * cylinder_.width + (1.0 - gain) * object.shape.dimensions.x; - cylinder_.height = gain * cylinder_.height + (1.0 - gain) * object.shape.dimensions.z; + cylinder_.width = gain_inv * cylinder_.width + gain * object.shape.dimensions.x; + cylinder_.height = gain_inv * cylinder_.height + gain * object.shape.dimensions.z; } else { return false; } @@ -313,12 +212,15 @@ bool PedestrianTracker::measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { - const auto & current_classification = getClassification(); + // keep the latest input object object_ = object; + + const auto & current_classification = getClassification(); if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { setClassification(current_classification); } + // check time gap if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { RCLCPP_WARN( logger_, "There is a large gap between predicted time and measurement time. (%f)", @@ -339,70 +241,84 @@ bool PedestrianTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict state - KalmanFilter tmp_ekf_for_no_update = ekf_; - const double dt = (time - last_update_time_).seconds(); - if (0.001 /*1msec*/ < dt) { - predict(dt, tmp_ekf_for_no_update); - } - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - Eigen::MatrixXd P(ekf_params_.dim_x, ekf_params_.dim_x); // predicted state - tmp_ekf_for_no_update.getX(X_t); - tmp_ekf_for_no_update.getP(P); - - /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; + // predict from motion model + if (!motion_model_.getPredictedState( + time, pose_with_cov.pose, pose_with_cov.covariance, twist_with_cov.twist, + twist_with_cov.covariance)) { + RCLCPP_WARN(logger_, "NormalVehicleTracker::getTrackedObject: Failed to get predicted state."); + return false; + } + // position - pose_with_cov.pose.position.x = X_t(IDX::X); - pose_with_cov.pose.position.y = X_t(IDX::Y); pose_with_cov.pose.position.z = z_; - // quaternion - { - double roll, pitch, yaw; - tf2::Quaternion original_quaternion; - tf2::fromMsg(object_.kinematics.pose_with_covariance.pose.orientation, original_quaternion); - tf2::Matrix3x3(original_quaternion).getRPY(roll, pitch, yaw); - tf2::Quaternion filtered_quaternion; - filtered_quaternion.setRPY(roll, pitch, X_t(IDX::YAW)); - pose_with_cov.pose.orientation.x = filtered_quaternion.x(); - pose_with_cov.pose.orientation.y = filtered_quaternion.y(); - pose_with_cov.pose.orientation.z = filtered_quaternion.z(); - pose_with_cov.pose.orientation.w = filtered_quaternion.w(); - object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; - } - // position covariance - constexpr double z_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double r_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double p_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = r_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = p_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); - - // twist - twist_with_cov.twist.linear.x = X_t(IDX::VX); - twist_with_cov.twist.angular.z = X_t(IDX::WZ); - // twist covariance - constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::WZ); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::WZ, IDX::VX); - twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ); + + // // predict state + // KalmanFilter tmp_ekf_for_no_update = ekf_; + // const double dt = (time - last_update_time_).seconds(); + // if (0.001 /*1msec*/ < dt) { + // predict(dt, tmp_ekf_for_no_update); + // } + // Eigen::MatrixXd X_t(DIM, 1); // predicted state + // Eigen::MatrixXd P(DIM, DIM); // predicted state + // tmp_ekf_for_no_update.getX(X_t); + // tmp_ekf_for_no_update.getP(P); + + // /* put predicted pose and twist to output object */ + // auto & pose_with_cov = object.kinematics.pose_with_covariance; + // auto & twist_with_cov = object.kinematics.twist_with_covariance; + + // // position + // pose_with_cov.pose.position.x = X_t(IDX::X); + // pose_with_cov.pose.position.y = X_t(IDX::Y); + // pose_with_cov.pose.position.z = z_; + // // quaternion + // { + // double roll, pitch, yaw; + // tf2::Quaternion original_quaternion; + // tf2::fromMsg(object_.kinematics.pose_with_covariance.pose.orientation, original_quaternion); + // tf2::Matrix3x3(original_quaternion).getRPY(roll, pitch, yaw); + // tf2::Quaternion filtered_quaternion; + // filtered_quaternion.setRPY(roll, pitch, X_t(IDX::YAW)); + // pose_with_cov.pose.orientation.x = filtered_quaternion.x(); + // pose_with_cov.pose.orientation.y = filtered_quaternion.y(); + // pose_with_cov.pose.orientation.z = filtered_quaternion.z(); + // pose_with_cov.pose.orientation.w = filtered_quaternion.w(); + // object.kinematics.orientation_availability = + // autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; + // } + // // position covariance + // constexpr double z_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + // constexpr double r_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + // constexpr double p_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + // pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); + // pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); + // pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); + // pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); + // pose_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov; + // pose_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = r_cov; + // pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = p_cov; + // pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); + + // // twist + // twist_with_cov.twist.linear.x = X_t(IDX::VEL); + // twist_with_cov.twist.angular.z = X_t(IDX::WZ); + // // twist covariance + // constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + // constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + // constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + // constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + + // twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VEL, IDX::VEL); + // twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov; + // twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; + // twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VEL, IDX::WZ); + // twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::WZ, IDX::VEL); + // twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; + // twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; + // twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ); // set shape if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { From aac22fc5fc79d5d43d6fe4d4221df00fc360ee01 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 5 Mar 2024 17:35:38 +0900 Subject: [PATCH 17/42] chore: refactoring parameters Signed-off-by: Taekjin LEE --- .../src/tracker/model/bicycle_tracker.cpp | 45 ++++++----- .../src/tracker/model/big_vehicle_tracker.cpp | 45 ++++++----- .../tracker/model/normal_vehicle_tracker.cpp | 45 ++++++----- .../src/tracker/model/pedestrian_tracker.cpp | 77 ++----------------- 4 files changed, 78 insertions(+), 134 deletions(-) diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 9db8510539c21..3df5858f0fb52 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -130,25 +130,28 @@ BicycleTracker::BicycleTracker( motion_model_.init(time, X, P, bounding_box_.length); // Set motion model parameters + constexpr double q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + constexpr double q_stddev_yaw_rate_min = 5.0; // [deg/s] uncertain yaw change rate, minimum + constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate, maximum + constexpr double q_stddev_slip_rate_min = + 1.0; // [deg/s] uncertain slip angle change rate, minimum + constexpr double q_stddev_slip_rate_max = + 10.0; // [deg/s] uncertain slip angle change rate, maximum + constexpr double q_max_slip_angle = 30; // [deg] max slip angle + constexpr double lf_ratio = 0.3; // [-] ratio of front wheel position + constexpr double lf_min = 0.3; // [m] minimum front wheel position + constexpr double lr_ratio = 0.3; // [-] ratio of rear wheel position + constexpr double lr_min = 0.3; // [m] minimum rear wheel position motion_model_.setMotionParams( - 9.8 * 0.35, /* [m/(s*s)] uncertain longitudinal acceleration */ - 9.8 * 0.15, /* [m/(s*s)] uncertain lateral acceleration */ - 5.0, /* [deg/s] uncertain yaw change rate, minimum */ - 15.0, /* [deg/s] uncertain yaw change rate, maximum */ - 1.0, /* [deg/s] uncertain slip angle change rate, minimum */ - 10.0, /* [deg/s] uncertain slip angle change rate, maximum */ - 30, /* [deg] max slip angle */ - 0.3, /* [-] ratio of front wheel position*/ - 0.3, /* [m] minimum front wheel position*/ - 0.3, /* [-] ratio of rear wheel position*/ - 0.3 /* [m] minimum rear wheel position*/ - ); + q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max, + q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio, + lr_min); // Set motion limits - motion_model_.setMotionLimits( - tier4_autoware_utils::kmph2mps(100), /* [m/s] maximum velocity */ - 30 /* [deg] maximum slip angle */ - ); + constexpr double max_vel = tier4_autoware_utils::kmph2mps(80); // [m/s] maximum velocity + constexpr double max_slip = 30; // [deg] maximum slip angle + motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle } bool BicycleTracker::predict(const rclcpp::Time & time) @@ -200,11 +203,7 @@ bool BicycleTracker::measureWithPose( double measurement_yaw = 0.0; bool is_yaw_available = utils::getMeasurementYaw(object, X_t(IDX::YAW), measurement_yaw); - // position z - constexpr float gain = 0.9; - z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; - - // motion model + // update bool is_updated = false; { const double x = object.kinematics.pose_with_covariance.pose.position.x; @@ -221,6 +220,10 @@ bool BicycleTracker::measureWithPose( motion_model_.limitStates(); } + // position z + constexpr float gain = 0.9; + z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; + return is_updated; } diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index a17c3a1cc36fc..116d9472a5415 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -144,25 +144,28 @@ BigVehicleTracker::BigVehicleTracker( motion_model_.init(time, X, P, bounding_box_.length); // Set motion model parameters + constexpr double q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + constexpr double q_stddev_yaw_rate_min = 1.5; // [deg/s] uncertain yaw change rate, minimum + constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate, maximum + constexpr double q_stddev_slip_rate_min = + 0.3; // [deg/s] uncertain slip angle change rate, minimum + constexpr double q_stddev_slip_rate_max = + 10.0; // [deg/s] uncertain slip angle change rate, maximum + constexpr double q_max_slip_angle = 30; // [deg] max slip angle + constexpr double lf_ratio = 0.3; // [-] ratio of front wheel position + constexpr double lf_min = 1.5; // [m] minimum front wheel position + constexpr double lr_ratio = 0.25; // [-] ratio of rear wheel position + constexpr double lr_min = 1.5; // [m] minimum rear wheel position motion_model_.setMotionParams( - 9.8 * 0.35, /* [m/(s*s)] uncertain longitudinal acceleration */ - 9.8 * 0.15, /* [m/(s*s)] uncertain lateral acceleration */ - 1.5, /* [deg/s] uncertain yaw change rate, minimum */ - 15.0, /* [deg/s] uncertain yaw change rate, maximum */ - 0.3, /* [deg/s] uncertain slip angle change rate, minimum */ - 10.0, /* [deg/s] uncertain slip angle change rate, maximum */ - 30, /* [deg] max slip angle */ - 0.3, /* [-] ratio of front wheel position*/ - 1.5, /* [m] minimum front wheel position*/ - 0.25, /* [-] ratio of rear wheel position*/ - 1.5 /* [m] minimum rear wheel position*/ - ); + q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max, + q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio, + lr_min); // Set motion limits - motion_model_.setMotionLimits( - tier4_autoware_utils::kmph2mps(100), /* [m/s] maximum velocity */ - 30 /* [deg] maximum slip angle */ - ); + constexpr double max_vel = tier4_autoware_utils::kmph2mps(100); // [m/s] maximum velocity + constexpr double max_slip = 30; // [deg] maximum slip angle + motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle } bool BigVehicleTracker::predict(const rclcpp::Time & time) @@ -274,11 +277,7 @@ bool BigVehicleTracker::measureWithPose( } } - // position z - constexpr float gain = 0.9; - z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; - - // motion model + // update bool is_updated = false; { const double x = object.kinematics.pose_with_covariance.pose.position.x; @@ -297,6 +296,10 @@ bool BigVehicleTracker::measureWithPose( motion_model_.limitStates(); } + // position z + constexpr float gain = 0.9; + z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; + return is_updated; } diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 1fd8c2410d7fe..299637aaf3b33 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -144,25 +144,28 @@ NormalVehicleTracker::NormalVehicleTracker( motion_model_.init(time, X, P, bounding_box_.length); // Set motion model parameters + constexpr double q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + constexpr double q_stddev_yaw_rate_min = 1.5; // [deg/s] uncertain yaw change rate, minimum + constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate, maximum + constexpr double q_stddev_slip_rate_min = + 0.3; // [deg/s] uncertain slip angle change rate, minimum + constexpr double q_stddev_slip_rate_max = + 10.0; // [deg/s] uncertain slip angle change rate, maximum + constexpr double q_max_slip_angle = 30; // [deg] max slip angle + constexpr double lf_ratio = 0.3; // [-] ratio of front wheel position + constexpr double lf_min = 1.0; // [m] minimum front wheel position + constexpr double lr_ratio = 0.25; // [-] ratio of rear wheel position + constexpr double lr_min = 1.0; // [m] minimum rear wheel position motion_model_.setMotionParams( - 9.8 * 0.35, /* [m/(s*s)] uncertain longitudinal acceleration */ - 9.8 * 0.15, /* [m/(s*s)] uncertain lateral acceleration */ - 1.5, /* [deg/s] uncertain yaw change rate, minimum */ - 15.0, /* [deg/s] uncertain yaw change rate, maximum */ - 0.3, /* [deg/s] uncertain slip angle change rate, minimum */ - 10.0, /* [deg/s] uncertain slip angle change rate, maximum */ - 30, /* [deg] max slip angle */ - 0.3, /* [-] ratio of front wheel position*/ - 1.0, /* [m] minimum front wheel position*/ - 0.25, /* [-] ratio of rear wheel position*/ - 1.0 /* [m] minimum rear wheel position*/ - ); + q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max, + q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio, + lr_min); // Set motion limits - motion_model_.setMotionLimits( - tier4_autoware_utils::kmph2mps(100), /* [m/s] maximum velocity */ - 30 /* [deg] maximum slip angle */ - ); + constexpr double max_vel = tier4_autoware_utils::kmph2mps(100); // [m/s] maximum velocity + constexpr double max_slip = 30; // [deg] maximum slip angle + motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle } bool NormalVehicleTracker::predict(const rclcpp::Time & time) @@ -274,11 +277,7 @@ bool NormalVehicleTracker::measureWithPose( } } - // position z - constexpr float gain = 0.9; - z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; - - // motion model + // update bool is_updated = false; { const double x = object.kinematics.pose_with_covariance.pose.position.x; @@ -297,6 +296,10 @@ bool NormalVehicleTracker::measureWithPose( motion_model_.limitStates(); } + // position z + constexpr float gain = 0.9; + z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; + return is_updated; } diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index 0d153fad127ab..6f3d20fcba907 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -162,11 +162,7 @@ bool PedestrianTracker::predict(const rclcpp::Time & time) bool PedestrianTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - // position z - constexpr float gain = 0.9; - z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; - - // motion model + // update motion model bool is_updated = false; { const double x = object.kinematics.pose_with_covariance.pose.position.x; @@ -177,6 +173,10 @@ bool PedestrianTracker::measureWithPose( motion_model_.limitStates(); } + // position z + constexpr float gain = 0.9; + z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; + return is_updated; } @@ -248,78 +248,13 @@ bool PedestrianTracker::getTrackedObject( if (!motion_model_.getPredictedState( time, pose_with_cov.pose, pose_with_cov.covariance, twist_with_cov.twist, twist_with_cov.covariance)) { - RCLCPP_WARN(logger_, "NormalVehicleTracker::getTrackedObject: Failed to get predicted state."); + RCLCPP_WARN(logger_, "PedestrianTracker::getTrackedObject: Failed to get predicted state."); return false; } // position pose_with_cov.pose.position.z = z_; - // // predict state - // KalmanFilter tmp_ekf_for_no_update = ekf_; - // const double dt = (time - last_update_time_).seconds(); - // if (0.001 /*1msec*/ < dt) { - // predict(dt, tmp_ekf_for_no_update); - // } - // Eigen::MatrixXd X_t(DIM, 1); // predicted state - // Eigen::MatrixXd P(DIM, DIM); // predicted state - // tmp_ekf_for_no_update.getX(X_t); - // tmp_ekf_for_no_update.getP(P); - - // /* put predicted pose and twist to output object */ - // auto & pose_with_cov = object.kinematics.pose_with_covariance; - // auto & twist_with_cov = object.kinematics.twist_with_covariance; - - // // position - // pose_with_cov.pose.position.x = X_t(IDX::X); - // pose_with_cov.pose.position.y = X_t(IDX::Y); - // pose_with_cov.pose.position.z = z_; - // // quaternion - // { - // double roll, pitch, yaw; - // tf2::Quaternion original_quaternion; - // tf2::fromMsg(object_.kinematics.pose_with_covariance.pose.orientation, original_quaternion); - // tf2::Matrix3x3(original_quaternion).getRPY(roll, pitch, yaw); - // tf2::Quaternion filtered_quaternion; - // filtered_quaternion.setRPY(roll, pitch, X_t(IDX::YAW)); - // pose_with_cov.pose.orientation.x = filtered_quaternion.x(); - // pose_with_cov.pose.orientation.y = filtered_quaternion.y(); - // pose_with_cov.pose.orientation.z = filtered_quaternion.z(); - // pose_with_cov.pose.orientation.w = filtered_quaternion.w(); - // object.kinematics.orientation_availability = - // autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; - // } - // // position covariance - // constexpr double z_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - // constexpr double r_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - // constexpr double p_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - // pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); - // pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); - // pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); - // pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); - // pose_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov; - // pose_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = r_cov; - // pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = p_cov; - // pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); - - // // twist - // twist_with_cov.twist.linear.x = X_t(IDX::VEL); - // twist_with_cov.twist.angular.z = X_t(IDX::WZ); - // // twist covariance - // constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - // constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - // constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - // constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - - // twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VEL, IDX::VEL); - // twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov; - // twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - // twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VEL, IDX::WZ); - // twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::WZ, IDX::VEL); - // twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; - // twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; - // twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ); - // set shape if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { object.shape.dimensions.x = bounding_box_.length; From 0f69b52321e3dc736ca2fa83fcfaa290c8cd045d Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 5 Mar 2024 18:56:11 +0900 Subject: [PATCH 18/42] fix: object initialization Signed-off-by: Taekjin LEE --- .../motion_model/bicycle_motion_model.hpp | 8 +- .../motion_model/ctrv_motion_model.hpp | 8 +- .../src/motion_model/bicycle_motion_model.cpp | 24 +++- .../src/motion_model/ctrv_motion_model.cpp | 27 +++- .../src/tracker/model/bicycle_tracker.cpp | 110 +++++++---------- .../src/tracker/model/big_vehicle_tracker.cpp | 112 ++++++++--------- .../tracker/model/normal_vehicle_tracker.cpp | 112 ++++++++--------- .../src/tracker/model/pedestrian_tracker.cpp | 115 ++++++++---------- 8 files changed, 260 insertions(+), 256 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp index 2251a74db8a77..856e5d0194f9f 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp @@ -71,6 +71,10 @@ class BicycleMotionModel bool init( const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P, const double & length); + bool init( + const rclcpp::Time & time, const double & x, const double & y, const double & yaw, + const std::array & pose_cov, const double & vel, const double & vel_cov, + const double & slip, const double & slip_cov, const double & length); bool checkInitialized() const { @@ -109,8 +113,8 @@ class BicycleMotionModel const std::array & pose_cov); bool updateStatePoseHeadVel( - const double & x, const double & y, const double & yaw, const double & vel, - const std::array & pose_cov, const std::array & twist_cov); + const double & x, const double & y, const double & yaw, const std::array & pose_cov, + const double & vel, const std::array & twist_cov); bool adjustPosition(const double & x, const double & y); diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp index a15093795b272..a54ee8881f310 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp @@ -59,6 +59,10 @@ class CTRVMotionModel const char DIM = 5; bool init(const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P); + bool init( + const rclcpp::Time & time, const double & x, const double & y, const double & yaw, + const std::array & pose_cov, const double & vel, const double & vel_cov, + const double & wz, const double & wz_cov); bool checkInitialized() const { @@ -94,8 +98,8 @@ class CTRVMotionModel const std::array & pose_cov); bool updateStatePoseHeadVel( - const double & x, const double & y, const double & yaw, const double & vel, - const std::array & pose_cov, const std::array & twist_cov); + const double & x, const double & y, const double & yaw, const std::array & pose_cov, + const double & vel, const std::array & twist_cov); bool adjustPosition(const double & x, const double & y); diff --git a/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp index c868ec51d10be..3b7528ee337aa 100644 --- a/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp @@ -56,6 +56,26 @@ bool BicycleMotionModel::init( return true; } +bool BicycleMotionModel::init( + const rclcpp::Time & time, const double & x, const double & y, const double & yaw, + const std::array & pose_cov, const double & vel, const double & vel_cov, + const double & slip, const double & slip_cov, const double & length) +{ + // initialize state vector X + Eigen::MatrixXd X(DIM, 1); + X << x, y, yaw, vel, slip; + + // initialize covariance matrix P + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(DIM, DIM); + P(IDX::X, IDX::X) = pose_cov[utils::MSG_COV_IDX::X_X]; + P(IDX::Y, IDX::Y) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + P(IDX::YAW, IDX::YAW) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; + P(IDX::VEL, IDX::VEL) = vel_cov; + P(IDX::SLIP, IDX::SLIP) = slip_cov; + + return init(time, X, P, length); +} + void BicycleMotionModel::setDefaultParams() { // set default motion parameters @@ -189,8 +209,8 @@ bool BicycleMotionModel::updateStatePoseHead( } bool BicycleMotionModel::updateStatePoseHeadVel( - const double & x, const double & y, const double & yaw, const double & vel, - const std::array & pose_cov, const std::array & twist_cov) + const double & x, const double & y, const double & yaw, const std::array & pose_cov, + const double & vel, const std::array & twist_cov) { // check if the state is initialized if (!checkInitialized()) return false; diff --git a/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp index 151b33eb532be..6c6b26dff1592 100644 --- a/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp @@ -45,15 +45,32 @@ bool CTRVMotionModel::init( // initialize Kalman filter ekf_.init(X, P); - // // set initial extended state - // updateExtendedState(length); - // set initialized flag is_initialized_ = true; return true; } +bool CTRVMotionModel::init( + const rclcpp::Time & time, const double & x, const double & y, const double & yaw, + const std::array & pose_cov, const double & vel, const double & vel_cov, + const double & wz, const double & wz_cov) +{ + // initialize state vector X + Eigen::MatrixXd X(DIM, 1); + X << x, y, yaw, vel, wz; + + // initialize covariance matrix P + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(DIM, DIM); + P(IDX::X, IDX::X) = pose_cov[utils::MSG_COV_IDX::X_X]; + P(IDX::Y, IDX::Y) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + P(IDX::YAW, IDX::YAW) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; + P(IDX::VEL, IDX::VEL) = vel_cov; + P(IDX::WZ, IDX::WZ) = wz_cov; + + return init(time, X, P); +} + void CTRVMotionModel::setDefaultParams() { // process noise covariance @@ -165,8 +182,8 @@ bool CTRVMotionModel::updateStatePoseHead( } bool CTRVMotionModel::updateStatePoseHeadVel( - const double & x, const double & y, const double & yaw, const double & vel, - const std::array & pose_cov, const std::array & twist_cov) + const double & x, const double & y, const double & yaw, const std::array & pose_cov, + const double & vel, const std::array & twist_cov) { // check if the state is initialized if (!checkInitialized()) return false; diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 3df5858f0fb52..f020481dd6312 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -60,74 +60,58 @@ BicycleTracker::BicycleTracker( ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); - // initial state covariance - float p0_stddev_x = 0.8; // in object coordinate [m] - float p0_stddev_y = 0.5; // in object coordinate [m] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] - float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] - float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] - ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); - ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); - ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); - ekf_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0); - ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); - - // initialize state vector X - Eigen::MatrixXd X(DIM, 1); - X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; - X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; - X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - X(IDX::SLIP) = 0.0; - if (object.kinematics.has_twist) { - X(IDX::VEL) = object.kinematics.twist_with_covariance.twist.linear.x; - } else { - X(IDX::VEL) = 0.0; - } - // UNCERTAINTY MODEL - // initialize state covariance matrix P - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(DIM, DIM); - if (!object.kinematics.has_position_covariance) { - const double cos_yaw = std::cos(X(IDX::YAW)); - const double sin_yaw = std::sin(X(IDX::YAW)); - const double sin_2yaw = std::sin(2.0f * X(IDX::YAW)); - // Rotate the covariance matrix according to the vehicle yaw - // because p0_cov_x and y are in the vehicle coordinate system. - P(IDX::X, IDX::X) = - ekf_params_.p0_cov_x * cos_yaw * cos_yaw + ekf_params_.p0_cov_y * sin_yaw * sin_yaw; - P(IDX::X, IDX::Y) = 0.5f * (ekf_params_.p0_cov_x - ekf_params_.p0_cov_y) * sin_2yaw; - P(IDX::Y, IDX::Y) = - ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; - P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); - P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; - P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; - P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; - } else { - P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - P(IDX::Y, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - P(IDX::Y, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - P(IDX::YAW, IDX::YAW) = - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - if (object.kinematics.has_twist_covariance) { - P(IDX::VEL, IDX::VEL) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + // Set initial state + { + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + auto pose_cov = object.kinematics.pose_with_covariance.covariance; + double vel = 0.0; + double vel_cov; + const double & length = bounding_box_.length; + + if (object.kinematics.has_twist) { + vel = object.kinematics.twist_with_covariance.twist.linear.x; + } + + if (!object.kinematics.has_position_covariance) { + // initial state covariance + constexpr double p0_stddev_x = 0.8; // in object coordinate [m] + constexpr double p0_stddev_y = 0.5; // in object coordinate [m] + constexpr double p0_stddev_yaw = + tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + constexpr double p0_cov_x = std::pow(p0_stddev_x, 2.0); + constexpr double p0_cov_y = std::pow(p0_stddev_y, 2.0); + constexpr double p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); + + const double cos_yaw = std::cos(yaw); + const double sin_yaw = std::sin(yaw); + const double sin_2yaw = std::sin(2.0 * yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; + pose_cov[utils::MSG_COV_IDX::Y_Y] = + p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = p0_cov_yaw; + } + + if (!object.kinematics.has_twist_covariance) { + constexpr double p0_stddev_vel = + tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + vel_cov = std::pow(p0_stddev_vel, 2.0); } else { - P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; + vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } - P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; - } - // OBJECT SHAPE MODEL - if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - bounding_box_ = { - object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; - } else { - bounding_box_ = {1.0, 0.5, 1.7}; - } + const double slip = 0.0; + const double p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] + const double slip_cov = std::pow(p0_stddev_slip, 2.0); - // Set motion model - motion_model_.init(time, X, P, bounding_box_.length); + // initialize motion model + motion_model_.init(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); + } // Set motion model parameters constexpr double q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 116d9472a5415..96d7720f47983 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -63,69 +63,12 @@ BigVehicleTracker::BigVehicleTracker( ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); ekf_params_.r_cov_vel = std::pow(r_stddev_vel, 2.0); - // initial state covariance - float p0_stddev_x = 1.5; // in object coordinate [m] - float p0_stddev_y = 0.5; // in object coordinate [m] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] - float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] - float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] - ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); - ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); - ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); - ekf_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0); - ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); // velocity deviation threshold // if the predicted velocity is close to the observed velocity, // the observed velocity is used as the measurement. velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] - // initialize state vector X - Eigen::MatrixXd X(DIM, 1); - X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; - X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; - X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - X(IDX::SLIP) = 0.0; - if (object.kinematics.has_twist) { - X(IDX::VEL) = object.kinematics.twist_with_covariance.twist.linear.x; - } else { - X(IDX::VEL) = 0.0; - } - - // UNCERTAINTY MODEL - // initialize state covariance matrix P - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(DIM, DIM); - if (!object.kinematics.has_position_covariance) { - const double cos_yaw = std::cos(X(IDX::YAW)); - const double sin_yaw = std::sin(X(IDX::YAW)); - const double sin_2yaw = std::sin(2.0f * X(IDX::YAW)); - // Rotate the covariance matrix according to the vehicle yaw - // because p0_cov_x and y are in the vehicle coordinate system. - P(IDX::X, IDX::X) = - ekf_params_.p0_cov_x * cos_yaw * cos_yaw + ekf_params_.p0_cov_y * sin_yaw * sin_yaw; - P(IDX::X, IDX::Y) = 0.5f * (ekf_params_.p0_cov_x - ekf_params_.p0_cov_y) * sin_2yaw; - P(IDX::Y, IDX::Y) = - ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; - P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); - P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; - P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; - P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; - } else { - P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - P(IDX::Y, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - P(IDX::Y, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - P(IDX::YAW, IDX::YAW) = - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - if (object.kinematics.has_twist_covariance) { - P(IDX::VEL, IDX::VEL) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - } else { - P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; - } - P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; - } - // OBJECT SHAPE MODEL if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { @@ -140,8 +83,57 @@ BigVehicleTracker::BigVehicleTracker( last_input_bounding_box_ = bounding_box_; } - // Set motion model - motion_model_.init(time, X, P, bounding_box_.length); + // Set initial state + { + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + auto pose_cov = object.kinematics.pose_with_covariance.covariance; + double vel = 0.0; + double vel_cov; + const double & length = bounding_box_.length; + + if (object.kinematics.has_twist) { + vel = object.kinematics.twist_with_covariance.twist.linear.x; + } + + if (!object.kinematics.has_position_covariance) { + // initial state covariance + constexpr double p0_stddev_x = 1.5; // in object coordinate [m] + constexpr double p0_stddev_y = 0.5; // in object coordinate [m] + constexpr double p0_stddev_yaw = + tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + constexpr double p0_cov_x = std::pow(p0_stddev_x, 2.0); + constexpr double p0_cov_y = std::pow(p0_stddev_y, 2.0); + constexpr double p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); + + const double cos_yaw = std::cos(yaw); + const double sin_yaw = std::sin(yaw); + const double sin_2yaw = std::sin(2.0 * yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; + pose_cov[utils::MSG_COV_IDX::Y_Y] = + p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = p0_cov_yaw; + } + + if (!object.kinematics.has_twist_covariance) { + constexpr double p0_stddev_vel = + tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + vel_cov = std::pow(p0_stddev_vel, 2.0); + } else { + vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + } + + const double slip = 0.0; + const double p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] + const double slip_cov = std::pow(p0_stddev_slip, 2.0); + + // initialize motion model + motion_model_.init(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); + } // Set motion model parameters constexpr double q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration @@ -287,7 +279,7 @@ bool BigVehicleTracker::measureWithPose( if (is_velocity_available) { is_updated = motion_model_.updateStatePoseHeadVel( - x, y, yaw, vel, object.kinematics.pose_with_covariance.covariance, + x, y, yaw, object.kinematics.pose_with_covariance.covariance, vel, object.kinematics.twist_with_covariance.covariance); } else { is_updated = motion_model_.updateStatePoseHead( diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 299637aaf3b33..0264e33db0762 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -63,69 +63,12 @@ NormalVehicleTracker::NormalVehicleTracker( ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); ekf_params_.r_cov_vel = std::pow(r_stddev_vel, 2.0); - // initial state covariance - float p0_stddev_x = 1.0; // in object coordinate [m] - float p0_stddev_y = 0.3; // in object coordinate [m] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] - float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] - float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] - ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); - ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); - ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); - ekf_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0); - ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); // velocity deviation threshold // if the predicted velocity is close to the observed velocity, // the observed velocity is used as the measurement. velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] - // initialize state vector X - Eigen::MatrixXd X(DIM, 1); - X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; - X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; - X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - X(IDX::SLIP) = 0.0; - if (object.kinematics.has_twist) { - X(IDX::VEL) = object.kinematics.twist_with_covariance.twist.linear.x; - } else { - X(IDX::VEL) = 0.0; - } - - // UNCERTAINTY MODEL - // initialize state covariance matrix P - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(DIM, DIM); - if (!object.kinematics.has_position_covariance) { - const double cos_yaw = std::cos(X(IDX::YAW)); - const double sin_yaw = std::sin(X(IDX::YAW)); - const double sin_2yaw = std::sin(2.0f * X(IDX::YAW)); - // Rotate the covariance matrix according to the vehicle yaw - // because p0_cov_x and y are in the vehicle coordinate system. - P(IDX::X, IDX::X) = - ekf_params_.p0_cov_x * cos_yaw * cos_yaw + ekf_params_.p0_cov_y * sin_yaw * sin_yaw; - P(IDX::X, IDX::Y) = 0.5f * (ekf_params_.p0_cov_x - ekf_params_.p0_cov_y) * sin_2yaw; - P(IDX::Y, IDX::Y) = - ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; - P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); - P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; - P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; - P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; - } else { - P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - P(IDX::Y, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - P(IDX::Y, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - P(IDX::YAW, IDX::YAW) = - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - if (object.kinematics.has_twist_covariance) { - P(IDX::VEL, IDX::VEL) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - } else { - P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; - } - P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; - } - // OBJECT SHAPE MODEL if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { @@ -140,8 +83,57 @@ NormalVehicleTracker::NormalVehicleTracker( last_input_bounding_box_ = bounding_box_; } - // Set motion model - motion_model_.init(time, X, P, bounding_box_.length); + // Set initial state + { + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + auto pose_cov = object.kinematics.pose_with_covariance.covariance; + double vel = 0.0; + double vel_cov; + const double & length = bounding_box_.length; + + if (object.kinematics.has_twist) { + vel = object.kinematics.twist_with_covariance.twist.linear.x; + } + + if (!object.kinematics.has_position_covariance) { + // initial state covariance + constexpr double p0_stddev_x = 1.0; // in object coordinate [m] + constexpr double p0_stddev_y = 0.3; // in object coordinate [m] + constexpr double p0_stddev_yaw = + tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + constexpr double p0_cov_x = std::pow(p0_stddev_x, 2.0); + constexpr double p0_cov_y = std::pow(p0_stddev_y, 2.0); + constexpr double p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); + + const double cos_yaw = std::cos(yaw); + const double sin_yaw = std::sin(yaw); + const double sin_2yaw = std::sin(2.0 * yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; + pose_cov[utils::MSG_COV_IDX::Y_Y] = + p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = p0_cov_yaw; + } + + if (!object.kinematics.has_twist_covariance) { + constexpr double p0_stddev_vel = + tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + vel_cov = std::pow(p0_stddev_vel, 2.0); + } else { + vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + } + + const double slip = 0.0; + const double p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] + const double slip_cov = std::pow(p0_stddev_slip, 2.0); + + // initialize motion model + motion_model_.init(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); + } // Set motion model parameters constexpr double q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration @@ -287,7 +279,7 @@ bool NormalVehicleTracker::measureWithPose( if (is_velocity_available) { is_updated = motion_model_.updateStatePoseHeadVel( - x, y, yaw, vel, object.kinematics.pose_with_covariance.covariance, + x, y, yaw, object.kinematics.pose_with_covariance.covariance, vel, object.kinematics.twist_with_covariance.covariance); } else { is_updated = motion_model_.updateStatePoseHead( diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index 6f3d20fcba907..101da75fdac9a 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -60,66 +60,6 @@ PedestrianTracker::PedestrianTracker( ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); - // initial state covariance - float p0_stddev_x = 2.0; // [m] - float p0_stddev_y = 2.0; // [m] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(1000); // [rad] - float p0_stddev_vx = tier4_autoware_utils::kmph2mps(120); // [m/s] - float p0_stddev_wz = tier4_autoware_utils::deg2rad(360); // [rad/s] - ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); - ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); - ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); - ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); - ekf_params_.p0_cov_wz = std::pow(p0_stddev_wz, 2.0); - - // initialize state vector X - Eigen::MatrixXd X(DIM, 1); - X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; - X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; - X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - if (object.kinematics.has_twist) { - X(IDX::VEL) = object.kinematics.twist_with_covariance.twist.linear.x; - X(IDX::WZ) = object.kinematics.twist_with_covariance.twist.angular.z; - } else { - X(IDX::VEL) = 0.0; - X(IDX::WZ) = 0.0; - } - - // UNCERTAINTY MODEL - // initialize state covariance matrix P - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(DIM, DIM); - if (!object.kinematics.has_position_covariance) { - const double cos_yaw = std::cos(X(IDX::YAW)); - const double sin_yaw = std::sin(X(IDX::YAW)); - const double sin_2yaw = std::sin(2.0f * X(IDX::YAW)); - // Rotate the covariance matrix according to the vehicle yaw - // because p0_cov_x and y are in the vehicle coordinate system. - P(IDX::X, IDX::X) = - ekf_params_.p0_cov_x * cos_yaw * cos_yaw + ekf_params_.p0_cov_y * sin_yaw * sin_yaw; - P(IDX::X, IDX::Y) = 0.5f * (ekf_params_.p0_cov_x - ekf_params_.p0_cov_y) * sin_2yaw; - P(IDX::Y, IDX::Y) = - ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; - P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); - P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; - P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vx; - P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz; - } else { - P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - P(IDX::Y, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - P(IDX::Y, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - P(IDX::YAW, IDX::YAW) = - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - if (object.kinematics.has_twist_covariance) { - P(IDX::VEL, IDX::VEL) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::WZ, IDX::WZ) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - } else { - P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vx; - P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz; - } - } // OBJECT SHAPE MODEL bounding_box_ = {0.5, 0.5, 1.7}; @@ -131,8 +71,59 @@ PedestrianTracker::PedestrianTracker( cylinder_ = {object.shape.dimensions.x, object.shape.dimensions.z}; } - // Set motion model - motion_model_.init(time, X, P); + // Set initial state + { + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + auto pose_cov = object.kinematics.pose_with_covariance.covariance; + double vel = 0.0; + double wz = 0.0; + double vel_cov; + double wz_cov; + + if (object.kinematics.has_twist) { + vel = object.kinematics.twist_with_covariance.twist.linear.x; + wz = object.kinematics.twist_with_covariance.twist.angular.z; + } + + if (!object.kinematics.has_position_covariance) { + // initial state covariance + constexpr double p0_stddev_x = 2.0; // in object coordinate [m] + constexpr double p0_stddev_y = 2.0; // in object coordinate [m] + constexpr double p0_stddev_yaw = + tier4_autoware_utils::deg2rad(1000); // in map coordinate [rad] + constexpr double p0_cov_x = std::pow(p0_stddev_x, 2.0); + constexpr double p0_cov_y = std::pow(p0_stddev_y, 2.0); + constexpr double p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); + + const double cos_yaw = std::cos(yaw); + const double sin_yaw = std::sin(yaw); + const double sin_2yaw = std::sin(2.0 * yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; + pose_cov[utils::MSG_COV_IDX::Y_Y] = + p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = p0_cov_yaw; + } + + if (!object.kinematics.has_twist_covariance) { + constexpr double p0_stddev_vel = + tier4_autoware_utils::kmph2mps(120); // in object coordinate [m/s] + constexpr double p0_stddev_wz = + tier4_autoware_utils::deg2rad(360); // in object coordinate [rad/s] + vel_cov = std::pow(p0_stddev_vel, 2.0); + wz_cov = std::pow(p0_stddev_wz, 2.0); + } else { + vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + wz_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; + } + + // initialize motion model + motion_model_.init(time, x, y, yaw, pose_cov, vel, vel_cov, wz, wz_cov); + } // Set motion model parameters constexpr double q_stddev_x = 0.4; // [m/s] From 159009775277d363c75042c63cab02dcb6300fba Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 6 Mar 2024 10:01:39 +0900 Subject: [PATCH 19/42] chore: grouping param setting Signed-off-by: Taekjin LEE --- .../src/motion_model/ctrv_motion_model.cpp | 1 + .../src/tracker/model/bicycle_tracker.cpp | 45 ++++++++++--------- .../src/tracker/model/big_vehicle_tracker.cpp | 45 ++++++++++--------- .../tracker/model/normal_vehicle_tracker.cpp | 45 ++++++++++--------- .../src/tracker/model/pedestrian_tracker.cpp | 15 ++++--- 5 files changed, 85 insertions(+), 66 deletions(-) diff --git a/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp index 6c6b26dff1592..2ee4f55367119 100644 --- a/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp @@ -27,6 +27,7 @@ #include #include +// cspell: ignore CTRV // Constant Turn Rate and constant Velocity (CTRV) motion model CTRVMotionModel::CTRVMotionModel() diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index f020481dd6312..fabca3a0eb416 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -114,28 +114,33 @@ BicycleTracker::BicycleTracker( } // Set motion model parameters - constexpr double q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration - constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration - constexpr double q_stddev_yaw_rate_min = 5.0; // [deg/s] uncertain yaw change rate, minimum - constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate, maximum - constexpr double q_stddev_slip_rate_min = - 1.0; // [deg/s] uncertain slip angle change rate, minimum - constexpr double q_stddev_slip_rate_max = - 10.0; // [deg/s] uncertain slip angle change rate, maximum - constexpr double q_max_slip_angle = 30; // [deg] max slip angle - constexpr double lf_ratio = 0.3; // [-] ratio of front wheel position - constexpr double lf_min = 0.3; // [m] minimum front wheel position - constexpr double lr_ratio = 0.3; // [-] ratio of rear wheel position - constexpr double lr_min = 0.3; // [m] minimum rear wheel position - motion_model_.setMotionParams( - q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max, - q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio, - lr_min); + { + constexpr double q_stddev_acc_long = + 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + constexpr double q_stddev_yaw_rate_min = 5.0; // [deg/s] uncertain yaw change rate, minimum + constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate, maximum + constexpr double q_stddev_slip_rate_min = + 1.0; // [deg/s] uncertain slip angle change rate, minimum + constexpr double q_stddev_slip_rate_max = + 10.0; // [deg/s] uncertain slip angle change rate, maximum + constexpr double q_max_slip_angle = 30; // [deg] max slip angle + constexpr double lf_ratio = 0.3; // [-] ratio of front wheel position + constexpr double lf_min = 0.3; // [m] minimum front wheel position + constexpr double lr_ratio = 0.3; // [-] ratio of rear wheel position + constexpr double lr_min = 0.3; // [m] minimum rear wheel position + motion_model_.setMotionParams( + q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max, + q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio, + lr_min); + } // Set motion limits - constexpr double max_vel = tier4_autoware_utils::kmph2mps(80); // [m/s] maximum velocity - constexpr double max_slip = 30; // [deg] maximum slip angle - motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle + { + constexpr double max_vel = tier4_autoware_utils::kmph2mps(80); // [m/s] maximum velocity + constexpr double max_slip = 30; // [deg] maximum slip angle + motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle + } } bool BicycleTracker::predict(const rclcpp::Time & time) diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 96d7720f47983..055654d5fc26c 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -136,28 +136,33 @@ BigVehicleTracker::BigVehicleTracker( } // Set motion model parameters - constexpr double q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration - constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration - constexpr double q_stddev_yaw_rate_min = 1.5; // [deg/s] uncertain yaw change rate, minimum - constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate, maximum - constexpr double q_stddev_slip_rate_min = - 0.3; // [deg/s] uncertain slip angle change rate, minimum - constexpr double q_stddev_slip_rate_max = - 10.0; // [deg/s] uncertain slip angle change rate, maximum - constexpr double q_max_slip_angle = 30; // [deg] max slip angle - constexpr double lf_ratio = 0.3; // [-] ratio of front wheel position - constexpr double lf_min = 1.5; // [m] minimum front wheel position - constexpr double lr_ratio = 0.25; // [-] ratio of rear wheel position - constexpr double lr_min = 1.5; // [m] minimum rear wheel position - motion_model_.setMotionParams( - q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max, - q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio, - lr_min); + { + constexpr double q_stddev_acc_long = + 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + constexpr double q_stddev_yaw_rate_min = 1.5; // [deg/s] uncertain yaw change rate, minimum + constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate, maximum + constexpr double q_stddev_slip_rate_min = + 0.3; // [deg/s] uncertain slip angle change rate, minimum + constexpr double q_stddev_slip_rate_max = + 10.0; // [deg/s] uncertain slip angle change rate, maximum + constexpr double q_max_slip_angle = 30; // [deg] max slip angle + constexpr double lf_ratio = 0.3; // [-] ratio of front wheel position + constexpr double lf_min = 1.5; // [m] minimum front wheel position + constexpr double lr_ratio = 0.25; // [-] ratio of rear wheel position + constexpr double lr_min = 1.5; // [m] minimum rear wheel position + motion_model_.setMotionParams( + q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max, + q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio, + lr_min); + } // Set motion limits - constexpr double max_vel = tier4_autoware_utils::kmph2mps(100); // [m/s] maximum velocity - constexpr double max_slip = 30; // [deg] maximum slip angle - motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle + { + constexpr double max_vel = tier4_autoware_utils::kmph2mps(100); // [m/s] maximum velocity + constexpr double max_slip = 30; // [deg] maximum slip angle + motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle + } } bool BigVehicleTracker::predict(const rclcpp::Time & time) diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 0264e33db0762..bfe08d4491eee 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -136,28 +136,33 @@ NormalVehicleTracker::NormalVehicleTracker( } // Set motion model parameters - constexpr double q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration - constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration - constexpr double q_stddev_yaw_rate_min = 1.5; // [deg/s] uncertain yaw change rate, minimum - constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate, maximum - constexpr double q_stddev_slip_rate_min = - 0.3; // [deg/s] uncertain slip angle change rate, minimum - constexpr double q_stddev_slip_rate_max = - 10.0; // [deg/s] uncertain slip angle change rate, maximum - constexpr double q_max_slip_angle = 30; // [deg] max slip angle - constexpr double lf_ratio = 0.3; // [-] ratio of front wheel position - constexpr double lf_min = 1.0; // [m] minimum front wheel position - constexpr double lr_ratio = 0.25; // [-] ratio of rear wheel position - constexpr double lr_min = 1.0; // [m] minimum rear wheel position - motion_model_.setMotionParams( - q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max, - q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio, - lr_min); + { + constexpr double q_stddev_acc_long = + 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + constexpr double q_stddev_yaw_rate_min = 1.5; // [deg/s] uncertain yaw change rate, minimum + constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate, maximum + constexpr double q_stddev_slip_rate_min = + 0.3; // [deg/s] uncertain slip angle change rate, minimum + constexpr double q_stddev_slip_rate_max = + 10.0; // [deg/s] uncertain slip angle change rate, maximum + constexpr double q_max_slip_angle = 30; // [deg] max slip angle + constexpr double lf_ratio = 0.3; // [-] ratio of front wheel position + constexpr double lf_min = 1.0; // [m] minimum front wheel position + constexpr double lr_ratio = 0.25; // [-] ratio of rear wheel position + constexpr double lr_min = 1.0; // [m] minimum rear wheel position + motion_model_.setMotionParams( + q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max, + q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio, + lr_min); + } // Set motion limits - constexpr double max_vel = tier4_autoware_utils::kmph2mps(100); // [m/s] maximum velocity - constexpr double max_slip = 30; // [deg] maximum slip angle - motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle + { + constexpr double max_vel = tier4_autoware_utils::kmph2mps(100); // [m/s] maximum velocity + constexpr double max_slip = 30; // [deg] maximum slip angle + motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle + } } bool NormalVehicleTracker::predict(const rclcpp::Time & time) diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index 101da75fdac9a..af09cf0b38a8b 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -126,12 +126,14 @@ PedestrianTracker::PedestrianTracker( } // Set motion model parameters - constexpr double q_stddev_x = 0.4; // [m/s] - constexpr double q_stddev_y = 0.4; // [m/s] - constexpr double q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] - constexpr double q_stddev_vx = tier4_autoware_utils::kmph2mps(5); // [m/(s*s)] - constexpr double q_stddev_wz = tier4_autoware_utils::deg2rad(30); // [rad/(s*s)] - motion_model_.setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vx, q_stddev_wz); + { + constexpr double q_stddev_x = 0.4; // [m/s] + constexpr double q_stddev_y = 0.4; // [m/s] + constexpr double q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] + constexpr double q_stddev_vx = tier4_autoware_utils::kmph2mps(5); // [m/(s*s)] + constexpr double q_stddev_wz = tier4_autoware_utils::deg2rad(30); // [rad/(s*s)] + motion_model_.setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vx, q_stddev_wz); + } // Set motion limits motion_model_.setMotionLimits( @@ -218,6 +220,7 @@ bool PedestrianTracker::measure( (time - last_update_time_).seconds()); } + // update object measureWithPose(object); measureWithShape(object); From 7cf087359fe9fdeffa9ceb60ac1a897d501d3e8c Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 6 Mar 2024 12:31:37 +0900 Subject: [PATCH 20/42] feat: create cv motion model, not tested Signed-off-by: Taekjin LEE --- .../multi_object_tracker/CMakeLists.txt | 1 + .../motion_model/cv_motion_model.hpp | 113 ++++++ .../src/motion_model/cv_motion_model.cpp | 360 ++++++++++++++++++ .../src/tracker/model/unknown_tracker.cpp | 7 +- 4 files changed, 479 insertions(+), 2 deletions(-) create mode 100644 perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp create mode 100644 perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt index 605ab4ccfff1d..911d0ceab0517 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/multi_object_tracker/CMakeLists.txt @@ -35,6 +35,7 @@ set(MULTI_OBJECT_TRACKER_SRC src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp src/motion_model/bicycle_motion_model.cpp src/motion_model/ctrv_motion_model.cpp + src/motion_model/cv_motion_model.cpp ) ament_auto_add_library(multi_object_tracker_node SHARED diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp new file mode 100644 index 0000000000000..5253ad3f45a3e --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp @@ -0,0 +1,113 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Taekjin Lee +// + +#ifndef MULTI_OBJECT_TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ +#define MULTI_OBJECT_TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ + +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include +#include + +#include "autoware_auto_perception_msgs/msg/detected_object.hpp" + +class CVMotionModel +{ +private: + // attributes + rclcpp::Logger logger_; + rclcpp::Time last_update_time_; + +private: + // state + bool is_initialized_{false}; + KalmanFilter ekf_; + + // motion parameters + struct MotionParams + { + double q_cov_x; + double q_cov_y; + double q_cov_vx; + double q_cov_vy; + double max_vx; + double max_vy; + } motion_params_; + +public: + CVMotionModel(); + + enum IDX { X = 0, Y = 1, VX = 2, VY = 3 }; + const char DIM = 4; + + bool init(const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P); + bool init( + const rclcpp::Time & time, const double & x, const double & y, + const std::array & pose_cov, const double & vx, const double & vy, + const std::array & twist_cov); + + bool checkInitialized() const + { + // if the state is not initialized, return false + if (!is_initialized_) { + RCLCPP_WARN(logger_, "CVMotionModel is not initialized."); + return false; + } + return true; + } + + void setDefaultParams(); + + void setMotionParams( + const double & q_stddev_x, const double & q_stddev_y, const double & q_stddev_vx, + const double & q_stddev_vy); + + void setMotionLimits(const double & max_vx, const double & max_vy); + + Eigen::MatrixXd getStateVector() const + { + Eigen::MatrixXd X_t(DIM, 1); + ekf_.getX(X_t); + return X_t; + } + + double getStateElement(unsigned int idx) const { return ekf_.getXelement(idx); } + + bool updateStatePose(const double & x, const double & y, const std::array & pose_cov); + + bool updateStatePoseVel( + const double & x, const double & y, const std::array & pose_cov, const double & vx, + const double & vy, const std::array & twist_cov); + + bool adjustPosition(const double & x, const double & y); + + bool limitStates(); + + bool predictState(const rclcpp::Time & time); + + bool predictState(const double dt, KalmanFilter & ekf) const; + + bool getPredictedState(const rclcpp::Time & time, Eigen::MatrixXd & X, Eigen::MatrixXd & P) const; + + bool getPredictedState( + const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, + geometry_msgs::msg::Twist & twist, std::array & twist_cov) const; +}; + +#endif // MULTI_OBJECT_TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp new file mode 100644 index 0000000000000..637b983ee724a --- /dev/null +++ b/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp @@ -0,0 +1,360 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Taekjin Lee +// + +#include "multi_object_tracker/motion_model/cv_motion_model.hpp" + +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +// cspell: ignore CV +// Constant Velocity (CV) motion model + +CVMotionModel::CVMotionModel() +: logger_(rclcpp::get_logger("CVMotionModel")), last_update_time_(rclcpp::Time(0, 0)) +{ + // Initialize motion parameters + setDefaultParams(); +} + +bool CVMotionModel::init( + const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P) +{ + // set last update time + last_update_time_ = time; + + // initialize Kalman filter + ekf_.init(X, P); + + // set initialized flag + is_initialized_ = true; + + return true; +} + +bool CVMotionModel::init( + const rclcpp::Time & time, const double & x, const double & y, + const std::array & pose_cov, const double & vx, const double & vy, + const std::array & twist_cov) +{ + // initialize state vector X + Eigen::MatrixXd X(DIM, 1); + X << x, y, vx, vy; + + // initialize covariance matrix P + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(DIM, DIM); + P(IDX::X, IDX::X) = pose_cov[utils::MSG_COV_IDX::X_X]; + P(IDX::Y, IDX::Y) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + P(IDX::VX, IDX::VX) = twist_cov[utils::MSG_COV_IDX::X_X]; + P(IDX::VY, IDX::VY) = twist_cov[utils::MSG_COV_IDX::Y_Y]; + + return init(time, X, P); +} + +void CVMotionModel::setDefaultParams() +{ + // process noise covariance + constexpr double q_stddev_x = 0.5; // [m/s] + constexpr double q_stddev_y = 0.5; // [m/s] + constexpr double q_stddev_vx = 9.8 * 0.3; // [m/(s*s)] + constexpr double q_stddev_vy = 9.8 * 0.3; // [m/(s*s)] + setMotionParams(q_stddev_x, q_stddev_y, q_stddev_vx, q_stddev_vy); + + // set motion limitations + constexpr double max_vx = tier4_autoware_utils::kmph2mps(60); // [m/s] + constexpr double max_vy = tier4_autoware_utils::kmph2mps(60); // [m/s] + setMotionLimits(max_vx, max_vy); +} + +void CVMotionModel::setMotionParams( + const double & q_stddev_x, const double & q_stddev_y, const double & q_stddev_vx, + const double & q_stddev_vy) +{ + // set process noise covariance parameters + motion_params_.q_cov_x = std::pow(q_stddev_x, 2.0); + motion_params_.q_cov_y = std::pow(q_stddev_y, 2.0); + motion_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); + motion_params_.q_cov_vy = std::pow(q_stddev_vy, 2.0); +} + +void CVMotionModel::setMotionLimits(const double & max_vx, const double & max_vy) +{ + // set motion limitations + motion_params_.max_vx = max_vx; + motion_params_.max_vy = max_vy; +} + +bool CVMotionModel::updateStatePose( + const double & x, const double & y, const std::array & pose_cov) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // update state, without velocity + constexpr int DIM_Y = 2; + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + + return ekf_.update(Y, C, R); +} + +bool CVMotionModel::updateStatePoseVel( + const double & x, const double & y, const std::array & pose_cov, const double & vx, + const double & vy, const std::array & twist_cov) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // update state with velocity + constexpr int DIM_Y = 4; + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y, vx, vy; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + C(2, IDX::VX) = 1.0; + C(3, IDX::VY) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + R(2, 2) = twist_cov[utils::MSG_COV_IDX::X_X]; + R(2, 3) = twist_cov[utils::MSG_COV_IDX::X_Y]; + R(3, 2) = twist_cov[utils::MSG_COV_IDX::Y_X]; + R(3, 3) = twist_cov[utils::MSG_COV_IDX::Y_Y]; + + return ekf_.update(Y, C, R); +} + +bool CVMotionModel::limitStates() +{ + Eigen::MatrixXd X_t(DIM, 1); + Eigen::MatrixXd P_t(DIM, DIM); + ekf_.getX(X_t); + ekf_.getP(P_t); + if (!(-motion_params_.max_vx <= X_t(IDX::VX) && X_t(IDX::VX) <= motion_params_.max_vx)) { + X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -motion_params_.max_vx : motion_params_.max_vx; + } + if (!(-motion_params_.max_vy <= X_t(IDX::VY) && X_t(IDX::VY) <= motion_params_.max_vy)) { + X_t(IDX::VY) = X_t(IDX::VY) < 0 ? -motion_params_.max_vy : motion_params_.max_vy; + } + ekf_.init(X_t, P_t); + + return true; +} + +bool CVMotionModel::adjustPosition(const double & x, const double & y) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // adjust position + Eigen::MatrixXd X_t(DIM, 1); + Eigen::MatrixXd P_t(DIM, DIM); + ekf_.getX(X_t); + ekf_.getP(P_t); + X_t(IDX::X) += x; + X_t(IDX::Y) += y; + ekf_.init(X_t, P_t); + + return true; +} + +bool CVMotionModel::predictState(const rclcpp::Time & time) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + const double dt = (time - last_update_time_).seconds(); + if (dt < 0.0) { + RCLCPP_WARN(logger_, "CVMotionModel::predictState: dt is negative. (%f)", dt); + return false; + } + if (!predictState(dt, ekf_)) { + return false; + } + last_update_time_ = time; + return true; +} + +bool CVMotionModel::predictState(const double dt, KalmanFilter & ekf) const +{ + /* Motion model: Constant velocity model + * + * x_{k+1} = x_k + vx_k * dt + * y_{k+1} = y_k + vx_k * dt + * vx_{k+1} = vx_k + * vy_{k+1} = vy_k + * + */ + + /* Jacobian Matrix + * + * A = [ 1, 0, dt, 0] + * [ 0, 1, 0, dt] + * [ 0, 0, 1, 0] + * [ 0, 0, 0, 1] + */ + + // MOTION MODEL (predict) + + // Current state vector X t + Eigen::MatrixXd X_t = getStateVector(); + + // Predict state vector X t+1 + Eigen::MatrixXd X_next_t(DIM, 1); // predicted state + X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VX) * dt; + X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VY) * dt; + X_next_t(IDX::VX) = X_t(IDX::VX); + X_next_t(IDX::VY) = X_t(IDX::VY); + + // State transition matrix A + Eigen::MatrixXd A = Eigen::MatrixXd::Identity(DIM, DIM); + A(IDX::X, IDX::VX) = dt; + A(IDX::Y, IDX::VY) = dt; + + // Process noise covariance Q + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(DIM, DIM); + Q(IDX::X, IDX::X) = motion_params_.q_cov_x * dt * dt; + Q(IDX::X, IDX::Y) = 0.0; + Q(IDX::Y, IDX::Y) = motion_params_.q_cov_y * dt * dt; + Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); + Q(IDX::VX, IDX::VX) = motion_params_.q_cov_vx * dt * dt; + Q(IDX::VY, IDX::VY) = motion_params_.q_cov_vy * dt * dt; + + // control-input model B and control-input u are not used + // Eigen::MatrixXd B = Eigen::MatrixXd::Zero(DIM, DIM); + // Eigen::MatrixXd u = Eigen::MatrixXd::Zero(DIM, 1); + + // predict state + return ekf.predict(X_next_t, A, Q); +} + +bool CVMotionModel::getPredictedState( + const rclcpp::Time & time, Eigen::MatrixXd & X, Eigen::MatrixXd & P) const +{ + // 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 = (time - last_update_time_).seconds(); + if (dt < 0.0) { + RCLCPP_WARN(logger_, "CVMotionModel::getPredictedState: dt is negative. (%f)", dt); + dt = 0.0; + } + + // predict only when dt is small enough + if (0.001 /*1msec*/ < dt) { + if (!predictState(dt, tmp_ekf_for_no_update)) { + return false; + } + } + tmp_ekf_for_no_update.getX(X); + tmp_ekf_for_no_update.getP(P); + return true; +} + +bool CVMotionModel::getPredictedState( + const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, + geometry_msgs::msg::Twist & twist, std::array & twist_cov) const +{ + // get predicted state + Eigen::MatrixXd X(DIM, 1); + Eigen::MatrixXd P(DIM, DIM); + if (!getPredictedState(time, X, P)) { + return false; + } + + // get yaw from pose + double yaw = tf2::getYaw(pose.orientation); + + // set position + pose.position.x = X(IDX::X); + pose.position.y = X(IDX::Y); + pose.position.z = 0.0; + + // set twist + twist.linear.x = X(IDX::VX) * std::cos(-yaw) - X(IDX::VY) * std::sin(-yaw); + twist.linear.y = X(IDX::VX) * std::sin(-yaw) + X(IDX::VY) * std::cos(-yaw); + twist.linear.z = 0.0; + twist.angular.x = 0.0; + twist.angular.y = 0.0; + twist.angular.z = 0.0; + + // set pose covariance + constexpr double zz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double rr_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double pp_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double yaw_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + pose_cov[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); + pose_cov[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); + pose_cov[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); + pose_cov[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); + pose_cov[utils::MSG_COV_IDX::Z_Z] = zz_cov; + pose_cov[utils::MSG_COV_IDX::ROLL_ROLL] = rr_cov; + pose_cov[utils::MSG_COV_IDX::PITCH_PITCH] = pp_cov; + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = yaw_cov; + + // set twist covariance + constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double wz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + // rotate covariance matrix + Eigen::MatrixXd twist_cov_rotate(2, 2); + twist_cov_rotate(0, 0) = P(IDX::VX, IDX::VX); + twist_cov_rotate(0, 1) = P(IDX::VX, IDX::VY); + twist_cov_rotate(1, 0) = P(IDX::VY, IDX::VX); + twist_cov_rotate(1, 1) = P(IDX::VY, IDX::VY); + Eigen::MatrixXd R_yaw = Eigen::Rotation2Dd(yaw).toRotationMatrix(); + twist_cov_rotate = R_yaw * twist_cov_rotate * R_yaw.transpose(); + twist_cov[utils::MSG_COV_IDX::X_X] = twist_cov_rotate(0, 0); + twist_cov[utils::MSG_COV_IDX::X_Y] = twist_cov_rotate(0, 1); + twist_cov[utils::MSG_COV_IDX::Y_X] = twist_cov_rotate(1, 0); + twist_cov[utils::MSG_COV_IDX::Y_Y] = twist_cov_rotate(1, 1); + twist_cov[utils::MSG_COV_IDX::Z_Z] = vz_cov; + twist_cov[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; + twist_cov[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; + twist_cov[utils::MSG_COV_IDX::YAW_YAW] = wz_cov; + + return true; +} diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp index 77cf51d3d505f..9bf63001a8836 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -73,8 +73,11 @@ UnknownTracker::UnknownTracker( X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; if (object.kinematics.has_twist) { - X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x; - X(IDX::VY) = object.kinematics.twist_with_covariance.twist.linear.y; + const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double & vx = object.kinematics.twist_with_covariance.twist.linear.x; + const double & vy = object.kinematics.twist_with_covariance.twist.linear.y; + X(IDX::VX) = std::cos(yaw) * vx - std::sin(yaw) * vy; + X(IDX::VY) = std::sin(yaw) * vx + std::cos(yaw) * vy; } else { X(IDX::VX) = 0.0; X(IDX::VY) = 0.0; From 05adc6e9220e412f1a613b9ffb7682572ad1989d Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 6 Mar 2024 14:17:12 +0900 Subject: [PATCH 21/42] feat: implementing unknown tracker, process is dying Signed-off-by: Taekjin LEE --- .../tracker/model/pedestrian_tracker.hpp | 8 +- .../tracker/model/unknown_tracker.hpp | 32 +- .../src/motion_model/cv_motion_model.cpp | 19 +- .../src/tracker/model/pedestrian_tracker.cpp | 35 +- .../src/tracker/model/unknown_tracker.cpp | 342 +++++++----------- 5 files changed, 192 insertions(+), 244 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index 0de31edec660f..cbce51015b743 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -34,11 +34,6 @@ class PedestrianTracker : public Tracker rclcpp::Time last_update_time_; struct EkfParams { - float p0_cov_vx; - float p0_cov_wz; - float p0_cov_x; - float p0_cov_y; - float p0_cov_yaw; float r_cov_x; float r_cov_y; float r_cov_yaw; @@ -74,6 +69,9 @@ class PedestrianTracker : public Tracker bool measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; + autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp index 18e6c0606a535..436adf664fb86 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -19,8 +19,8 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ -#include "tracker_base.hpp" - +#include "multi_object_tracker/motion_model/cv_motion_model.hpp" +#include "multi_object_tracker/tracker/model/tracker_base.hpp" #include class UnknownTracker : public Tracker @@ -30,41 +30,33 @@ class UnknownTracker : public Tracker rclcpp::Logger logger_; private: - KalmanFilter ekf_; rclcpp::Time last_update_time_; - enum IDX { - X = 0, - Y = 1, - VX = 2, - VY = 3, - }; struct EkfParams { - char dim_x = 4; - float q_cov_x; - float q_cov_y; - float q_cov_vx; - float q_cov_vy; - float p0_cov_vx; - float p0_cov_vy; float r_cov_x; float r_cov_y; - float p0_cov_x; - float p0_cov_y; + float r_cov_vx; + float r_cov_vy; } ekf_params_; - float max_vx_, max_vy_; float z_; +private: + CVMotionModel motion_model_; + const char DIM = motion_model_.DIM; + using IDX = CVMotionModel::IDX; + public: UnknownTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & self_transform); bool predict(const rclcpp::Time & time) override; - bool predict(const double dt, KalmanFilter & ekf) const; bool measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; + autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( diff --git a/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp index 637b983ee724a..a627eb55d8bc0 100644 --- a/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp @@ -27,6 +27,8 @@ #include #include +#include + // cspell: ignore CV // Constant Velocity (CV) motion model @@ -255,7 +257,7 @@ bool CVMotionModel::predictState(const double dt, KalmanFilter & ekf) const Q(IDX::X, IDX::X) = motion_params_.q_cov_x * dt * dt; Q(IDX::X, IDX::Y) = 0.0; Q(IDX::Y, IDX::Y) = motion_params_.q_cov_y * dt * dt; - Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); + Q(IDX::Y, IDX::X) = 0.0; Q(IDX::VX, IDX::VX) = motion_params_.q_cov_vx * dt * dt; Q(IDX::VY, IDX::VY) = motion_params_.q_cov_vy * dt * dt; @@ -305,7 +307,8 @@ bool CVMotionModel::getPredictedState( } // get yaw from pose - double yaw = tf2::getYaw(pose.orientation); + // const double yaw = tf2::getYaw(pose.orientation); + const double yaw = 0.0; // set position pose.position.x = X(IDX::X); @@ -345,12 +348,12 @@ bool CVMotionModel::getPredictedState( twist_cov_rotate(0, 1) = P(IDX::VX, IDX::VY); twist_cov_rotate(1, 0) = P(IDX::VY, IDX::VX); twist_cov_rotate(1, 1) = P(IDX::VY, IDX::VY); - Eigen::MatrixXd R_yaw = Eigen::Rotation2Dd(yaw).toRotationMatrix(); - twist_cov_rotate = R_yaw * twist_cov_rotate * R_yaw.transpose(); - twist_cov[utils::MSG_COV_IDX::X_X] = twist_cov_rotate(0, 0); - twist_cov[utils::MSG_COV_IDX::X_Y] = twist_cov_rotate(0, 1); - twist_cov[utils::MSG_COV_IDX::Y_X] = twist_cov_rotate(1, 0); - twist_cov[utils::MSG_COV_IDX::Y_Y] = twist_cov_rotate(1, 1); + Eigen::MatrixXd R_yaw = Eigen::Rotation2Dd(-yaw).toRotationMatrix(); + Eigen::MatrixXd twist_cov_rotated = R_yaw * twist_cov_rotate * R_yaw.transpose(); + twist_cov[utils::MSG_COV_IDX::X_X] = twist_cov_rotated(0, 0); + twist_cov[utils::MSG_COV_IDX::X_Y] = twist_cov_rotated(0, 1); + twist_cov[utils::MSG_COV_IDX::Y_X] = twist_cov_rotated(1, 0); + twist_cov[utils::MSG_COV_IDX::Y_Y] = twist_cov_rotated(1, 1); twist_cov[utils::MSG_COV_IDX::Z_Z] = vz_cov; twist_cov[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; twist_cov[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index af09cf0b38a8b..92ad2aae757a3 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -152,6 +152,32 @@ bool PedestrianTracker::predict(const rclcpp::Time & time) return is_predicted; } + +autoware_auto_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & /*self_transform*/) +{ + autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; + + // UNCERTAINTY MODEL + if (!object.kinematics.has_position_covariance) { + float & r_cov_x= ekf_params_.r_cov_x; + float & r_cov_y= ekf_params_.r_cov_y; + auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; + const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double cos_yaw = std::cos(pose_yaw); + const double sin_yaw = std::sin(pose_yaw); + const double sin_2yaw = std::sin(2.0f * pose_yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + pose_cov[utils::MSG_COV_IDX::Y_Y] = + r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; // y - x + } + return updating_object; +} + bool PedestrianTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { @@ -216,13 +242,16 @@ bool PedestrianTracker::measure( // check time gap if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { RCLCPP_WARN( - logger_, "There is a large gap between predicted time and measurement time. (%f)", + logger_, "PedestrianTracker: There is a large gap between predicted time and measurement time. (%f)", (time - last_update_time_).seconds()); } // update object - measureWithPose(object); - measureWithShape(object); + const autoware_auto_perception_msgs::msg::DetectedObject updating_object = + getUpdatingObject(object, self_transform); + measureWithPose(updating_object); + measureWithShape(updating_object); + (void)self_transform; // currently do not use self vehicle position return true; diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp index 9bf63001a8836..dba8abe3aacf5 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -12,6 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "multi_object_tracker/tracker/model/unknown_tracker.hpp" + +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include +#include + #include #include #include @@ -22,17 +30,11 @@ #else #include #endif - -#define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/unknown_tracker.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" +#define EIGEN_MPL2_ONLY #include #include -#include -#include -#include UnknownTracker::UnknownTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, @@ -45,207 +47,166 @@ UnknownTracker::UnknownTracker( object_ = object; // initialize params - float q_stddev_x = 0.0; // [m/s] - float q_stddev_y = 0.0; // [m/s] - float q_stddev_vx = tier4_autoware_utils::kmph2mps(20); // [m/(s*s)] - float q_stddev_vy = tier4_autoware_utils::kmph2mps(20); // [m/(s*s)] + // measurement noise covariance float r_stddev_x = 1.0; // [m] float r_stddev_y = 1.0; // [m] - float p0_stddev_x = 1.0; // [m/s] - float p0_stddev_y = 1.0; // [m/s] - float p0_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)] - float p0_stddev_vy = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)] - ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); - ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); - ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); - ekf_params_.q_cov_vy = std::pow(q_stddev_vy, 2.0); ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); - ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); - ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); - ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); - ekf_params_.p0_cov_vy = std::pow(p0_stddev_vy, 2.0); - max_vx_ = tier4_autoware_utils::kmph2mps(60); // [m/s] - max_vy_ = tier4_autoware_utils::kmph2mps(60); // [m/s] - - // initialize X matrix - Eigen::MatrixXd X(ekf_params_.dim_x, 1); - X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; - X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; - if (object.kinematics.has_twist) { + + // Set initial state + { + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + auto pose_cov = object.kinematics.pose_with_covariance.covariance; + auto twist_cov = object.kinematics.twist_with_covariance.covariance; const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - const double & vx = object.kinematics.twist_with_covariance.twist.linear.x; - const double & vy = object.kinematics.twist_with_covariance.twist.linear.y; - X(IDX::VX) = std::cos(yaw) * vx - std::sin(yaw) * vy; - X(IDX::VY) = std::sin(yaw) * vx + std::cos(yaw) * vy; - } else { - X(IDX::VX) = 0.0; - X(IDX::VY) = 0.0; - } - // initialize P matrix - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - if (!object.kinematics.has_position_covariance) { - // Rotate the covariance matrix according to the vehicle yaw - // because p0_cov_x and y are in the vehicle coordinate system. - P(IDX::X, IDX::X) = ekf_params_.p0_cov_x; - P(IDX::X, IDX::Y) = 0.0; - P(IDX::Y, IDX::Y) = ekf_params_.p0_cov_y; - P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; - P(IDX::VY, IDX::VY) = ekf_params_.p0_cov_vy; - } else { - P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - P(IDX::Y, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - P(IDX::Y, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - if (object.kinematics.has_twist_covariance) { - P(IDX::VX, IDX::VX) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::VY, IDX::VY) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - } else { - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; - P(IDX::VY, IDX::VY) = ekf_params_.p0_cov_vy; + double vx = 0.0; + double vy = 0.0; + if (object.kinematics.has_twist) { + const double & vel_x = object.kinematics.twist_with_covariance.twist.linear.x; + const double & vel_y = object.kinematics.twist_with_covariance.twist.linear.y; + vx = std::cos(yaw) * vel_x - std::sin(yaw) * vel_y; + vy = std::sin(yaw) * vel_x + std::cos(yaw) * vel_y; + } + + if(!object.kinematics.has_position_covariance){ + constexpr double p0_stddev_x = 1.0; // [m] + constexpr double p0_stddev_y = 1.0; // [m] + + const double p0_cov_x = std::pow(p0_stddev_x, 2.0); + const double p0_cov_y = std::pow(p0_stddev_y, 2.0); + + const double cos_yaw = std::cos(yaw); + const double sin_yaw = std::sin(yaw); + const double sin_2yaw = std::sin(2.0 * yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; + pose_cov[utils::MSG_COV_IDX::Y_Y] = + p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; } + + if (!object.kinematics.has_twist_covariance) { + constexpr double p0_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/s] + constexpr double p0_stddev_vy = tier4_autoware_utils::kmph2mps(10); // [m/s] + const double p0_cov_vx = std::pow(p0_stddev_vx, 2.0); + const double p0_cov_vy = std::pow(p0_stddev_vy, 2.0); + twist_cov[utils::MSG_COV_IDX::X_X] = p0_cov_vx; + twist_cov[utils::MSG_COV_IDX::X_Y] = 0.0; + twist_cov[utils::MSG_COV_IDX::Y_X] = 0.0; + twist_cov[utils::MSG_COV_IDX::Y_Y] = p0_cov_vy; + } + + // rotate twist covariance matrix, since it is in the vehicle coordinate system + Eigen::MatrixXd twist_cov_rotate(2, 2); + twist_cov_rotate(0, 0) = twist_cov[utils::MSG_COV_IDX::X_X]; + twist_cov_rotate(0, 1) = twist_cov[utils::MSG_COV_IDX::X_Y]; + twist_cov_rotate(1, 0) = twist_cov[utils::MSG_COV_IDX::Y_X]; + twist_cov_rotate(1, 1) = twist_cov[utils::MSG_COV_IDX::Y_Y]; + Eigen::MatrixXd R_yaw = Eigen::Rotation2Dd(-yaw).toRotationMatrix(); + Eigen::MatrixXd twist_cov_rotated = R_yaw * twist_cov_rotate * R_yaw.transpose(); + twist_cov[utils::MSG_COV_IDX::X_X] = twist_cov_rotated(0, 0); + twist_cov[utils::MSG_COV_IDX::X_Y] = twist_cov_rotated(0, 1); + twist_cov[utils::MSG_COV_IDX::Y_X] = twist_cov_rotated(1, 0); + twist_cov[utils::MSG_COV_IDX::Y_Y] = twist_cov_rotated(1, 1); + + // initialize motion model + motion_model_.init(time, x, y, pose_cov, vx, vy, twist_cov); } - ekf_.init(X, P); + // Set motion model parameters + { + constexpr double q_stddev_x = 0.5; // [m/s] + constexpr double q_stddev_y = 0.5; // [m/s] + constexpr double q_stddev_vx = 9.8 * 0.3; // [m/(s*s)] + constexpr double q_stddev_vy = 9.8 * 0.3; // [m/(s*s)] + motion_model_.setMotionParams(q_stddev_x, q_stddev_y, q_stddev_vx, q_stddev_vy); + } + + // Set motion limits + motion_model_.setMotionLimits( + tier4_autoware_utils::kmph2mps(60), /* [m/s] maximum velocity, x */ + tier4_autoware_utils::kmph2mps(60) /* [m/s] maximum velocity, y */ + ); } bool UnknownTracker::predict(const rclcpp::Time & time) { - const double dt = (time - last_update_time_).seconds(); - bool ret = predict(dt, ekf_); - if (ret) { + // predict state vector X t+1 + bool is_predicted = motion_model_.predictState(time); + if (is_predicted) { last_update_time_ = time; } - return ret; + return is_predicted; } -bool UnknownTracker::predict(const double dt, KalmanFilter & ekf) const +autoware_auto_perception_msgs::msg::DetectedObject UnknownTracker::getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & /*self_transform*/) { - /* Motion model: Constant velocity model - * - * x_{k+1} = x_k + vx_k * dt - * y_{k+1} = y_k + vx_k * dt - * vx_{k+1} = vx_k - * vy_{k+1} = vy_k - * - */ - - /* Jacobian Matrix - * - * A = [ 1, 0, dt, 0] - * [ 0, 1, 0, dt] - * [ 0, 0, 1, 0] - * [ 0, 0, 0, 1] - */ - - // X t - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf.getX(X_t); - - // X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VX) * dt; - X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VY) * dt; - X_next_t(IDX::VX) = X_t(IDX::VX); - X_next_t(IDX::VY) = X_t(IDX::VY); - - // A - Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::VX) = dt; - A(IDX::Y, IDX::VY) = dt; - - // Q - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - // Rotate the covariance matrix according to the vehicle yaw - // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = ekf_params_.q_cov_x * dt * dt; - Q(IDX::X, IDX::Y) = 0.0; - Q(IDX::Y, IDX::Y) = ekf_params_.q_cov_y * dt * dt; - Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt; - Q(IDX::VY, IDX::VY) = ekf_params_.q_cov_vy * dt * dt; - Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); - - if (!ekf.predict(X_next_t, A, Q)) { - RCLCPP_WARN(logger_, "Pedestrian : Cannot predict"); - } + autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; - return true; + // UNCERTAINTY MODEL + if (!object.kinematics.has_position_covariance) { + float & r_cov_x= ekf_params_.r_cov_x; + float & r_cov_y= ekf_params_.r_cov_y; + auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; + const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double cos_yaw = std::cos(pose_yaw); + const double sin_yaw = std::sin(pose_yaw); + const double sin_2yaw = std::sin(2.0f * pose_yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + pose_cov[utils::MSG_COV_IDX::Y_Y] = + r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; // y - x + } + return updating_object; } bool UnknownTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - constexpr int dim_y = 2; // pos x, pos y depending on Pose output - - /* Set measurement matrix */ - Eigen::MatrixXd Y(dim_y, 1); - Y << object.kinematics.pose_with_covariance.pose.position.x, - object.kinematics.pose_with_covariance.pose.position.y; - - /* Set measurement matrix */ - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); - C(0, IDX::X) = 1.0; // for pos x - C(1, IDX::Y) = 1.0; // for pos y - - /* Set measurement noise covariance */ - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - if (!object.kinematics.has_position_covariance) { - R(0, 0) = ekf_params_.r_cov_x; // x - x - R(0, 1) = 0.0; // x - y - R(1, 1) = ekf_params_.r_cov_y; // y - y - R(1, 0) = R(0, 1); // y - x - } else { - R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - } - if (!ekf_.update(Y, C, R)) { - RCLCPP_WARN(logger_, "Pedestrian : Cannot update"); - } - - // limit vx, vy + // update motion model + bool is_updated = false; { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); - ekf_.getX(X_t); - ekf_.getP(P_t); - if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { - X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; - } - if (!(-max_vy_ <= X_t(IDX::VY) && X_t(IDX::VY) <= max_vy_)) { - X_t(IDX::VY) = X_t(IDX::VY) < 0 ? -max_vy_ : max_vy_; - } - ekf_.init(X_t, P_t); + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + + is_updated = + motion_model_.updateStatePose(x, y, object.kinematics.pose_with_covariance.covariance); + motion_model_.limitStates(); } // position z constexpr float gain = 0.9; z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; - return true; + return is_updated; } bool UnknownTracker::measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & /*self_transform*/) + const geometry_msgs::msg::Transform & self_transform) { + // keep the latest input object object_ = object; + // check time gap if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { RCLCPP_WARN( logger_, - "Pedestrian : There is a large gap between predicted time and measurement time. (%f)", + "UnknownTracker: There is a large gap between predicted time and measurement time. (%f)", (time - last_update_time_).seconds()); } - measureWithPose(object); + // update object + const autoware_auto_perception_msgs::msg::DetectedObject updating_object = + getUpdatingObject(object, self_transform); + measureWithPose(updating_object); return true; } @@ -257,54 +218,19 @@ bool UnknownTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict kinematics - KalmanFilter tmp_ekf_for_no_update = ekf_; - const double dt = (time - last_update_time_).seconds(); - if (0.001 /*1msec*/ < dt) { - predict(dt, tmp_ekf_for_no_update); - } - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - Eigen::MatrixXd P(ekf_params_.dim_x, ekf_params_.dim_x); // predicted state - tmp_ekf_for_no_update.getX(X_t); - tmp_ekf_for_no_update.getP(P); - auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; + + // predict from motion model + if (!motion_model_.getPredictedState( + time, pose_with_cov.pose, pose_with_cov.covariance, twist_with_cov.twist, + twist_with_cov.covariance)) { + RCLCPP_WARN(logger_, "UnknownTracker::getTrackedObject: Failed to get predicted state."); + return false; + } + // position - pose_with_cov.pose.position.x = X_t(IDX::X); - pose_with_cov.pose.position.y = X_t(IDX::Y); pose_with_cov.pose.position.z = z_; - constexpr double z_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double r_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double p_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double yaw_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = r_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = p_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = yaw_cov; - - // twist - const auto pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); - const double cos = std::cos(-pose_yaw); - const double sin = std::sin(-pose_yaw); - twist_with_cov.twist.linear.x = cos * X_t(IDX::VX) - sin * X_t(IDX::VY); - twist_with_cov.twist.linear.y = sin * X_t(IDX::VX) + cos * X_t(IDX::VY); - - // twist covariance - constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P(IDX::VY, IDX::VY); - twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = wz_cov; return true; } From 201f8acff9c5a6cdda0495adfd39f9f15e789fab Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 6 Mar 2024 15:02:07 +0900 Subject: [PATCH 22/42] fix: align number precisions to double Signed-off-by: Taekjin LEE --- .../tracker/model/bicycle_tracker.hpp | 13 ++++------ .../tracker/model/big_vehicle_tracker.hpp | 15 ++++-------- .../tracker/model/normal_vehicle_tracker.hpp | 15 ++++-------- .../tracker/model/pedestrian_tracker.hpp | 8 +++---- .../tracker/model/unknown_tracker.hpp | 11 +++++---- .../src/motion_model/cv_motion_model.cpp | 4 ++++ .../src/tracker/model/bicycle_tracker.cpp | 6 ++--- .../src/tracker/model/pedestrian_tracker.cpp | 10 ++++---- .../src/tracker/model/unknown_tracker.cpp | 24 +++++++++---------- 9 files changed, 47 insertions(+), 59 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp index fbe29228f0009..22220ed3f2ad4 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -35,16 +35,11 @@ class BicycleTracker : public Tracker struct EkfParams { - float p0_cov_vel; - float p0_cov_slip; - float p0_cov_x; - float p0_cov_y; - float p0_cov_yaw; - float r_cov_x; - float r_cov_y; - float r_cov_yaw; + double r_cov_x; + double r_cov_y; + double r_cov_yaw; } ekf_params_; - float z_; + double z_; private: struct BoundingBox diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index 2682e13f5e57c..9df39959b13f6 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -34,17 +34,12 @@ class BigVehicleTracker : public Tracker rclcpp::Time last_update_time_; struct EkfParams { - float p0_cov_vel; - float p0_cov_slip; - float p0_cov_x; - float p0_cov_y; - float p0_cov_yaw; - float r_cov_x; - float r_cov_y; - float r_cov_yaw; - float r_cov_vel; + double r_cov_x; + double r_cov_y; + double r_cov_yaw; + double r_cov_vel; } ekf_params_; - float z_; + double z_; double velocity_deviation_threshold_; private: diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index c237ef5dc21c5..2d4e19a8ccf67 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -35,17 +35,12 @@ class NormalVehicleTracker : public Tracker struct EkfParams { - float p0_cov_vel; - float p0_cov_slip; - float p0_cov_x; - float p0_cov_y; - float p0_cov_yaw; - float r_cov_x; - float r_cov_y; - float r_cov_yaw; - float r_cov_vel; + double r_cov_x; + double r_cov_y; + double r_cov_yaw; + double r_cov_vel; } ekf_params_; - float z_; + double z_; double velocity_deviation_threshold_; private: diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index cbce51015b743..c2f323ca0190b 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -34,11 +34,11 @@ class PedestrianTracker : public Tracker rclcpp::Time last_update_time_; struct EkfParams { - float r_cov_x; - float r_cov_y; - float r_cov_yaw; + double r_cov_x; + double r_cov_y; + double r_cov_yaw; } ekf_params_; - float z_; + double z_; private: struct BoundingBox diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp index 436adf664fb86..cf6f5dc6a3b99 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -21,6 +21,7 @@ #include "multi_object_tracker/motion_model/cv_motion_model.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" + #include class UnknownTracker : public Tracker @@ -33,12 +34,12 @@ class UnknownTracker : public Tracker rclcpp::Time last_update_time_; struct EkfParams { - float r_cov_x; - float r_cov_y; - float r_cov_vx; - float r_cov_vy; + double r_cov_x; + double r_cov_y; + double r_cov_vx; + double r_cov_vy; } ekf_params_; - float z_; + double z_; private: CVMotionModel motion_model_; diff --git a/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp index a627eb55d8bc0..5fa7eda54a858 100644 --- a/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp @@ -310,6 +310,10 @@ bool CVMotionModel::getPredictedState( // const double yaw = tf2::getYaw(pose.orientation); const double yaw = 0.0; + // print pose orientatin + // RCLCPP_INFO(logger_, "CVMotionModel::getPredictedState: pose.orientation: %f, %f, %f, %f", + // pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w); + // set position pose.position.x = X(IDX::X); pose.position.y = X(IDX::Y); diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index fabca3a0eb416..5634bc4f2ac69 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -54,9 +54,9 @@ BicycleTracker::BicycleTracker( // Initialize parameters // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty - float r_stddev_x = 0.5; // in vehicle coordinate [m] - float r_stddev_y = 0.4; // in vehicle coordinate [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // in map coordinate [rad] + double r_stddev_x = 0.5; // in vehicle coordinate [m] + double r_stddev_y = 0.4; // in vehicle coordinate [m] + double r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // in map coordinate [rad] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index 92ad2aae757a3..671e6f6a1cea3 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -152,7 +152,6 @@ bool PedestrianTracker::predict(const rclcpp::Time & time) return is_predicted; } - autoware_auto_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObject( const autoware_auto_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & /*self_transform*/) @@ -161,8 +160,8 @@ autoware_auto_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatin // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { - float & r_cov_x= ekf_params_.r_cov_x; - float & r_cov_y= ekf_params_.r_cov_y; + const double & r_cov_x = ekf_params_.r_cov_x; + const double & r_cov_y = ekf_params_.r_cov_y; auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); const double cos_yaw = std::cos(pose_yaw); @@ -205,7 +204,6 @@ bool PedestrianTracker::measureWithShape( constexpr float gain = 0.2; constexpr float gain_inv = 1.0 - gain; - // constexpr float gain = 0.9; if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x; bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y; @@ -242,7 +240,8 @@ bool PedestrianTracker::measure( // check time gap if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { RCLCPP_WARN( - logger_, "PedestrianTracker: There is a large gap between predicted time and measurement time. (%f)", + logger_, + "PedestrianTracker: There is a large gap between predicted time and measurement time. (%f)", (time - last_update_time_).seconds()); } @@ -252,7 +251,6 @@ bool PedestrianTracker::measure( measureWithPose(updating_object); measureWithShape(updating_object); - (void)self_transform; // currently do not use self vehicle position return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp index dba8abe3aacf5..5246396214a80 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -48,8 +48,8 @@ UnknownTracker::UnknownTracker( // initialize params // measurement noise covariance - float r_stddev_x = 1.0; // [m] - float r_stddev_y = 1.0; // [m] + constexpr double r_stddev_x = 1.0; // [m] + constexpr double r_stddev_y = 1.0; // [m] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); @@ -70,9 +70,9 @@ UnknownTracker::UnknownTracker( vy = std::sin(yaw) * vel_x + std::cos(yaw) * vel_y; } - if(!object.kinematics.has_position_covariance){ - constexpr double p0_stddev_x = 1.0; // [m] - constexpr double p0_stddev_y = 1.0; // [m] + if (!object.kinematics.has_position_covariance) { + constexpr double p0_stddev_x = 1.0; // [m] + constexpr double p0_stddev_y = 1.0; // [m] const double p0_cov_x = std::pow(p0_stddev_x, 2.0); const double p0_cov_y = std::pow(p0_stddev_y, 2.0); @@ -118,10 +118,10 @@ UnknownTracker::UnknownTracker( // Set motion model parameters { - constexpr double q_stddev_x = 0.5; // [m/s] - constexpr double q_stddev_y = 0.5; // [m/s] - constexpr double q_stddev_vx = 9.8 * 0.3; // [m/(s*s)] - constexpr double q_stddev_vy = 9.8 * 0.3; // [m/(s*s)] + constexpr double q_stddev_x = 0.5; // [m/s] + constexpr double q_stddev_y = 0.5; // [m/s] + constexpr double q_stddev_vx = 9.8 * 0.3; // [m/(s*s)] + constexpr double q_stddev_vy = 9.8 * 0.3; // [m/(s*s)] motion_model_.setMotionParams(q_stddev_x, q_stddev_y, q_stddev_vx, q_stddev_vy); } @@ -150,8 +150,8 @@ autoware_auto_perception_msgs::msg::DetectedObject UnknownTracker::getUpdatingOb // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { - float & r_cov_x= ekf_params_.r_cov_x; - float & r_cov_y= ekf_params_.r_cov_y; + const double & r_cov_x = ekf_params_.r_cov_x; + const double & r_cov_y = ekf_params_.r_cov_y; auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); const double cos_yaw = std::cos(pose_yaw); @@ -182,7 +182,7 @@ bool UnknownTracker::measureWithPose( } // position z - constexpr float gain = 0.9; + constexpr double gain = 0.9; z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; return is_updated; From f6d7d0640e710bcdd5ccd494cdba1ac73e585fe2 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 6 Mar 2024 15:43:57 +0900 Subject: [PATCH 23/42] fix: resolve overflow issue by including proper header files Signed-off-by: Taekjin LEE --- .../motion_model/bicycle_motion_model.hpp | 6 +++- .../motion_model/ctrv_motion_model.hpp | 8 +++-- .../motion_model/cv_motion_model.hpp | 6 +++- .../src/motion_model/ctrv_motion_model.cpp | 34 +++++++++---------- .../src/motion_model/cv_motion_model.cpp | 11 ++---- .../src/tracker/model/pedestrian_tracker.cpp | 4 +-- 6 files changed, 38 insertions(+), 31 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp index 856e5d0194f9f..b0ec2278cbf15 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp @@ -25,7 +25,11 @@ #include #include -#include "autoware_auto_perception_msgs/msg/detected_object.hpp" +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif class BicycleMotionModel { diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp index a54ee8881f310..a98e12da7921e 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp @@ -25,7 +25,11 @@ #include #include -#include "autoware_auto_perception_msgs/msg/detected_object.hpp" +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif class CTRVMotionModel { @@ -45,7 +49,7 @@ class CTRVMotionModel double q_cov_x; double q_cov_y; double q_cov_yaw; - double q_cov_vx; + double q_cov_vel; double q_cov_wz; double max_vel; double max_wz; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp index 5253ad3f45a3e..16f2bf5629304 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp @@ -25,7 +25,11 @@ #include #include -#include "autoware_auto_perception_msgs/msg/detected_object.hpp" +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif class CVMotionModel { diff --git a/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp index 2ee4f55367119..9f52442074ff2 100644 --- a/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp @@ -75,13 +75,13 @@ bool CTRVMotionModel::init( void CTRVMotionModel::setDefaultParams() { // process noise covariance - constexpr double q_stddev_x = 0.4; // [m/s] - constexpr double q_stddev_y = 0.4; // [m/s] + constexpr double q_stddev_x = 0.5; // [m/s] + constexpr double q_stddev_y = 0.5; // [m/s] constexpr double q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] - constexpr double q_stddev_vx = tier4_autoware_utils::kmph2mps(5); // [m/(s*s)] + constexpr double q_stddev_vel = 9.8 * 0.3; // [m/(s*s)] constexpr double q_stddev_wz = tier4_autoware_utils::deg2rad(30); // [rad/(s*s)] - setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vx, q_stddev_wz); + setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vel, q_stddev_wz); // set motion limitations constexpr double max_vel = tier4_autoware_utils::kmph2mps(10); // [m/s] @@ -95,13 +95,13 @@ void CTRVMotionModel::setDefaultParams() void CTRVMotionModel::setMotionParams( const double & q_stddev_x, const double & q_stddev_y, const double & q_stddev_yaw, - const double & q_stddev_vx, const double & q_stddev_wz) + const double & q_stddev_vel, const double & q_stddev_wz) { // set process noise covariance parameters motion_params_.q_cov_x = std::pow(q_stddev_x, 2.0); motion_params_.q_cov_y = std::pow(q_stddev_y, 2.0); motion_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); - motion_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); + motion_params_.q_cov_vel = std::pow(q_stddev_vel, 2.0); motion_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0); } @@ -333,21 +333,21 @@ bool CTRVMotionModel::predictState(const double dt, KalmanFilter & ekf) const A(IDX::YAW, IDX::WZ) = dt; // Process noise covariance Q + const double q_cov_x = std::pow(0.5 * motion_params_.q_cov_x * dt, 2); + const double q_cov_y = std::pow(0.5 * motion_params_.q_cov_y * dt, 2); + const double q_cov_yaw = std::pow(motion_params_.q_cov_yaw * dt, 2); + const double q_cov_vel = std::pow(motion_params_.q_cov_vel * dt, 2); + const double q_cov_wz = std::pow(motion_params_.q_cov_wz * dt, 2); Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(DIM, DIM); // Rotate the covariance matrix according to the vehicle yaw // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = - (motion_params_.q_cov_x * cos_yaw * cos_yaw + motion_params_.q_cov_y * sin_yaw * sin_yaw) * dt * - dt; - Q(IDX::X, IDX::Y) = - (0.5f * (motion_params_.q_cov_x - motion_params_.q_cov_y) * sin_2yaw) * dt * dt; - Q(IDX::Y, IDX::Y) = - (motion_params_.q_cov_x * sin_yaw * sin_yaw + motion_params_.q_cov_y * cos_yaw * cos_yaw) * dt * - dt; + Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); + Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); + Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::YAW, IDX::YAW) = motion_params_.q_cov_yaw * dt * dt; - Q(IDX::VEL, IDX::VEL) = motion_params_.q_cov_vx * dt * dt; - Q(IDX::WZ, IDX::WZ) = motion_params_.q_cov_wz * dt * dt; + Q(IDX::YAW, IDX::YAW) = q_cov_yaw; + Q(IDX::VEL, IDX::VEL) = q_cov_vel; + Q(IDX::WZ, IDX::WZ) = q_cov_wz; // control-input model B and control-input u are not used // Eigen::MatrixXd B = Eigen::MatrixXd::Zero(DIM, DIM); diff --git a/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp index 5fa7eda54a858..3595152111f90 100644 --- a/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp @@ -23,12 +23,12 @@ #include #include +#include + #define EIGEN_MPL2_ONLY #include #include -#include - // cspell: ignore CV // Constant Velocity (CV) motion model @@ -307,12 +307,7 @@ bool CVMotionModel::getPredictedState( } // get yaw from pose - // const double yaw = tf2::getYaw(pose.orientation); - const double yaw = 0.0; - - // print pose orientatin - // RCLCPP_INFO(logger_, "CVMotionModel::getPredictedState: pose.orientation: %f, %f, %f, %f", - // pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w); + const double yaw = tf2::getYaw(pose.orientation); // set position pose.position.x = X(IDX::X); diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index 671e6f6a1cea3..9de6416efad10 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -127,8 +127,8 @@ PedestrianTracker::PedestrianTracker( // Set motion model parameters { - constexpr double q_stddev_x = 0.4; // [m/s] - constexpr double q_stddev_y = 0.4; // [m/s] + constexpr double q_stddev_x = 0.5; // [m/s] + constexpr double q_stddev_y = 0.3; // [m/s] constexpr double q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] constexpr double q_stddev_vx = tier4_autoware_utils::kmph2mps(5); // [m/(s*s)] constexpr double q_stddev_wz = tier4_autoware_utils::deg2rad(30); // [rad/(s*s)] From c0b706c8b9bd5884d9aa4447d5919b2b68b09e07 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 6 Mar 2024 16:00:51 +0900 Subject: [PATCH 24/42] fix: missing twist cov Signed-off-by: Taekjin LEE --- .../multi_object_tracker/src/motion_model/ctrv_motion_model.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp index 9f52442074ff2..fd96fc533f899 100644 --- a/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp @@ -444,7 +444,7 @@ bool CTRVMotionModel::getPredictedState( twist_cov[utils::MSG_COV_IDX::Y_X] = 0.0; twist_cov[utils::MSG_COV_IDX::Y_Y] = vy_cov; twist_cov[utils::MSG_COV_IDX::Y_YAW] = 0.0; - twist_cov[utils::MSG_COV_IDX::YAW_X] = 0.0; + twist_cov[utils::MSG_COV_IDX::YAW_X] = P(IDX::WZ, IDX::VEL); twist_cov[utils::MSG_COV_IDX::YAW_Y] = 0.0; twist_cov[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ); twist_cov[utils::MSG_COV_IDX::Z_Z] = vz_cov; From 9d1a509f9c41c3e5d017073f7fca24f66e3c7e6d Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 6 Mar 2024 17:40:14 +0900 Subject: [PATCH 25/42] fix: limit too large dt on unknown tracker, set size gain to 0.1 Signed-off-by: Taekjin LEE --- .../motion_model/cv_motion_model.hpp | 1 + .../src/motion_model/bicycle_motion_model.cpp | 7 +++-- .../src/motion_model/ctrv_motion_model.cpp | 9 +++--- .../src/motion_model/cv_motion_model.cpp | 29 ++++++++++++++++--- .../src/tracker/model/bicycle_tracker.cpp | 8 ++--- .../src/tracker/model/big_vehicle_tracker.cpp | 8 ++--- .../tracker/model/normal_vehicle_tracker.cpp | 8 ++--- .../src/tracker/model/pedestrian_tracker.cpp | 12 ++++---- .../src/tracker/model/unknown_tracker.cpp | 4 +-- 9 files changed, 55 insertions(+), 31 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp index 16f2bf5629304..1269bdc4a1ec0 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp @@ -52,6 +52,7 @@ class CVMotionModel double q_cov_vy; double max_vx; double max_vy; + double dt_max; } motion_params_; public: diff --git a/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp index 3b7528ee337aa..279502550b344 100644 --- a/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp @@ -314,8 +314,11 @@ bool BicycleMotionModel::predictState(const rclcpp::Time & time) if (!predictState(dt_, ekf_)) { return false; } - last_update_time_ = time; + // add interval to last_update_time_ + last_update_time_ += rclcpp::Duration::from_seconds(dt_); } + // update last_update_time_ to the estimation time + last_update_time_ = time; return true; } @@ -346,8 +349,6 @@ bool BicycleMotionModel::predictState(const double dt, KalmanFilter & ekf) const * */ - // MOTION MODEL (predict) - // Current state vector X t Eigen::MatrixXd X_t = getStateVector(); diff --git a/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp index fd96fc533f899..f2eaac63e1216 100644 --- a/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp @@ -78,7 +78,7 @@ void CTRVMotionModel::setDefaultParams() constexpr double q_stddev_x = 0.5; // [m/s] constexpr double q_stddev_y = 0.5; // [m/s] constexpr double q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] - constexpr double q_stddev_vel = 9.8 * 0.3; // [m/(s*s)] + constexpr double q_stddev_vel = 9.8 * 0.3; // [m/(s*s)] constexpr double q_stddev_wz = tier4_autoware_utils::deg2rad(30); // [rad/(s*s)] setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vel, q_stddev_wz); @@ -281,8 +281,11 @@ bool CTRVMotionModel::predictState(const rclcpp::Time & time) if (!predictState(dt_, ekf_)) { return false; } - last_update_time_ = time; + // add interval to last_update_time_ + last_update_time_ += rclcpp::Duration::from_seconds(dt_); } + // update last_update_time_ to the estimation time + last_update_time_ = time; return true; } @@ -307,8 +310,6 @@ bool CTRVMotionModel::predictState(const double dt, KalmanFilter & ekf) const * [ 0, 0, 0, 0, 1] */ - // MOTION MODEL (predict) - // Current state vector X t Eigen::MatrixXd X_t = getStateVector(); diff --git a/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp index 3595152111f90..eea0a5299e987 100644 --- a/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp @@ -86,6 +86,10 @@ void CVMotionModel::setDefaultParams() constexpr double max_vx = tier4_autoware_utils::kmph2mps(60); // [m/s] constexpr double max_vy = tier4_autoware_utils::kmph2mps(60); // [m/s] setMotionLimits(max_vx, max_vy); + + // set prediction parameters + constexpr double dt_max = 0.11; // [s] + motion_params_.dt_max = dt_max; } void CVMotionModel::setMotionParams( @@ -209,9 +213,17 @@ bool CVMotionModel::predictState(const rclcpp::Time & time) RCLCPP_WARN(logger_, "CVMotionModel::predictState: dt is negative. (%f)", dt); return false; } - if (!predictState(dt, ekf_)) { - return false; + // if dt is too large, shorten dt and repeat prediction + const uint32_t repeat = std::ceil(dt / motion_params_.dt_max); + const double dt_ = dt / repeat; + for (uint32_t i = 0; i < repeat; ++i) { + if (!predictState(dt_, ekf_)) { + return false; + } + // add interval to last_update_time_ + last_update_time_ += rclcpp::Duration::from_seconds(dt_); } + // update last_update_time_ to the estimation time last_update_time_ = time; return true; } @@ -286,8 +298,13 @@ bool CVMotionModel::getPredictedState( // predict only when dt is small enough if (0.001 /*1msec*/ < dt) { - if (!predictState(dt, tmp_ekf_for_no_update)) { - return false; + // if dt is too large, shorten dt and repeat prediction + const uint32_t repeat = std::ceil(dt / motion_params_.dt_max); + const double dt_ = dt / repeat; + for (uint32_t i = 0; i < repeat; ++i) { + if (!predictState(dt_, tmp_ekf_for_no_update)) { + return false; + } } } tmp_ekf_for_no_update.getX(X); @@ -308,6 +325,10 @@ bool CVMotionModel::getPredictedState( // get yaw from pose const double yaw = tf2::getYaw(pose.orientation); + // const double yaw = std::atan2( + // 2.0 * (pose.orientation.w * pose.orientation.z + pose.orientation.x * pose.orientation.y), + // 1.0 - 2.0 * (pose.orientation.y * pose.orientation.y + pose.orientation.z * + // pose.orientation.z)); // set position pose.position.x = X(IDX::X); diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 5634bc4f2ac69..1631fd17f10f3 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -210,8 +210,8 @@ bool BicycleTracker::measureWithPose( } // position z - constexpr float gain = 0.9; - z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; + constexpr double gain = 0.1; + z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; return is_updated; } @@ -219,8 +219,8 @@ bool BicycleTracker::measureWithPose( bool BicycleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - constexpr float gain = 0.2; - constexpr float gain_inv = 1.0 - gain; + constexpr double gain = 0.1; + constexpr double gain_inv = 1.0 - gain; // update object size bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x; diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 055654d5fc26c..ae3fd71a34770 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -294,8 +294,8 @@ bool BigVehicleTracker::measureWithPose( } // position z - constexpr float gain = 0.9; - z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; + constexpr double gain = 0.1; + z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; return is_updated; } @@ -303,8 +303,8 @@ bool BigVehicleTracker::measureWithPose( bool BigVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - constexpr float gain = 0.2; - constexpr float gain_inv = 1.0 - gain; + constexpr double gain = 0.1; + constexpr double gain_inv = 1.0 - gain; // update object size bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x; diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index bfe08d4491eee..4c2610a7b35ae 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -294,8 +294,8 @@ bool NormalVehicleTracker::measureWithPose( } // position z - constexpr float gain = 0.9; - z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; + constexpr double gain = 0.1; + z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; return is_updated; } @@ -303,8 +303,8 @@ bool NormalVehicleTracker::measureWithPose( bool NormalVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - constexpr float gain = 0.2; - constexpr float gain_inv = 1.0 - gain; + constexpr double gain = 0.1; + constexpr double gain_inv = 1.0 - gain; // update object size bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x; diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index 9de6416efad10..fa9a794379f06 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -128,9 +128,9 @@ PedestrianTracker::PedestrianTracker( // Set motion model parameters { constexpr double q_stddev_x = 0.5; // [m/s] - constexpr double q_stddev_y = 0.3; // [m/s] + constexpr double q_stddev_y = 0.5; // [m/s] constexpr double q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] - constexpr double q_stddev_vx = tier4_autoware_utils::kmph2mps(5); // [m/(s*s)] + constexpr double q_stddev_vx = 9.8 * 0.3; // [m/(s*s)] constexpr double q_stddev_wz = tier4_autoware_utils::deg2rad(30); // [rad/(s*s)] motion_model_.setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vx, q_stddev_wz); } @@ -192,8 +192,8 @@ bool PedestrianTracker::measureWithPose( } // position z - constexpr float gain = 0.9; - z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; + constexpr double gain = 0.1; + z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; return is_updated; } @@ -201,8 +201,8 @@ bool PedestrianTracker::measureWithPose( bool PedestrianTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - constexpr float gain = 0.2; - constexpr float gain_inv = 1.0 - gain; + constexpr double gain = 0.1; + constexpr double gain_inv = 1.0 - gain; if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x; diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp index 5246396214a80..a582238628490 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -182,8 +182,8 @@ bool UnknownTracker::measureWithPose( } // position z - constexpr double gain = 0.9; - z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; + constexpr double gain = 0.1; + z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; return is_updated; } From d017983e4ae8c15fac8cde867347a826916e7bf6 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 7 Mar 2024 13:09:23 +0900 Subject: [PATCH 26/42] fix: set parameter before init Signed-off-by: Taekjin LEE --- .../src/motion_model/bicycle_motion_model.cpp | 95 ++++++++++--------- .../src/motion_model/ctrv_motion_model.cpp | 76 +++++++-------- .../src/motion_model/cv_motion_model.cpp | 78 ++++++++------- .../src/tracker/model/bicycle_tracker.cpp | 61 ++++++------ .../src/tracker/model/big_vehicle_tracker.cpp | 60 ++++++------ .../tracker/model/normal_vehicle_tracker.cpp | 60 ++++++------ .../src/tracker/model/pedestrian_tracker.cpp | 32 +++---- .../src/tracker/model/unknown_tracker.cpp | 30 +++--- 8 files changed, 245 insertions(+), 247 deletions(-) diff --git a/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp index 279502550b344..4237dd2886705 100644 --- a/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp @@ -37,45 +37,6 @@ BicycleMotionModel::BicycleMotionModel() setDefaultParams(); } -bool BicycleMotionModel::init( - const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P, - const double & length) -{ - // set last update time - last_update_time_ = time; - - // initialize Kalman filter - ekf_.init(X, P); - - // set initial extended state - updateExtendedState(length); - - // set initialized flag - is_initialized_ = true; - - return true; -} - -bool BicycleMotionModel::init( - const rclcpp::Time & time, const double & x, const double & y, const double & yaw, - const std::array & pose_cov, const double & vel, const double & vel_cov, - const double & slip, const double & slip_cov, const double & length) -{ - // initialize state vector X - Eigen::MatrixXd X(DIM, 1); - X << x, y, yaw, vel, slip; - - // initialize covariance matrix P - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(DIM, DIM); - P(IDX::X, IDX::X) = pose_cov[utils::MSG_COV_IDX::X_X]; - P(IDX::Y, IDX::Y) = pose_cov[utils::MSG_COV_IDX::Y_Y]; - P(IDX::YAW, IDX::YAW) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; - P(IDX::VEL, IDX::VEL) = vel_cov; - P(IDX::SLIP, IDX::SLIP) = slip_cov; - - return init(time, X, P, length); -} - void BicycleMotionModel::setDefaultParams() { // set default motion parameters @@ -97,12 +58,12 @@ void BicycleMotionModel::setDefaultParams() lr_min); // set motion limitations - constexpr double max_vel = tier4_autoware_utils::kmph2mps(100); // [m/s] - constexpr double max_slip = 30.0; // [deg] + constexpr double max_vel = tier4_autoware_utils::kmph2mps(100); // [m/s] maximum velocity + constexpr double max_slip = 30.0; // [deg] maximum slip angle setMotionLimits(max_vel, max_slip); // set prediction parameters - constexpr double dt_max = 0.11; // [s] + constexpr double dt_max = 0.11; // [s] maximum time interval for prediction motion_params_.dt_max = dt_max; } @@ -124,11 +85,16 @@ void BicycleMotionModel::setMotionParams( std::pow(tier4_autoware_utils::deg2rad(q_stddev_slip_rate_max), 2.0); motion_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(q_max_slip_angle); - // set extended state parameters + constexpr double minimum_wheel_pos = 0.01; // minimum of 0.01m + if (lf_min < minimum_wheel_pos || lr_min < minimum_wheel_pos) { + RCLCPP_WARN( + logger_, + "BicycleMotionModel::setMotionParams: minimum wheel position should be greater than 0.01m."); + } + motion_params_.lf_min = (lf_min < minimum_wheel_pos) ? minimum_wheel_pos : lf_min; + motion_params_.lr_min = (lr_min < minimum_wheel_pos) ? minimum_wheel_pos : lr_min; motion_params_.lf_ratio = lf_ratio; - motion_params_.lf_min = lf_min; motion_params_.lr_ratio = lr_ratio; - motion_params_.lr_min = lr_min; } void BicycleMotionModel::setMotionLimits(const double & max_vel, const double & max_slip) @@ -138,6 +104,45 @@ void BicycleMotionModel::setMotionLimits(const double & max_vel, const double & motion_params_.max_slip = tier4_autoware_utils::deg2rad(max_slip); } +bool BicycleMotionModel::init( + const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P, + const double & length) +{ + // set last update time + last_update_time_ = time; + + // initialize Kalman filter + if (!ekf_.init(X, P)) return false; + + // set initial extended state + if (!updateExtendedState(length)) return false; + + // set initialized flag + is_initialized_ = true; + + return true; +} + +bool BicycleMotionModel::init( + const rclcpp::Time & time, const double & x, const double & y, const double & yaw, + const std::array & pose_cov, const double & vel, const double & vel_cov, + const double & slip, const double & slip_cov, const double & length) +{ + // initialize state vector X + Eigen::MatrixXd X(DIM, 1); + X << x, y, yaw, vel, slip; + + // initialize covariance matrix P + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(DIM, DIM); + P(IDX::X, IDX::X) = pose_cov[utils::MSG_COV_IDX::X_X]; + P(IDX::Y, IDX::Y) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + P(IDX::YAW, IDX::YAW) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; + P(IDX::VEL, IDX::VEL) = vel_cov; + P(IDX::SLIP, IDX::SLIP) = slip_cov; + + return init(time, X, P, length); +} + bool BicycleMotionModel::updateStatePose( const double & x, const double & y, const std::array & pose_cov) { diff --git a/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp index f2eaac63e1216..3d4c58292710a 100644 --- a/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp @@ -37,41 +37,6 @@ CTRVMotionModel::CTRVMotionModel() setDefaultParams(); } -bool CTRVMotionModel::init( - const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P) -{ - // set last update time - last_update_time_ = time; - - // initialize Kalman filter - ekf_.init(X, P); - - // set initialized flag - is_initialized_ = true; - - return true; -} - -bool CTRVMotionModel::init( - const rclcpp::Time & time, const double & x, const double & y, const double & yaw, - const std::array & pose_cov, const double & vel, const double & vel_cov, - const double & wz, const double & wz_cov) -{ - // initialize state vector X - Eigen::MatrixXd X(DIM, 1); - X << x, y, yaw, vel, wz; - - // initialize covariance matrix P - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(DIM, DIM); - P(IDX::X, IDX::X) = pose_cov[utils::MSG_COV_IDX::X_X]; - P(IDX::Y, IDX::Y) = pose_cov[utils::MSG_COV_IDX::Y_Y]; - P(IDX::YAW, IDX::YAW) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; - P(IDX::VEL, IDX::VEL) = vel_cov; - P(IDX::WZ, IDX::WZ) = wz_cov; - - return init(time, X, P); -} - void CTRVMotionModel::setDefaultParams() { // process noise covariance @@ -84,12 +49,12 @@ void CTRVMotionModel::setDefaultParams() setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vel, q_stddev_wz); // set motion limitations - constexpr double max_vel = tier4_autoware_utils::kmph2mps(10); // [m/s] - constexpr double max_wz = 30.0; // [deg] + constexpr double max_vel = tier4_autoware_utils::kmph2mps(10); // [m/s] maximum velocity + constexpr double max_wz = 30.0; // [deg] maximum yaw rate setMotionLimits(max_vel, max_wz); // set prediction parameters - constexpr double dt_max = 0.11; // [s] + constexpr double dt_max = 0.11; // [s] maximum time interval for prediction motion_params_.dt_max = dt_max; } @@ -112,6 +77,41 @@ void CTRVMotionModel::setMotionLimits(const double & max_vel, const double & max motion_params_.max_wz = tier4_autoware_utils::deg2rad(max_wz); } +bool CTRVMotionModel::init( + const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P) +{ + // set last update time + last_update_time_ = time; + + // initialize Kalman filter + if (!ekf_.init(X, P)) return false; + + // set initialized flag + is_initialized_ = true; + + return true; +} + +bool CTRVMotionModel::init( + const rclcpp::Time & time, const double & x, const double & y, const double & yaw, + const std::array & pose_cov, const double & vel, const double & vel_cov, + const double & wz, const double & wz_cov) +{ + // initialize state vector X + Eigen::MatrixXd X(DIM, 1); + X << x, y, yaw, vel, wz; + + // initialize covariance matrix P + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(DIM, DIM); + P(IDX::X, IDX::X) = pose_cov[utils::MSG_COV_IDX::X_X]; + P(IDX::Y, IDX::Y) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + P(IDX::YAW, IDX::YAW) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; + P(IDX::VEL, IDX::VEL) = vel_cov; + P(IDX::WZ, IDX::WZ) = wz_cov; + + return init(time, X, P); +} + bool CTRVMotionModel::updateStatePose( const double & x, const double & y, const std::array & pose_cov) { diff --git a/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp index eea0a5299e987..8d7817c64b5c5 100644 --- a/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp @@ -39,6 +39,43 @@ CVMotionModel::CVMotionModel() setDefaultParams(); } +void CVMotionModel::setDefaultParams() +{ + // process noise covariance + constexpr double q_stddev_x = 0.5; // [m/s] standard deviation of x + constexpr double q_stddev_y = 0.5; // [m/s] standard deviation of y + constexpr double q_stddev_vx = 9.8 * 0.3; // [m/(s*s)] standard deviation of vx + constexpr double q_stddev_vy = 9.8 * 0.3; // [m/(s*s)] standard deviation of vy + setMotionParams(q_stddev_x, q_stddev_y, q_stddev_vx, q_stddev_vy); + + // set motion limitations + constexpr double max_vx = tier4_autoware_utils::kmph2mps(60); // [m/s] maximum x velocity + constexpr double max_vy = tier4_autoware_utils::kmph2mps(60); // [m/s] maximum y velocity + setMotionLimits(max_vx, max_vy); + + // set prediction parameters + constexpr double dt_max = 0.11; // [s] maximum time interval for prediction + motion_params_.dt_max = dt_max; +} + +void CVMotionModel::setMotionParams( + const double & q_stddev_x, const double & q_stddev_y, const double & q_stddev_vx, + const double & q_stddev_vy) +{ + // set process noise covariance parameters + motion_params_.q_cov_x = std::pow(q_stddev_x, 2.0); + motion_params_.q_cov_y = std::pow(q_stddev_y, 2.0); + motion_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); + motion_params_.q_cov_vy = std::pow(q_stddev_vy, 2.0); +} + +void CVMotionModel::setMotionLimits(const double & max_vx, const double & max_vy) +{ + // set motion limitations + motion_params_.max_vx = max_vx; + motion_params_.max_vy = max_vy; +} + bool CVMotionModel::init( const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P) { @@ -46,7 +83,7 @@ bool CVMotionModel::init( last_update_time_ = time; // initialize Kalman filter - ekf_.init(X, P); + if (!ekf_.init(X, P)) return false; // set initialized flag is_initialized_ = true; @@ -73,43 +110,6 @@ bool CVMotionModel::init( return init(time, X, P); } -void CVMotionModel::setDefaultParams() -{ - // process noise covariance - constexpr double q_stddev_x = 0.5; // [m/s] - constexpr double q_stddev_y = 0.5; // [m/s] - constexpr double q_stddev_vx = 9.8 * 0.3; // [m/(s*s)] - constexpr double q_stddev_vy = 9.8 * 0.3; // [m/(s*s)] - setMotionParams(q_stddev_x, q_stddev_y, q_stddev_vx, q_stddev_vy); - - // set motion limitations - constexpr double max_vx = tier4_autoware_utils::kmph2mps(60); // [m/s] - constexpr double max_vy = tier4_autoware_utils::kmph2mps(60); // [m/s] - setMotionLimits(max_vx, max_vy); - - // set prediction parameters - constexpr double dt_max = 0.11; // [s] - motion_params_.dt_max = dt_max; -} - -void CVMotionModel::setMotionParams( - const double & q_stddev_x, const double & q_stddev_y, const double & q_stddev_vx, - const double & q_stddev_vy) -{ - // set process noise covariance parameters - motion_params_.q_cov_x = std::pow(q_stddev_x, 2.0); - motion_params_.q_cov_y = std::pow(q_stddev_y, 2.0); - motion_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); - motion_params_.q_cov_vy = std::pow(q_stddev_vy, 2.0); -} - -void CVMotionModel::setMotionLimits(const double & max_vx, const double & max_vy) -{ - // set motion limitations - motion_params_.max_vx = max_vx; - motion_params_.max_vy = max_vy; -} - bool CVMotionModel::updateStatePose( const double & x, const double & y, const std::array & pose_cov) { @@ -247,8 +247,6 @@ bool CVMotionModel::predictState(const double dt, KalmanFilter & ekf) const * [ 0, 0, 0, 1] */ - // MOTION MODEL (predict) - // Current state vector X t Eigen::MatrixXd X_t = getStateVector(); diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 1631fd17f10f3..63492682cc453 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -61,6 +61,35 @@ BicycleTracker::BicycleTracker( ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); + // Set motion model parameters + { + constexpr double q_stddev_acc_long = + 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + constexpr double q_stddev_yaw_rate_min = 5.0; // [deg/s] uncertain yaw change rate, minimum + constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate, maximum + constexpr double q_stddev_slip_rate_min = + 1.0; // [deg/s] uncertain slip angle change rate, minimum + constexpr double q_stddev_slip_rate_max = + 10.0; // [deg/s] uncertain slip angle change rate, maximum + constexpr double q_max_slip_angle = 30; // [deg] max slip angle + constexpr double lf_ratio = 0.3; // [-] ratio of front wheel position + constexpr double lf_min = 0.3; // [m] minimum front wheel position + constexpr double lr_ratio = 0.3; // [-] ratio of rear wheel position + constexpr double lr_min = 0.3; // [m] minimum rear wheel position + motion_model_.setMotionParams( + q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max, + q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio, + lr_min); + } + + // Set motion limits + { + constexpr double max_vel = tier4_autoware_utils::kmph2mps(80); // [m/s] maximum velocity + constexpr double max_slip = 30; // [deg] maximum slip angle + motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle + } + // Set initial state { const double x = object.kinematics.pose_with_covariance.pose.position.x; @@ -112,35 +141,6 @@ BicycleTracker::BicycleTracker( // initialize motion model motion_model_.init(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); } - - // Set motion model parameters - { - constexpr double q_stddev_acc_long = - 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration - constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration - constexpr double q_stddev_yaw_rate_min = 5.0; // [deg/s] uncertain yaw change rate, minimum - constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate, maximum - constexpr double q_stddev_slip_rate_min = - 1.0; // [deg/s] uncertain slip angle change rate, minimum - constexpr double q_stddev_slip_rate_max = - 10.0; // [deg/s] uncertain slip angle change rate, maximum - constexpr double q_max_slip_angle = 30; // [deg] max slip angle - constexpr double lf_ratio = 0.3; // [-] ratio of front wheel position - constexpr double lf_min = 0.3; // [m] minimum front wheel position - constexpr double lr_ratio = 0.3; // [-] ratio of rear wheel position - constexpr double lr_min = 0.3; // [m] minimum rear wheel position - motion_model_.setMotionParams( - q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max, - q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio, - lr_min); - } - - // Set motion limits - { - constexpr double max_vel = tier4_autoware_utils::kmph2mps(80); // [m/s] maximum velocity - constexpr double max_slip = 30; // [deg] maximum slip angle - motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle - } } bool BicycleTracker::predict(const rclcpp::Time & time) @@ -169,6 +169,7 @@ autoware_auto_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingOb // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { + // fill covariance matrix auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; pose_cov[utils::MSG_COV_IDX::X_X] = ekf_params_.r_cov_x; // x - x pose_cov[utils::MSG_COV_IDX::X_Y] = 0; // x - y @@ -186,8 +187,6 @@ bool BicycleTracker::measureWithPose( // current (predicted) state Eigen::MatrixXd X_t = motion_model_.getStateVector(); - // MOTION MODEL (update) - // get measurement yaw angle to update double measurement_yaw = 0.0; bool is_yaw_available = utils::getMeasurementYaw(object, X_t(IDX::YAW), measurement_yaw); diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index ae3fd71a34770..4fa1d671a896d 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -83,6 +83,35 @@ BigVehicleTracker::BigVehicleTracker( last_input_bounding_box_ = bounding_box_; } + // Set motion model parameters + { + constexpr double q_stddev_acc_long = + 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + constexpr double q_stddev_yaw_rate_min = 1.5; // [deg/s] uncertain yaw change rate, minimum + constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate, maximum + constexpr double q_stddev_slip_rate_min = + 0.3; // [deg/s] uncertain slip angle change rate, minimum + constexpr double q_stddev_slip_rate_max = + 10.0; // [deg/s] uncertain slip angle change rate, maximum + constexpr double q_max_slip_angle = 30; // [deg] max slip angle + constexpr double lf_ratio = 0.3; // [-] ratio of front wheel position + constexpr double lf_min = 1.5; // [m] minimum front wheel position + constexpr double lr_ratio = 0.25; // [-] ratio of rear wheel position + constexpr double lr_min = 1.5; // [m] minimum rear wheel position + motion_model_.setMotionParams( + q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max, + q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio, + lr_min); + } + + // Set motion limits + { + constexpr double max_vel = tier4_autoware_utils::kmph2mps(100); // [m/s] maximum velocity + constexpr double max_slip = 30; // [deg] maximum slip angle + motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle + } + // Set initial state { const double x = object.kinematics.pose_with_covariance.pose.position.x; @@ -134,35 +163,6 @@ BigVehicleTracker::BigVehicleTracker( // initialize motion model motion_model_.init(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); } - - // Set motion model parameters - { - constexpr double q_stddev_acc_long = - 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration - constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration - constexpr double q_stddev_yaw_rate_min = 1.5; // [deg/s] uncertain yaw change rate, minimum - constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate, maximum - constexpr double q_stddev_slip_rate_min = - 0.3; // [deg/s] uncertain slip angle change rate, minimum - constexpr double q_stddev_slip_rate_max = - 10.0; // [deg/s] uncertain slip angle change rate, maximum - constexpr double q_max_slip_angle = 30; // [deg] max slip angle - constexpr double lf_ratio = 0.3; // [-] ratio of front wheel position - constexpr double lf_min = 1.5; // [m] minimum front wheel position - constexpr double lr_ratio = 0.25; // [-] ratio of rear wheel position - constexpr double lr_min = 1.5; // [m] minimum rear wheel position - motion_model_.setMotionParams( - q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max, - q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio, - lr_min); - } - - // Set motion limits - { - constexpr double max_vel = tier4_autoware_utils::kmph2mps(100); // [m/s] maximum velocity - constexpr double max_slip = 30; // [deg] maximum slip angle - motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle - } } bool BigVehicleTracker::predict(const rclcpp::Time & time) @@ -260,8 +260,6 @@ bool BigVehicleTracker::measureWithPose( // current (predicted) state Eigen::MatrixXd X_t = motion_model_.getStateVector(); - // MOTION MODEL (update) - // velocity capability is checked only when the object has velocity measurement // and the predicted velocity is close to the observed velocity bool is_velocity_available = false; diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 4c2610a7b35ae..a832f9a024502 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -83,6 +83,35 @@ NormalVehicleTracker::NormalVehicleTracker( last_input_bounding_box_ = bounding_box_; } + // Set motion model parameters + { + constexpr double q_stddev_acc_long = + 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + constexpr double q_stddev_yaw_rate_min = 1.5; // [deg/s] uncertain yaw change rate, minimum + constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate, maximum + constexpr double q_stddev_slip_rate_min = + 0.3; // [deg/s] uncertain slip angle change rate, minimum + constexpr double q_stddev_slip_rate_max = + 10.0; // [deg/s] uncertain slip angle change rate, maximum + constexpr double q_max_slip_angle = 30; // [deg] max slip angle + constexpr double lf_ratio = 0.3; // [-] ratio of front wheel position + constexpr double lf_min = 1.0; // [m] minimum front wheel position + constexpr double lr_ratio = 0.25; // [-] ratio of rear wheel position + constexpr double lr_min = 1.0; // [m] minimum rear wheel position + motion_model_.setMotionParams( + q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max, + q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio, + lr_min); + } + + // Set motion limits + { + constexpr double max_vel = tier4_autoware_utils::kmph2mps(100); // [m/s] maximum velocity + constexpr double max_slip = 30; // [deg] maximum slip angle + motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle + } + // Set initial state { const double x = object.kinematics.pose_with_covariance.pose.position.x; @@ -134,35 +163,6 @@ NormalVehicleTracker::NormalVehicleTracker( // initialize motion model motion_model_.init(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); } - - // Set motion model parameters - { - constexpr double q_stddev_acc_long = - 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration - constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration - constexpr double q_stddev_yaw_rate_min = 1.5; // [deg/s] uncertain yaw change rate, minimum - constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate, maximum - constexpr double q_stddev_slip_rate_min = - 0.3; // [deg/s] uncertain slip angle change rate, minimum - constexpr double q_stddev_slip_rate_max = - 10.0; // [deg/s] uncertain slip angle change rate, maximum - constexpr double q_max_slip_angle = 30; // [deg] max slip angle - constexpr double lf_ratio = 0.3; // [-] ratio of front wheel position - constexpr double lf_min = 1.0; // [m] minimum front wheel position - constexpr double lr_ratio = 0.25; // [-] ratio of rear wheel position - constexpr double lr_min = 1.0; // [m] minimum rear wheel position - motion_model_.setMotionParams( - q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max, - q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio, - lr_min); - } - - // Set motion limits - { - constexpr double max_vel = tier4_autoware_utils::kmph2mps(100); // [m/s] maximum velocity - constexpr double max_slip = 30; // [deg] maximum slip angle - motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle - } } bool NormalVehicleTracker::predict(const rclcpp::Time & time) @@ -260,8 +260,6 @@ bool NormalVehicleTracker::measureWithPose( // current (predicted) state Eigen::MatrixXd X_t = motion_model_.getStateVector(); - // MOTION MODEL (update) - // velocity capability is checked only when the object has velocity measurement // and the predicted velocity is close to the observed velocity bool is_velocity_available = false; diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index fa9a794379f06..a536ecd17a3ad 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -71,6 +71,22 @@ PedestrianTracker::PedestrianTracker( cylinder_ = {object.shape.dimensions.x, object.shape.dimensions.z}; } + // Set motion model parameters + { + constexpr double q_stddev_x = 0.5; // [m/s] + constexpr double q_stddev_y = 0.5; // [m/s] + constexpr double q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] + constexpr double q_stddev_vx = 9.8 * 0.3; // [m/(s*s)] + constexpr double q_stddev_wz = tier4_autoware_utils::deg2rad(30); // [rad/(s*s)] + motion_model_.setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vx, q_stddev_wz); + } + + // Set motion limits + motion_model_.setMotionLimits( + tier4_autoware_utils::kmph2mps(100), /* [m/s] maximum velocity */ + 30.0 /* [deg/s] maximum turn rate */ + ); + // Set initial state { const double x = object.kinematics.pose_with_covariance.pose.position.x; @@ -124,22 +140,6 @@ PedestrianTracker::PedestrianTracker( // initialize motion model motion_model_.init(time, x, y, yaw, pose_cov, vel, vel_cov, wz, wz_cov); } - - // Set motion model parameters - { - constexpr double q_stddev_x = 0.5; // [m/s] - constexpr double q_stddev_y = 0.5; // [m/s] - constexpr double q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] - constexpr double q_stddev_vx = 9.8 * 0.3; // [m/(s*s)] - constexpr double q_stddev_wz = tier4_autoware_utils::deg2rad(30); // [rad/(s*s)] - motion_model_.setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vx, q_stddev_wz); - } - - // Set motion limits - motion_model_.setMotionLimits( - tier4_autoware_utils::kmph2mps(100), /* [m/s] maximum velocity */ - 30.0 /* [deg/s] maximum turn rate */ - ); } bool PedestrianTracker::predict(const rclcpp::Time & time) diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp index a582238628490..93f9aa1c3516d 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -53,6 +53,21 @@ UnknownTracker::UnknownTracker( ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); + // Set motion model parameters + { + constexpr double q_stddev_x = 0.5; // [m/s] + constexpr double q_stddev_y = 0.5; // [m/s] + constexpr double q_stddev_vx = 9.8 * 0.3; // [m/(s*s)] + constexpr double q_stddev_vy = 9.8 * 0.3; // [m/(s*s)] + motion_model_.setMotionParams(q_stddev_x, q_stddev_y, q_stddev_vx, q_stddev_vy); + } + + // Set motion limits + motion_model_.setMotionLimits( + tier4_autoware_utils::kmph2mps(60), /* [m/s] maximum velocity, x */ + tier4_autoware_utils::kmph2mps(60) /* [m/s] maximum velocity, y */ + ); + // Set initial state { const double x = object.kinematics.pose_with_covariance.pose.position.x; @@ -115,21 +130,6 @@ UnknownTracker::UnknownTracker( // initialize motion model motion_model_.init(time, x, y, pose_cov, vx, vy, twist_cov); } - - // Set motion model parameters - { - constexpr double q_stddev_x = 0.5; // [m/s] - constexpr double q_stddev_y = 0.5; // [m/s] - constexpr double q_stddev_vx = 9.8 * 0.3; // [m/(s*s)] - constexpr double q_stddev_vy = 9.8 * 0.3; // [m/(s*s)] - motion_model_.setMotionParams(q_stddev_x, q_stddev_y, q_stddev_vx, q_stddev_vy); - } - - // Set motion limits - motion_model_.setMotionLimits( - tier4_autoware_utils::kmph2mps(60), /* [m/s] maximum velocity, x */ - tier4_autoware_utils::kmph2mps(60) /* [m/s] maximum velocity, y */ - ); } bool UnknownTracker::predict(const rclcpp::Time & time) From 5fc6f933be6e67d67bb131e869ccb13428677cdd Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 7 Mar 2024 15:08:26 +0900 Subject: [PATCH 27/42] chore: fix init to initialize chore: fix init to initialize Signed-off-by: Taekjin LEE --- .../motion_model/bicycle_motion_model.hpp | 4 ++-- .../multi_object_tracker/motion_model/ctrv_motion_model.hpp | 4 ++-- .../multi_object_tracker/motion_model/cv_motion_model.hpp | 4 ++-- .../src/motion_model/bicycle_motion_model.cpp | 6 +++--- .../src/motion_model/ctrv_motion_model.cpp | 6 +++--- .../src/motion_model/cv_motion_model.cpp | 6 +++--- .../src/tracker/model/bicycle_tracker.cpp | 2 +- .../src/tracker/model/big_vehicle_tracker.cpp | 2 +- .../src/tracker/model/normal_vehicle_tracker.cpp | 2 +- .../src/tracker/model/pedestrian_tracker.cpp | 2 +- .../src/tracker/model/unknown_tracker.cpp | 2 +- 11 files changed, 20 insertions(+), 20 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp index b0ec2278cbf15..112c624118c68 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp @@ -72,10 +72,10 @@ class BicycleMotionModel enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; const char DIM = 5; - bool init( + bool initialize( const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P, const double & length); - bool init( + bool initialize( const rclcpp::Time & time, const double & x, const double & y, const double & yaw, const std::array & pose_cov, const double & vel, const double & vel_cov, const double & slip, const double & slip_cov, const double & length); diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp index a98e12da7921e..a745130f95f31 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp @@ -62,8 +62,8 @@ class CTRVMotionModel enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, WZ = 4 }; const char DIM = 5; - bool init(const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P); - bool init( + bool initialize(const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P); + bool initialize( const rclcpp::Time & time, const double & x, const double & y, const double & yaw, const std::array & pose_cov, const double & vel, const double & vel_cov, const double & wz, const double & wz_cov); diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp index 1269bdc4a1ec0..78c86a06bf171 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp @@ -61,8 +61,8 @@ class CVMotionModel enum IDX { X = 0, Y = 1, VX = 2, VY = 3 }; const char DIM = 4; - bool init(const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P); - bool init( + bool initialize(const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P); + bool initialize( const rclcpp::Time & time, const double & x, const double & y, const std::array & pose_cov, const double & vx, const double & vy, const std::array & twist_cov); diff --git a/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp index 4237dd2886705..5d63ad8d6834c 100644 --- a/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp @@ -104,7 +104,7 @@ void BicycleMotionModel::setMotionLimits(const double & max_vel, const double & motion_params_.max_slip = tier4_autoware_utils::deg2rad(max_slip); } -bool BicycleMotionModel::init( +bool BicycleMotionModel::initialize( const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P, const double & length) { @@ -123,7 +123,7 @@ bool BicycleMotionModel::init( return true; } -bool BicycleMotionModel::init( +bool BicycleMotionModel::initialize( const rclcpp::Time & time, const double & x, const double & y, const double & yaw, const std::array & pose_cov, const double & vel, const double & vel_cov, const double & slip, const double & slip_cov, const double & length) @@ -140,7 +140,7 @@ bool BicycleMotionModel::init( P(IDX::VEL, IDX::VEL) = vel_cov; P(IDX::SLIP, IDX::SLIP) = slip_cov; - return init(time, X, P, length); + return initialize(time, X, P, length); } bool BicycleMotionModel::updateStatePose( diff --git a/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp index 3d4c58292710a..d59ee518b2b74 100644 --- a/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp @@ -77,7 +77,7 @@ void CTRVMotionModel::setMotionLimits(const double & max_vel, const double & max motion_params_.max_wz = tier4_autoware_utils::deg2rad(max_wz); } -bool CTRVMotionModel::init( +bool CTRVMotionModel::initialize( const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P) { // set last update time @@ -92,7 +92,7 @@ bool CTRVMotionModel::init( return true; } -bool CTRVMotionModel::init( +bool CTRVMotionModel::initialize( const rclcpp::Time & time, const double & x, const double & y, const double & yaw, const std::array & pose_cov, const double & vel, const double & vel_cov, const double & wz, const double & wz_cov) @@ -109,7 +109,7 @@ bool CTRVMotionModel::init( P(IDX::VEL, IDX::VEL) = vel_cov; P(IDX::WZ, IDX::WZ) = wz_cov; - return init(time, X, P); + return initialize(time, X, P); } bool CTRVMotionModel::updateStatePose( diff --git a/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp index 8d7817c64b5c5..e7863643cf6f6 100644 --- a/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp @@ -76,7 +76,7 @@ void CVMotionModel::setMotionLimits(const double & max_vx, const double & max_vy motion_params_.max_vy = max_vy; } -bool CVMotionModel::init( +bool CVMotionModel::initialize( const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P) { // set last update time @@ -91,7 +91,7 @@ bool CVMotionModel::init( return true; } -bool CVMotionModel::init( +bool CVMotionModel::initialize( const rclcpp::Time & time, const double & x, const double & y, const std::array & pose_cov, const double & vx, const double & vy, const std::array & twist_cov) @@ -107,7 +107,7 @@ bool CVMotionModel::init( P(IDX::VX, IDX::VX) = twist_cov[utils::MSG_COV_IDX::X_X]; P(IDX::VY, IDX::VY) = twist_cov[utils::MSG_COV_IDX::Y_Y]; - return init(time, X, P); + return initialize(time, X, P); } bool CVMotionModel::updateStatePose( diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 63492682cc453..c2451a2b54b6c 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -139,7 +139,7 @@ BicycleTracker::BicycleTracker( const double slip_cov = std::pow(p0_stddev_slip, 2.0); // initialize motion model - motion_model_.init(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); + motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); } } diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 4fa1d671a896d..f58f0fd816d40 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -161,7 +161,7 @@ BigVehicleTracker::BigVehicleTracker( const double slip_cov = std::pow(p0_stddev_slip, 2.0); // initialize motion model - motion_model_.init(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); + motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); } } diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index a832f9a024502..59a327f6960ca 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -161,7 +161,7 @@ NormalVehicleTracker::NormalVehicleTracker( const double slip_cov = std::pow(p0_stddev_slip, 2.0); // initialize motion model - motion_model_.init(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); + motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); } } diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index a536ecd17a3ad..dbfb1c16059f4 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -138,7 +138,7 @@ PedestrianTracker::PedestrianTracker( } // initialize motion model - motion_model_.init(time, x, y, yaw, pose_cov, vel, vel_cov, wz, wz_cov); + motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, wz, wz_cov); } } diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp index 93f9aa1c3516d..e170062aa1542 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -128,7 +128,7 @@ UnknownTracker::UnknownTracker( twist_cov[utils::MSG_COV_IDX::Y_Y] = twist_cov_rotated(1, 1); // initialize motion model - motion_model_.init(time, x, y, pose_cov, vx, vy, twist_cov); + motion_model_.initialize(time, x, y, pose_cov, vx, vy, twist_cov); } } From 09d7189aa94f502875576cd41d20101caeb9766c Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 7 Mar 2024 17:15:16 +0900 Subject: [PATCH 28/42] feat: motion model base class Signed-off-by: Taekjin LEE --- .../multi_object_tracker/CMakeLists.txt | 1 + .../motion_model/bicycle_motion_model.hpp | 33 +++++------ .../motion_model/ctrv_motion_model.hpp | 29 ++++------ .../motion_model/cv_motion_model.hpp | 29 ++++------ .../motion_model/motion_model_base.hpp | 55 +++++++++++++++++++ .../src/motion_model/bicycle_motion_model.cpp | 35 ++++++------ .../src/motion_model/ctrv_motion_model.cpp | 28 +++++----- .../src/motion_model/cv_motion_model.cpp | 28 +++++----- .../src/motion_model/motion_model_base.cpp | 38 +++++++++++++ 9 files changed, 178 insertions(+), 98 deletions(-) create mode 100644 perception/multi_object_tracker/include/multi_object_tracker/motion_model/motion_model_base.hpp create mode 100644 perception/multi_object_tracker/src/motion_model/motion_model_base.cpp diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt index 911d0ceab0517..bd7ebaae54694 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/multi_object_tracker/CMakeLists.txt @@ -33,6 +33,7 @@ set(MULTI_OBJECT_TRACKER_SRC src/tracker/model/pass_through_tracker.cpp src/data_association/data_association.cpp src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp + src/motion_model/motion_model_base.cpp src/motion_model/bicycle_motion_model.cpp src/motion_model/ctrv_motion_model.cpp src/motion_model/cv_motion_model.cpp diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp index 112c624118c68..d75297c363483 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp @@ -19,6 +19,7 @@ #ifndef MULTI_OBJECT_TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ #define MULTI_OBJECT_TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ +#include "multi_object_tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/utils/utils.hpp" #include @@ -31,17 +32,11 @@ #include #endif -class BicycleMotionModel +class BicycleMotionModel : public MotionModel { private: // attributes rclcpp::Logger logger_; - rclcpp::Time last_update_time_; - -private: - // state - bool is_initialized_{false}; - KalmanFilter ekf_; // extended state double lf_; @@ -72,23 +67,23 @@ class BicycleMotionModel enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; const char DIM = 5; - bool initialize( - const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P, - const double & length); + // bool initialize( + // const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P, + // const double & length); bool initialize( const rclcpp::Time & time, const double & x, const double & y, const double & yaw, const std::array & pose_cov, const double & vel, const double & vel_cov, const double & slip, const double & slip_cov, const double & length); - bool checkInitialized() const - { - // if the state is not initialized, return false - if (!is_initialized_) { - RCLCPP_WARN(logger_, "BicycleMotionModel is not initialized."); - return false; - } - return true; - } + // bool checkInitialized() const + // { + // // if the state is not initialized, return false + // if (!is_initialized_) { + // RCLCPP_WARN(logger_, "BicycleMotionModel is not initialized."); + // return false; + // } + // return true; + // } void setDefaultParams(); diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp index a745130f95f31..44d80c39fd5d5 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp @@ -19,6 +19,7 @@ #ifndef MULTI_OBJECT_TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ #define MULTI_OBJECT_TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ +#include "multi_object_tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/utils/utils.hpp" #include @@ -31,17 +32,11 @@ #include #endif -class CTRVMotionModel +class CTRVMotionModel : public MotionModel { private: // attributes rclcpp::Logger logger_; - rclcpp::Time last_update_time_; - -private: - // state - bool is_initialized_{false}; - KalmanFilter ekf_; // motion parameters struct MotionParams @@ -62,21 +57,21 @@ class CTRVMotionModel enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, WZ = 4 }; const char DIM = 5; - bool initialize(const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P); + // bool initialize(const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P); bool initialize( const rclcpp::Time & time, const double & x, const double & y, const double & yaw, const std::array & pose_cov, const double & vel, const double & vel_cov, const double & wz, const double & wz_cov); - bool checkInitialized() const - { - // if the state is not initialized, return false - if (!is_initialized_) { - RCLCPP_WARN(logger_, "CTRVMotionModel is not initialized."); - return false; - } - return true; - } + // bool checkInitialized() const + // { + // // if the state is not initialized, return false + // if (!is_initialized_) { + // RCLCPP_WARN(logger_, "CTRVMotionModel is not initialized."); + // return false; + // } + // return true; + // } void setDefaultParams(); diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp index 78c86a06bf171..d0a8223a2dacb 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp @@ -19,6 +19,7 @@ #ifndef MULTI_OBJECT_TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ #define MULTI_OBJECT_TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ +#include "multi_object_tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/utils/utils.hpp" #include @@ -31,17 +32,11 @@ #include #endif -class CVMotionModel +class CVMotionModel : public MotionModel { private: // attributes rclcpp::Logger logger_; - rclcpp::Time last_update_time_; - -private: - // state - bool is_initialized_{false}; - KalmanFilter ekf_; // motion parameters struct MotionParams @@ -61,21 +56,21 @@ class CVMotionModel enum IDX { X = 0, Y = 1, VX = 2, VY = 3 }; const char DIM = 4; - bool initialize(const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P); + // bool initialize(const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P); bool initialize( const rclcpp::Time & time, const double & x, const double & y, const std::array & pose_cov, const double & vx, const double & vy, const std::array & twist_cov); - bool checkInitialized() const - { - // if the state is not initialized, return false - if (!is_initialized_) { - RCLCPP_WARN(logger_, "CVMotionModel is not initialized."); - return false; - } - return true; - } + // bool checkInitialized() const + // { + // // if the state is not initialized, return false + // if (!is_initialized_) { + // RCLCPP_WARN(logger_, "CVMotionModel is not initialized."); + // return false; + // } + // return true; + // } void setDefaultParams(); diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/motion_model_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/motion_model_base.hpp new file mode 100644 index 0000000000000..610e9c2f0fe4e --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/motion_model_base.hpp @@ -0,0 +1,55 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Taekjin Lee +// + +#ifndef MULTI_OBJECT_TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ +#define MULTI_OBJECT_TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ + +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +class MotionModel +{ +protected: + rclcpp::Time last_update_time_; + bool is_initialized_{false}; + KalmanFilter ekf_; + +public: + MotionModel(); + virtual ~MotionModel() = default; + + bool checkInitialized() const { return is_initialized_; } + + bool initialize(const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P); + + // virtual void initialize(const rclcpp::Time & time, + // const Eigen::VectorXd & state, const Eigen::MatrixXd & covariance) = 0; + + // virtual void predict(const rclcpp::Time & time) = 0; +}; + +#endif // MULTI_OBJECT_TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ diff --git a/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp index 5d63ad8d6834c..c45f44cb4cacc 100644 --- a/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp @@ -16,6 +16,7 @@ // Author: v1.0 Taekjin Lee // +#include "multi_object_tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/motion_model/bicycle_motion_model.hpp" #include "multi_object_tracker/utils/utils.hpp" @@ -31,7 +32,7 @@ // CTRV : Constant Turn Rate and constant Velocity BicycleMotionModel::BicycleMotionModel() -: logger_(rclcpp::get_logger("BicycleMotionModel")), last_update_time_(rclcpp::Time(0, 0)) +: MotionModel(), logger_(rclcpp::get_logger("BicycleMotionModel")) { // Initialize motion parameters setDefaultParams(); @@ -104,24 +105,21 @@ void BicycleMotionModel::setMotionLimits(const double & max_vel, const double & motion_params_.max_slip = tier4_autoware_utils::deg2rad(max_slip); } -bool BicycleMotionModel::initialize( - const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P, - const double & length) -{ - // set last update time - last_update_time_ = time; +// bool BicycleMotionModel::initialize( +// const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P, +// const double & length) +// { +// // set last update time +// last_update_time_ = time; - // initialize Kalman filter - if (!ekf_.init(X, P)) return false; +// // initialize Kalman filter +// if (!ekf_.init(X, P)) return false; - // set initial extended state - if (!updateExtendedState(length)) return false; +// // set initialized flag +// is_initialized_ = true; - // set initialized flag - is_initialized_ = true; - - return true; -} +// return true; +// } bool BicycleMotionModel::initialize( const rclcpp::Time & time, const double & x, const double & y, const double & yaw, @@ -140,7 +138,10 @@ bool BicycleMotionModel::initialize( P(IDX::VEL, IDX::VEL) = vel_cov; P(IDX::SLIP, IDX::SLIP) = slip_cov; - return initialize(time, X, P, length); + // set initial extended state + if (!updateExtendedState(length)) return false; + + return MotionModel::initialize(time, X, P); } bool BicycleMotionModel::updateStatePose( diff --git a/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp index d59ee518b2b74..394c2564a9f58 100644 --- a/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp @@ -16,6 +16,7 @@ // Author: v1.0 Taekjin Lee // +#include "multi_object_tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/motion_model/ctrv_motion_model.hpp" #include "multi_object_tracker/utils/utils.hpp" @@ -30,8 +31,7 @@ // cspell: ignore CTRV // Constant Turn Rate and constant Velocity (CTRV) motion model -CTRVMotionModel::CTRVMotionModel() -: logger_(rclcpp::get_logger("CTRVMotionModel")), last_update_time_(rclcpp::Time(0, 0)) +CTRVMotionModel::CTRVMotionModel() : MotionModel(), logger_(rclcpp::get_logger("CTRVMotionModel")) { // Initialize motion parameters setDefaultParams(); @@ -77,20 +77,20 @@ void CTRVMotionModel::setMotionLimits(const double & max_vel, const double & max motion_params_.max_wz = tier4_autoware_utils::deg2rad(max_wz); } -bool CTRVMotionModel::initialize( - const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P) -{ - // set last update time - last_update_time_ = time; +// bool CTRVMotionModel::initialize( +// const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P) +// { +// // set last update time +// last_update_time_ = time; - // initialize Kalman filter - if (!ekf_.init(X, P)) return false; +// // initialize Kalman filter +// if (!ekf_.init(X, P)) return false; - // set initialized flag - is_initialized_ = true; +// // set initialized flag +// is_initialized_ = true; - return true; -} +// return true; +// } bool CTRVMotionModel::initialize( const rclcpp::Time & time, const double & x, const double & y, const double & yaw, @@ -109,7 +109,7 @@ bool CTRVMotionModel::initialize( P(IDX::VEL, IDX::VEL) = vel_cov; P(IDX::WZ, IDX::WZ) = wz_cov; - return initialize(time, X, P); + return MotionModel::initialize(time, X, P); } bool CTRVMotionModel::updateStatePose( diff --git a/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp index e7863643cf6f6..e41f5fc31d124 100644 --- a/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp @@ -16,6 +16,7 @@ // Author: v1.0 Taekjin Lee // +#include "multi_object_tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/motion_model/cv_motion_model.hpp" #include "multi_object_tracker/utils/utils.hpp" @@ -32,8 +33,7 @@ // cspell: ignore CV // Constant Velocity (CV) motion model -CVMotionModel::CVMotionModel() -: logger_(rclcpp::get_logger("CVMotionModel")), last_update_time_(rclcpp::Time(0, 0)) +CVMotionModel::CVMotionModel() : MotionModel(), logger_(rclcpp::get_logger("CVMotionModel")) { // Initialize motion parameters setDefaultParams(); @@ -76,20 +76,20 @@ void CVMotionModel::setMotionLimits(const double & max_vx, const double & max_vy motion_params_.max_vy = max_vy; } -bool CVMotionModel::initialize( - const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P) -{ - // set last update time - last_update_time_ = time; +// bool CVMotionModel::initialize( +// const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P) +// { +// // set last update time +// last_update_time_ = time; - // initialize Kalman filter - if (!ekf_.init(X, P)) return false; +// // initialize Kalman filter +// if (!ekf_.init(X, P)) return false; - // set initialized flag - is_initialized_ = true; +// // set initialized flag +// is_initialized_ = true; - return true; -} +// return true; +// } bool CVMotionModel::initialize( const rclcpp::Time & time, const double & x, const double & y, @@ -107,7 +107,7 @@ bool CVMotionModel::initialize( P(IDX::VX, IDX::VX) = twist_cov[utils::MSG_COV_IDX::X_X]; P(IDX::VY, IDX::VY) = twist_cov[utils::MSG_COV_IDX::Y_Y]; - return initialize(time, X, P); + return MotionModel::initialize(time, X, P); } bool CVMotionModel::updateStatePose( diff --git a/perception/multi_object_tracker/src/motion_model/motion_model_base.cpp b/perception/multi_object_tracker/src/motion_model/motion_model_base.cpp new file mode 100644 index 0000000000000..4e5df506f99b5 --- /dev/null +++ b/perception/multi_object_tracker/src/motion_model/motion_model_base.cpp @@ -0,0 +1,38 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Taekjin Lee +// + +#include "multi_object_tracker/motion_model/motion_model_base.hpp" + +MotionModel::MotionModel() : last_update_time_(rclcpp::Time(0, 0)) +{ +} + +bool MotionModel::initialize(const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P) +{ + // initialize Kalman filter + if (!ekf_.init(X, P)) return false; + + // set last update time + last_update_time_ = time; + + // set initialized flag + is_initialized_ = true; + + return true; +} + From ca4c32cb3427b22fb1fc41764f46bbcee29992e9 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 7 Mar 2024 17:28:14 +0900 Subject: [PATCH 29/42] feat: common methods to the base class Signed-off-by: Taekjin LEE --- .../motion_model/bicycle_motion_model.hpp | 20 +---- .../motion_model/ctrv_motion_model.hpp | 18 +---- .../motion_model/cv_motion_model.hpp | 18 +---- .../motion_model/motion_model_base.hpp | 13 ++- .../src/motion_model/bicycle_motion_model.cpp | 80 +------------------ .../src/motion_model/ctrv_motion_model.cpp | 79 +----------------- .../src/motion_model/cv_motion_model.cpp | 79 +----------------- .../src/motion_model/motion_model_base.cpp | 57 ++++++++++++- 8 files changed, 80 insertions(+), 284 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp index d75297c363483..68e7447d90e4b 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp @@ -58,7 +58,6 @@ class BicycleMotionModel : public MotionModel double lr_min; double max_vel; double max_slip; - double dt_max; } motion_params_; public: @@ -67,24 +66,11 @@ class BicycleMotionModel : public MotionModel enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; const char DIM = 5; - // bool initialize( - // const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P, - // const double & length); bool initialize( const rclcpp::Time & time, const double & x, const double & y, const double & yaw, const std::array & pose_cov, const double & vel, const double & vel_cov, const double & slip, const double & slip_cov, const double & length); - // bool checkInitialized() const - // { - // // if the state is not initialized, return false - // if (!is_initialized_) { - // RCLCPP_WARN(logger_, "BicycleMotionModel is not initialized."); - // return false; - // } - // return true; - // } - void setDefaultParams(); void setMotionParams( @@ -121,11 +107,7 @@ class BicycleMotionModel : public MotionModel bool updateExtendedState(const double & length); - bool predictState(const rclcpp::Time & time); - - bool predictState(const double dt, KalmanFilter & ekf) const; - - bool getPredictedState(const rclcpp::Time & time, Eigen::MatrixXd & X, Eigen::MatrixXd & P) const; + bool predictStateStep(const double dt, KalmanFilter & ekf) const override; bool getPredictedState( const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp index 44d80c39fd5d5..3915c4cc71dc7 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp @@ -48,7 +48,6 @@ class CTRVMotionModel : public MotionModel double q_cov_wz; double max_vel; double max_wz; - double dt_max; } motion_params_; public: @@ -57,22 +56,11 @@ class CTRVMotionModel : public MotionModel enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, WZ = 4 }; const char DIM = 5; - // bool initialize(const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P); bool initialize( const rclcpp::Time & time, const double & x, const double & y, const double & yaw, const std::array & pose_cov, const double & vel, const double & vel_cov, const double & wz, const double & wz_cov); - // bool checkInitialized() const - // { - // // if the state is not initialized, return false - // if (!is_initialized_) { - // RCLCPP_WARN(logger_, "CTRVMotionModel is not initialized."); - // return false; - // } - // return true; - // } - void setDefaultParams(); void setMotionParams( @@ -104,11 +92,7 @@ class CTRVMotionModel : public MotionModel bool limitStates(); - bool predictState(const rclcpp::Time & time); - - bool predictState(const double dt, KalmanFilter & ekf) const; - - bool getPredictedState(const rclcpp::Time & time, Eigen::MatrixXd & X, Eigen::MatrixXd & P) const; + bool predictStateStep(const double dt, KalmanFilter & ekf) const override; bool getPredictedState( const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp index d0a8223a2dacb..0495dc01af450 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp @@ -47,7 +47,6 @@ class CVMotionModel : public MotionModel double q_cov_vy; double max_vx; double max_vy; - double dt_max; } motion_params_; public: @@ -56,22 +55,11 @@ class CVMotionModel : public MotionModel enum IDX { X = 0, Y = 1, VX = 2, VY = 3 }; const char DIM = 4; - // bool initialize(const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P); bool initialize( const rclcpp::Time & time, const double & x, const double & y, const std::array & pose_cov, const double & vx, const double & vy, const std::array & twist_cov); - // bool checkInitialized() const - // { - // // if the state is not initialized, return false - // if (!is_initialized_) { - // RCLCPP_WARN(logger_, "CVMotionModel is not initialized."); - // return false; - // } - // return true; - // } - void setDefaultParams(); void setMotionParams( @@ -99,11 +87,7 @@ class CVMotionModel : public MotionModel bool limitStates(); - bool predictState(const rclcpp::Time & time); - - bool predictState(const double dt, KalmanFilter & ekf) const; - - bool getPredictedState(const rclcpp::Time & time, Eigen::MatrixXd & X, Eigen::MatrixXd & P) const; + bool predictStateStep(const double dt, KalmanFilter & ekf) const override; bool getPredictedState( const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/motion_model_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/motion_model_base.hpp index 610e9c2f0fe4e..2693c1bb36d42 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/motion_model_base.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/motion_model_base.hpp @@ -33,9 +33,12 @@ class MotionModel { +private: + bool is_initialized_{false}; + double dt_max_{0.11}; // [s] maximum time interval for prediction + protected: rclcpp::Time last_update_time_; - bool is_initialized_{false}; KalmanFilter ekf_; public: @@ -43,13 +46,15 @@ class MotionModel virtual ~MotionModel() = default; bool checkInitialized() const { return is_initialized_; } + double getDeltaTime(rclcpp::Time time) const { return (time - last_update_time_).seconds(); } + void setMaxDeltaTime(const double dt_max) { dt_max_ = dt_max; } bool initialize(const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P); - // virtual void initialize(const rclcpp::Time & time, - // const Eigen::VectorXd & state, const Eigen::MatrixXd & covariance) = 0; + bool predictState(const rclcpp::Time & time); + bool getPredictedState(const rclcpp::Time & time, Eigen::MatrixXd & X, Eigen::MatrixXd & P) const; - // virtual void predict(const rclcpp::Time & time) = 0; + virtual bool predictStateStep(const double dt, KalmanFilter & ekf) const = 0; }; #endif // MULTI_OBJECT_TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ diff --git a/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp index c45f44cb4cacc..bdafac51afd06 100644 --- a/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp @@ -16,9 +16,9 @@ // Author: v1.0 Taekjin Lee // -#include "multi_object_tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/motion_model/bicycle_motion_model.hpp" +#include "multi_object_tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/utils/utils.hpp" #include @@ -65,7 +65,7 @@ void BicycleMotionModel::setDefaultParams() // set prediction parameters constexpr double dt_max = 0.11; // [s] maximum time interval for prediction - motion_params_.dt_max = dt_max; + setMaxDeltaTime(dt_max); } void BicycleMotionModel::setMotionParams( @@ -105,22 +105,6 @@ void BicycleMotionModel::setMotionLimits(const double & max_vel, const double & motion_params_.max_slip = tier4_autoware_utils::deg2rad(max_slip); } -// bool BicycleMotionModel::initialize( -// const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P, -// const double & length) -// { -// // set last update time -// last_update_time_ = time; - -// // initialize Kalman filter -// if (!ekf_.init(X, P)) return false; - -// // set initialized flag -// is_initialized_ = true; - -// return true; -// } - bool BicycleMotionModel::initialize( const rclcpp::Time & time, const double & x, const double & y, const double & yaw, const std::array & pose_cov, const double & vel, const double & vel_cov, @@ -303,32 +287,7 @@ bool BicycleMotionModel::adjustPosition(const double & x, const double & y) return true; } -bool BicycleMotionModel::predictState(const rclcpp::Time & time) -{ - // check if the state is initialized - if (!checkInitialized()) return false; - - const double dt = (time - last_update_time_).seconds(); - if (dt < 0.0) { - RCLCPP_WARN(logger_, "BicycleMotionModel::predictState: dt is negative. (%f)", dt); - return false; - } - // if dt is too large, shorten dt and repeat prediction - const uint32_t repeat = std::ceil(dt / motion_params_.dt_max); - const double dt_ = dt / repeat; - for (uint32_t i = 0; i < repeat; ++i) { - if (!predictState(dt_, ekf_)) { - return false; - } - // add interval to last_update_time_ - last_update_time_ += rclcpp::Duration::from_seconds(dt_); - } - // update last_update_time_ to the estimation time - last_update_time_ = time; - return true; -} - -bool BicycleMotionModel::predictState(const double dt, KalmanFilter & ekf) const +bool BicycleMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) const { /* Motion model: static bicycle model (constant slip angle, constant velocity) * @@ -445,37 +404,6 @@ bool BicycleMotionModel::predictState(const double dt, KalmanFilter & ekf) const return ekf.predict(X_next_t, A, Q); } -bool BicycleMotionModel::getPredictedState( - const rclcpp::Time & time, Eigen::MatrixXd & X, Eigen::MatrixXd & P) const -{ - // 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 = (time - last_update_time_).seconds(); - if (dt < 0.0) { - RCLCPP_WARN(logger_, "BicycleMotionModel::getPredictedState: dt is negative. (%f)", dt); - 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 / motion_params_.dt_max); - const double dt_ = dt / repeat; - for (uint32_t i = 0; i < repeat; ++i) { - if (!predictState(dt_, tmp_ekf_for_no_update)) { - return false; - } - } - } - tmp_ekf_for_no_update.getX(X); - tmp_ekf_for_no_update.getP(P); - return true; -} - bool BicycleMotionModel::getPredictedState( const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, geometry_msgs::msg::Twist & twist, std::array & twist_cov) const @@ -483,7 +411,7 @@ bool BicycleMotionModel::getPredictedState( // get predicted state Eigen::MatrixXd X(DIM, 1); Eigen::MatrixXd P(DIM, DIM); - if (!getPredictedState(time, X, P)) { + if (!MotionModel::getPredictedState(time, X, P)) { return false; } diff --git a/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp index 394c2564a9f58..3880941838136 100644 --- a/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp @@ -16,9 +16,9 @@ // Author: v1.0 Taekjin Lee // -#include "multi_object_tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/motion_model/ctrv_motion_model.hpp" +#include "multi_object_tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/utils/utils.hpp" #include @@ -55,7 +55,7 @@ void CTRVMotionModel::setDefaultParams() // set prediction parameters constexpr double dt_max = 0.11; // [s] maximum time interval for prediction - motion_params_.dt_max = dt_max; + setMaxDeltaTime(dt_max); } void CTRVMotionModel::setMotionParams( @@ -77,21 +77,6 @@ void CTRVMotionModel::setMotionLimits(const double & max_vel, const double & max motion_params_.max_wz = tier4_autoware_utils::deg2rad(max_wz); } -// bool CTRVMotionModel::initialize( -// const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P) -// { -// // set last update time -// last_update_time_ = time; - -// // initialize Kalman filter -// if (!ekf_.init(X, P)) return false; - -// // set initialized flag -// is_initialized_ = true; - -// return true; -// } - bool CTRVMotionModel::initialize( const rclcpp::Time & time, const double & x, const double & y, const double & yaw, const std::array & pose_cov, const double & vel, const double & vel_cov, @@ -264,32 +249,7 @@ bool CTRVMotionModel::adjustPosition(const double & x, const double & y) return true; } -bool CTRVMotionModel::predictState(const rclcpp::Time & time) -{ - // check if the state is initialized - if (!checkInitialized()) return false; - - const double dt = (time - last_update_time_).seconds(); - if (dt < 0.0) { - RCLCPP_WARN(logger_, "CTRVMotionModel::predictState: dt is negative. (%f)", dt); - return false; - } - // if dt is too large, shorten dt and repeat prediction - const uint32_t repeat = std::ceil(dt / motion_params_.dt_max); - const double dt_ = dt / repeat; - for (uint32_t i = 0; i < repeat; ++i) { - if (!predictState(dt_, ekf_)) { - return false; - } - // add interval to last_update_time_ - last_update_time_ += rclcpp::Duration::from_seconds(dt_); - } - // update last_update_time_ to the estimation time - last_update_time_ = time; - return true; -} - -bool CTRVMotionModel::predictState(const double dt, KalmanFilter & ekf) const +bool CTRVMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) const { /* Motion model: Constant Turn Rate and constant Velocity model (CTRV) * @@ -358,37 +318,6 @@ bool CTRVMotionModel::predictState(const double dt, KalmanFilter & ekf) const return ekf.predict(X_next_t, A, Q); } -bool CTRVMotionModel::getPredictedState( - const rclcpp::Time & time, Eigen::MatrixXd & X, Eigen::MatrixXd & P) const -{ - // 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 = (time - last_update_time_).seconds(); - if (dt < 0.0) { - RCLCPP_WARN(logger_, "CTRVMotionModel::getPredictedState: dt is negative. (%f)", dt); - 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 / motion_params_.dt_max); - const double dt_ = dt / repeat; - for (uint32_t i = 0; i < repeat; ++i) { - if (!predictState(dt_, tmp_ekf_for_no_update)) { - return false; - } - } - } - tmp_ekf_for_no_update.getX(X); - tmp_ekf_for_no_update.getP(P); - return true; -} - bool CTRVMotionModel::getPredictedState( const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, geometry_msgs::msg::Twist & twist, std::array & twist_cov) const @@ -396,7 +325,7 @@ bool CTRVMotionModel::getPredictedState( // get predicted state Eigen::MatrixXd X(DIM, 1); Eigen::MatrixXd P(DIM, DIM); - if (!getPredictedState(time, X, P)) { + if (!MotionModel::getPredictedState(time, X, P)) { return false; } diff --git a/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp index e41f5fc31d124..e35348b6844f1 100644 --- a/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp @@ -16,9 +16,9 @@ // Author: v1.0 Taekjin Lee // -#include "multi_object_tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/motion_model/cv_motion_model.hpp" +#include "multi_object_tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/utils/utils.hpp" #include @@ -55,7 +55,7 @@ void CVMotionModel::setDefaultParams() // set prediction parameters constexpr double dt_max = 0.11; // [s] maximum time interval for prediction - motion_params_.dt_max = dt_max; + setMaxDeltaTime(dt_max); } void CVMotionModel::setMotionParams( @@ -76,21 +76,6 @@ void CVMotionModel::setMotionLimits(const double & max_vx, const double & max_vy motion_params_.max_vy = max_vy; } -// bool CVMotionModel::initialize( -// const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P) -// { -// // set last update time -// last_update_time_ = time; - -// // initialize Kalman filter -// if (!ekf_.init(X, P)) return false; - -// // set initialized flag -// is_initialized_ = true; - -// return true; -// } - bool CVMotionModel::initialize( const rclcpp::Time & time, const double & x, const double & y, const std::array & pose_cov, const double & vx, const double & vy, @@ -203,32 +188,7 @@ bool CVMotionModel::adjustPosition(const double & x, const double & y) return true; } -bool CVMotionModel::predictState(const rclcpp::Time & time) -{ - // check if the state is initialized - if (!checkInitialized()) return false; - - const double dt = (time - last_update_time_).seconds(); - if (dt < 0.0) { - RCLCPP_WARN(logger_, "CVMotionModel::predictState: dt is negative. (%f)", dt); - return false; - } - // if dt is too large, shorten dt and repeat prediction - const uint32_t repeat = std::ceil(dt / motion_params_.dt_max); - const double dt_ = dt / repeat; - for (uint32_t i = 0; i < repeat; ++i) { - if (!predictState(dt_, ekf_)) { - return false; - } - // add interval to last_update_time_ - last_update_time_ += rclcpp::Duration::from_seconds(dt_); - } - // update last_update_time_ to the estimation time - last_update_time_ = time; - return true; -} - -bool CVMotionModel::predictState(const double dt, KalmanFilter & ekf) const +bool CVMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) const { /* Motion model: Constant velocity model * @@ -279,37 +239,6 @@ bool CVMotionModel::predictState(const double dt, KalmanFilter & ekf) const return ekf.predict(X_next_t, A, Q); } -bool CVMotionModel::getPredictedState( - const rclcpp::Time & time, Eigen::MatrixXd & X, Eigen::MatrixXd & P) const -{ - // 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 = (time - last_update_time_).seconds(); - if (dt < 0.0) { - RCLCPP_WARN(logger_, "CVMotionModel::getPredictedState: dt is negative. (%f)", dt); - 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 / motion_params_.dt_max); - const double dt_ = dt / repeat; - for (uint32_t i = 0; i < repeat; ++i) { - if (!predictState(dt_, tmp_ekf_for_no_update)) { - return false; - } - } - } - tmp_ekf_for_no_update.getX(X); - tmp_ekf_for_no_update.getP(P); - return true; -} - bool CVMotionModel::getPredictedState( const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, geometry_msgs::msg::Twist & twist, std::array & twist_cov) const @@ -317,7 +246,7 @@ bool CVMotionModel::getPredictedState( // get predicted state Eigen::MatrixXd X(DIM, 1); Eigen::MatrixXd P(DIM, DIM); - if (!getPredictedState(time, X, P)) { + if (!MotionModel::getPredictedState(time, X, P)) { return false; } diff --git a/perception/multi_object_tracker/src/motion_model/motion_model_base.cpp b/perception/multi_object_tracker/src/motion_model/motion_model_base.cpp index 4e5df506f99b5..6a945283e1a8c 100644 --- a/perception/multi_object_tracker/src/motion_model/motion_model_base.cpp +++ b/perception/multi_object_tracker/src/motion_model/motion_model_base.cpp @@ -22,7 +22,8 @@ MotionModel::MotionModel() : last_update_time_(rclcpp::Time(0, 0)) { } -bool MotionModel::initialize(const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P) +bool MotionModel::initialize( + const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P) { // initialize Kalman filter if (!ekf_.init(X, P)) return false; @@ -36,3 +37,57 @@ bool MotionModel::initialize(const rclcpp::Time & time, const Eigen::MatrixXd & return true; } +bool MotionModel::predictState(const rclcpp::Time & time) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + const double dt = getDeltaTime(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_); + } + // update last_update_time_ to the estimation time + last_update_time_ = time; + return true; +} + +bool MotionModel::getPredictedState( + const rclcpp::Time & time, Eigen::MatrixXd & X, Eigen::MatrixXd & P) const +{ + // 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; + } + + // 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; + } + } + } + tmp_ekf_for_no_update.getX(X); + tmp_ekf_for_no_update.getP(P); + return true; +} From 76118a5eda3bd4323ccff42fbe63b411e2c5093c Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 7 Mar 2024 18:02:02 +0900 Subject: [PATCH 30/42] fix: access to state vector Signed-off-by: Taekjin LEE --- .../motion_model/bicycle_motion_model.hpp | 11 +---------- .../motion_model/ctrv_motion_model.hpp | 11 +---------- .../motion_model/cv_motion_model.hpp | 11 +---------- .../motion_model/motion_model_base.hpp | 5 +++++ .../src/motion_model/bicycle_motion_model.cpp | 3 ++- .../src/motion_model/ctrv_motion_model.cpp | 3 ++- .../src/motion_model/cv_motion_model.cpp | 3 ++- .../src/tracker/model/bicycle_tracker.cpp | 6 ++---- .../src/tracker/model/big_vehicle_tracker.cpp | 14 +++++++------- .../src/tracker/model/normal_vehicle_tracker.cpp | 14 +++++++------- 10 files changed, 30 insertions(+), 51 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp index 68e7447d90e4b..727bf62d016fe 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp @@ -82,15 +82,6 @@ class BicycleMotionModel : public MotionModel void setMotionLimits(const double & max_vel, const double & max_slip); - Eigen::MatrixXd getStateVector() const - { - Eigen::MatrixXd X_t(DIM, 1); - ekf_.getX(X_t); - return X_t; - } - - double getStateElement(unsigned int idx) const { return ekf_.getXelement(idx); } - bool updateStatePose(const double & x, const double & y, const std::array & pose_cov); bool updateStatePoseHead( @@ -111,7 +102,7 @@ class BicycleMotionModel : public MotionModel bool getPredictedState( const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, - geometry_msgs::msg::Twist & twist, std::array & twist_cov) const; + geometry_msgs::msg::Twist & twist, std::array & twist_cov) const override; }; #endif // MULTI_OBJECT_TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp index 3915c4cc71dc7..78eda6894e356 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp @@ -69,15 +69,6 @@ class CTRVMotionModel : public MotionModel void setMotionLimits(const double & max_vel, const double & max_wz); - Eigen::MatrixXd getStateVector() const - { - Eigen::MatrixXd X_t(DIM, 1); - ekf_.getX(X_t); - return X_t; - } - - double getStateElement(unsigned int idx) const { return ekf_.getXelement(idx); } - bool updateStatePose(const double & x, const double & y, const std::array & pose_cov); bool updateStatePoseHead( @@ -96,7 +87,7 @@ class CTRVMotionModel : public MotionModel bool getPredictedState( const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, - geometry_msgs::msg::Twist & twist, std::array & twist_cov) const; + geometry_msgs::msg::Twist & twist, std::array & twist_cov) const override; }; #endif // MULTI_OBJECT_TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp index 0495dc01af450..ae8b8c6cb5f07 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp @@ -68,15 +68,6 @@ class CVMotionModel : public MotionModel void setMotionLimits(const double & max_vx, const double & max_vy); - Eigen::MatrixXd getStateVector() const - { - Eigen::MatrixXd X_t(DIM, 1); - ekf_.getX(X_t); - return X_t; - } - - double getStateElement(unsigned int idx) const { return ekf_.getXelement(idx); } - bool updateStatePose(const double & x, const double & y, const std::array & pose_cov); bool updateStatePoseVel( @@ -91,7 +82,7 @@ class CVMotionModel : public MotionModel bool getPredictedState( const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, - geometry_msgs::msg::Twist & twist, std::array & twist_cov) const; + geometry_msgs::msg::Twist & twist, std::array & twist_cov) const override; }; #endif // MULTI_OBJECT_TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/motion_model_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/motion_model_base.hpp index 2693c1bb36d42..35fc0bffc2742 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/motion_model_base.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/motion_model_base.hpp @@ -48,6 +48,8 @@ class MotionModel bool checkInitialized() const { return is_initialized_; } double getDeltaTime(rclcpp::Time time) const { return (time - last_update_time_).seconds(); } void setMaxDeltaTime(const double dt_max) { dt_max_ = dt_max; } + double getStateElement(unsigned int idx) const { return ekf_.getXelement(idx); } + void getStateVector(Eigen::MatrixXd & X) const { ekf_.getX(X); } bool initialize(const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P); @@ -55,6 +57,9 @@ class MotionModel bool getPredictedState(const rclcpp::Time & time, Eigen::MatrixXd & X, Eigen::MatrixXd & P) const; virtual bool predictStateStep(const double dt, KalmanFilter & ekf) const = 0; + virtual bool getPredictedState( + const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, + geometry_msgs::msg::Twist & twist, std::array & twist_cov) const = 0; }; #endif // MULTI_OBJECT_TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ diff --git a/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp index bdafac51afd06..10afdce14116b 100644 --- a/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp @@ -315,7 +315,8 @@ bool BicycleMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) c */ // Current state vector X t - Eigen::MatrixXd X_t = getStateVector(); + Eigen::MatrixXd X_t(DIM, 1); + getStateVector(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/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp index 3880941838136..6284a4b8e3980 100644 --- a/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp @@ -271,7 +271,8 @@ bool CTRVMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) cons */ // Current state vector X t - Eigen::MatrixXd X_t = getStateVector(); + Eigen::MatrixXd X_t(DIM, 1); + getStateVector(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/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp index e35348b6844f1..be5d4a20a45a1 100644 --- a/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp @@ -208,7 +208,8 @@ bool CVMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) const */ // Current state vector X t - Eigen::MatrixXd X_t = getStateVector(); + Eigen::MatrixXd X_t(DIM, 1); + getStateVector(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/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index c2451a2b54b6c..0977c2e10304f 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -184,12 +184,10 @@ autoware_auto_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingOb bool BicycleTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - // current (predicted) state - Eigen::MatrixXd X_t = motion_model_.getStateVector(); - // get measurement yaw angle to update + const double tracked_yaw = motion_model_.getStateElement(IDX::YAW); double measurement_yaw = 0.0; - bool is_yaw_available = utils::getMeasurementYaw(object, X_t(IDX::YAW), measurement_yaw); + bool is_yaw_available = utils::getMeasurementYaw(object, tracked_yaw, measurement_yaw); // update bool is_updated = false; diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index f58f0fd816d40..5f18dd743af6d 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -182,7 +182,9 @@ autoware_auto_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatin autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; // current (predicted) state - Eigen::MatrixXd X_t = motion_model_.getStateVector(); + const double tracked_x = motion_model_.getStateElement(IDX::X); + const double tracked_y = motion_model_.getStateElement(IDX::Y); + const double tracked_yaw = motion_model_.getStateElement(IDX::YAW); // OBJECT SHAPE MODEL // convert to bounding box if input is convex shape @@ -195,11 +197,10 @@ autoware_auto_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatin // get offset measurement int nearest_corner_index = utils::getNearestCornerOrSurface( - X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, - self_transform); + tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform); utils::calcAnchorPointOffset( last_input_bounding_box_.width, last_input_bounding_box_.length, nearest_corner_index, - bbox_object, X_t(IDX::YAW), updating_object, tracking_offset_); + bbox_object, tracked_yaw, updating_object, tracking_offset_); // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { @@ -258,15 +259,14 @@ bool BigVehicleTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { // current (predicted) state - Eigen::MatrixXd X_t = motion_model_.getStateVector(); + const double tracked_vel = motion_model_.getStateElement(IDX::VEL); // velocity capability is checked only when the object has velocity measurement // and the predicted velocity is close to the observed velocity bool is_velocity_available = false; if (object.kinematics.has_twist) { - const double & predicted_vel = X_t(IDX::VEL); const double & observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; - if (std::fabs(predicted_vel - observed_vel) < velocity_deviation_threshold_) { + if (std::fabs(tracked_vel - observed_vel) < velocity_deviation_threshold_) { // Velocity deviation is small is_velocity_available = true; } diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 59a327f6960ca..47fc44f56297a 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -182,7 +182,9 @@ autoware_auto_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpda autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; // current (predicted) state - Eigen::MatrixXd X_t = motion_model_.getStateVector(); + const double tracked_x = motion_model_.getStateElement(IDX::X); + const double tracked_y = motion_model_.getStateElement(IDX::Y); + const double tracked_yaw = motion_model_.getStateElement(IDX::YAW); // OBJECT SHAPE MODEL // convert to bounding box if input is convex shape @@ -195,11 +197,10 @@ autoware_auto_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpda // get offset measurement int nearest_corner_index = utils::getNearestCornerOrSurface( - X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, - self_transform); + tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform); utils::calcAnchorPointOffset( last_input_bounding_box_.width, last_input_bounding_box_.length, nearest_corner_index, - bbox_object, X_t(IDX::YAW), updating_object, tracking_offset_); + bbox_object, tracked_yaw, updating_object, tracking_offset_); // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { @@ -258,15 +259,14 @@ bool NormalVehicleTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { // current (predicted) state - Eigen::MatrixXd X_t = motion_model_.getStateVector(); + const double tracked_vel = motion_model_.getStateElement(IDX::VEL); // velocity capability is checked only when the object has velocity measurement // and the predicted velocity is close to the observed velocity bool is_velocity_available = false; if (object.kinematics.has_twist) { - const double & predicted_vel = X_t(IDX::VEL); const double & observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; - if (std::fabs(predicted_vel - observed_vel) < velocity_deviation_threshold_) { + if (std::fabs(tracked_vel - observed_vel) < velocity_deviation_threshold_) { // Velocity deviation is small is_velocity_available = true; } From 35fd24ae466d6475885db951ac81c0a91ab1a6c0 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 7 Mar 2024 18:39:47 +0900 Subject: [PATCH 31/42] fix: remove not-used tracker's last_update_time_ member Signed-off-by: Taekjin LEE --- .../motion_model/motion_model_base.hpp | 2 +- .../tracker/model/bicycle_tracker.hpp | 2 +- .../tracker/model/big_vehicle_tracker.hpp | 2 +- .../tracker/model/normal_vehicle_tracker.hpp | 2 +- .../tracker/model/pedestrian_tracker.hpp | 2 +- .../tracker/model/unknown_tracker.hpp | 2 +- .../src/tracker/model/bicycle_tracker.cpp | 14 ++++---------- .../src/tracker/model/big_vehicle_tracker.cpp | 14 ++++---------- .../src/tracker/model/normal_vehicle_tracker.cpp | 14 ++++---------- .../src/tracker/model/pedestrian_tracker.cpp | 15 ++++----------- .../src/tracker/model/unknown_tracker.cpp | 15 ++++----------- 11 files changed, 26 insertions(+), 58 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/motion_model_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/motion_model_base.hpp index 35fc0bffc2742..8ea04e629eada 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/motion_model_base.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/motion_model_base.hpp @@ -34,11 +34,11 @@ class MotionModel { private: + rclcpp::Time last_update_time_; bool is_initialized_{false}; double dt_max_{0.11}; // [s] maximum time interval for prediction protected: - rclcpp::Time last_update_time_; KalmanFilter ekf_; public: diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 22220ed3f2ad4..4573886674a0f 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -31,7 +31,7 @@ class BicycleTracker : public Tracker rclcpp::Logger logger_; private: - rclcpp::Time last_update_time_; + // rclcpp::Time last_update_time_; struct EkfParams { diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index 9df39959b13f6..9c7781c6d258e 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -31,7 +31,7 @@ class BigVehicleTracker : public Tracker rclcpp::Logger logger_; private: - rclcpp::Time last_update_time_; + // rclcpp::Time last_update_time_; struct EkfParams { double r_cov_x; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 2d4e19a8ccf67..a2b67c62ecae6 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -31,7 +31,7 @@ class NormalVehicleTracker : public Tracker rclcpp::Logger logger_; private: - rclcpp::Time last_update_time_; + // rclcpp::Time last_update_time_; struct EkfParams { diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index c2f323ca0190b..7c33f3a612b3c 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -31,7 +31,7 @@ class PedestrianTracker : public Tracker rclcpp::Logger logger_; private: - rclcpp::Time last_update_time_; + // rclcpp::Time last_update_time_; struct EkfParams { double r_cov_x; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp index cf6f5dc6a3b99..9a9460ba50b96 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -31,7 +31,7 @@ class UnknownTracker : public Tracker rclcpp::Logger logger_; private: - rclcpp::Time last_update_time_; + // rclcpp::Time last_update_time_; struct EkfParams { double r_cov_x; diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 0977c2e10304f..3fba98e493c67 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -47,7 +47,6 @@ BicycleTracker::BicycleTracker( const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("BicycleTracker")), - last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object; @@ -145,12 +144,7 @@ BicycleTracker::BicycleTracker( bool BicycleTracker::predict(const rclcpp::Time & time) { - // predict state vector X t+1 - bool is_predicted = motion_model_.predictState(time); - if (is_predicted) { - last_update_time_ = time; - } - return is_predicted; + return motion_model_.predictState(time); } autoware_auto_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( @@ -244,10 +238,10 @@ bool BicycleTracker::measure( } // check time gap - if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { + const auto time_gap = motion_model_.getDeltaTime(time); + if (0.01 /*10msec*/ < std::fabs(time_gap)) { RCLCPP_WARN( - logger_, "There is a large gap between predicted time and measurement time. (%f)", - (time - last_update_time_).seconds()); + logger_, "There is a large gap between predicted time and measurement time. (%f)", time_gap); } // update object diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 5f18dd743af6d..017e061674519 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -47,7 +47,6 @@ BigVehicleTracker::BigVehicleTracker( const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("BigVehicleTracker")), - last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z), tracking_offset_(Eigen::Vector2d::Zero()) { @@ -167,12 +166,7 @@ BigVehicleTracker::BigVehicleTracker( bool BigVehicleTracker::predict(const rclcpp::Time & time) { - // predict state vector X t+1 - bool is_predicted = motion_model_.predictState(time); - if (is_predicted) { - last_update_time_ = time; - } - return is_predicted; + return motion_model_.predictState(time); } autoware_auto_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObject( @@ -336,10 +330,10 @@ bool BigVehicleTracker::measure( } // check time gap - if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { + const auto time_gap = motion_model_.getDeltaTime(time); + if (0.01 /*10msec*/ < std::fabs(time_gap)) { RCLCPP_WARN( - logger_, "There is a large gap between predicted time and measurement time. (%f)", - (time - last_update_time_).seconds()); + logger_, "There is a large gap between predicted time and measurement time. (%f)", time_gap); } // update object diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 47fc44f56297a..c651de1f02514 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -47,7 +47,6 @@ NormalVehicleTracker::NormalVehicleTracker( const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("NormalVehicleTracker")), - last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z), tracking_offset_(Eigen::Vector2d::Zero()) { @@ -167,12 +166,7 @@ NormalVehicleTracker::NormalVehicleTracker( bool NormalVehicleTracker::predict(const rclcpp::Time & time) { - // predict state vector X t+1 - bool is_predicted = motion_model_.predictState(time); - if (is_predicted) { - last_update_time_ = time; - } - return is_predicted; + return motion_model_.predictState(time); } autoware_auto_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingObject( @@ -336,10 +330,10 @@ bool NormalVehicleTracker::measure( } // check time gap - if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { + const auto time_gap = motion_model_.getDeltaTime(time); + if (0.01 /*10msec*/ < std::fabs(time_gap)) { RCLCPP_WARN( - logger_, "There is a large gap between predicted time and measurement time. (%f)", - (time - last_update_time_).seconds()); + logger_, "There is a large gap between predicted time and measurement time. (%f)", time_gap); } // update object diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index dbfb1c16059f4..9013fe09e253d 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -47,7 +47,6 @@ PedestrianTracker::PedestrianTracker( const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("PedestrianTracker")), - last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object; @@ -144,12 +143,7 @@ PedestrianTracker::PedestrianTracker( bool PedestrianTracker::predict(const rclcpp::Time & time) { - // predict state vector X t+1 - bool is_predicted = motion_model_.predictState(time); - if (is_predicted) { - last_update_time_ = time; - } - return is_predicted; + return motion_model_.predictState(time); } autoware_auto_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObject( @@ -238,11 +232,10 @@ bool PedestrianTracker::measure( } // check time gap - if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { + const auto time_gap = motion_model_.getDeltaTime(time); + if (0.01 /*10msec*/ < std::fabs(time_gap)) { RCLCPP_WARN( - logger_, - "PedestrianTracker: There is a large gap between predicted time and measurement time. (%f)", - (time - last_update_time_).seconds()); + logger_, "There is a large gap between predicted time and measurement time. (%f)", time_gap); } // update object diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp index e170062aa1542..425fa099f6c2b 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -41,7 +41,6 @@ UnknownTracker::UnknownTracker( const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("UnknownTracker")), - last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object; @@ -134,12 +133,7 @@ UnknownTracker::UnknownTracker( bool UnknownTracker::predict(const rclcpp::Time & time) { - // predict state vector X t+1 - bool is_predicted = motion_model_.predictState(time); - if (is_predicted) { - last_update_time_ = time; - } - return is_predicted; + return motion_model_.predictState(time); } autoware_auto_perception_msgs::msg::DetectedObject UnknownTracker::getUpdatingObject( @@ -196,11 +190,10 @@ bool UnknownTracker::measure( object_ = object; // check time gap - if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { + const auto time_gap = motion_model_.getDeltaTime(time); + if (0.01 /*10msec*/ < std::fabs(time_gap)) { RCLCPP_WARN( - logger_, - "UnknownTracker: There is a large gap between predicted time and measurement time. (%f)", - (time - last_update_time_).seconds()); + logger_, "There is a large gap between predicted time and measurement time. (%f)", time_gap); } // update object From cd353d11328e34b54cbbe44cb8a8748c9a8648fc Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 7 Mar 2024 19:06:27 +0900 Subject: [PATCH 32/42] Revert "fix: remove not-used tracker's last_update_time_ member" This reverts commit bd8aabba1b49ca5c2fe9e7b947e075152cfa3863. Signed-off-by: Taekjin LEE --- .../motion_model/motion_model_base.hpp | 2 +- .../tracker/model/bicycle_tracker.hpp | 2 +- .../tracker/model/big_vehicle_tracker.hpp | 2 +- .../tracker/model/normal_vehicle_tracker.hpp | 2 +- .../tracker/model/pedestrian_tracker.hpp | 2 +- .../tracker/model/unknown_tracker.hpp | 2 +- .../src/tracker/model/bicycle_tracker.cpp | 14 ++++++++++---- .../src/tracker/model/big_vehicle_tracker.cpp | 14 ++++++++++---- .../src/tracker/model/normal_vehicle_tracker.cpp | 14 ++++++++++---- .../src/tracker/model/pedestrian_tracker.cpp | 15 +++++++++++---- .../src/tracker/model/unknown_tracker.cpp | 15 +++++++++++---- 11 files changed, 58 insertions(+), 26 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/motion_model_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/motion_model_base.hpp index 8ea04e629eada..35fc0bffc2742 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/motion_model_base.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/motion_model_base.hpp @@ -34,11 +34,11 @@ class MotionModel { private: - rclcpp::Time last_update_time_; bool is_initialized_{false}; double dt_max_{0.11}; // [s] maximum time interval for prediction protected: + rclcpp::Time last_update_time_; KalmanFilter ekf_; public: diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 4573886674a0f..22220ed3f2ad4 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -31,7 +31,7 @@ class BicycleTracker : public Tracker rclcpp::Logger logger_; private: - // rclcpp::Time last_update_time_; + rclcpp::Time last_update_time_; struct EkfParams { diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index 9c7781c6d258e..9df39959b13f6 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -31,7 +31,7 @@ class BigVehicleTracker : public Tracker rclcpp::Logger logger_; private: - // rclcpp::Time last_update_time_; + rclcpp::Time last_update_time_; struct EkfParams { double r_cov_x; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index a2b67c62ecae6..2d4e19a8ccf67 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -31,7 +31,7 @@ class NormalVehicleTracker : public Tracker rclcpp::Logger logger_; private: - // rclcpp::Time last_update_time_; + rclcpp::Time last_update_time_; struct EkfParams { diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index 7c33f3a612b3c..c2f323ca0190b 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -31,7 +31,7 @@ class PedestrianTracker : public Tracker rclcpp::Logger logger_; private: - // rclcpp::Time last_update_time_; + rclcpp::Time last_update_time_; struct EkfParams { double r_cov_x; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp index 9a9460ba50b96..cf6f5dc6a3b99 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -31,7 +31,7 @@ class UnknownTracker : public Tracker rclcpp::Logger logger_; private: - // rclcpp::Time last_update_time_; + rclcpp::Time last_update_time_; struct EkfParams { double r_cov_x; diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 3fba98e493c67..0977c2e10304f 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -47,6 +47,7 @@ BicycleTracker::BicycleTracker( const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("BicycleTracker")), + last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object; @@ -144,7 +145,12 @@ BicycleTracker::BicycleTracker( bool BicycleTracker::predict(const rclcpp::Time & time) { - return motion_model_.predictState(time); + // predict state vector X t+1 + bool is_predicted = motion_model_.predictState(time); + if (is_predicted) { + last_update_time_ = time; + } + return is_predicted; } autoware_auto_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( @@ -238,10 +244,10 @@ bool BicycleTracker::measure( } // check time gap - const auto time_gap = motion_model_.getDeltaTime(time); - if (0.01 /*10msec*/ < std::fabs(time_gap)) { + if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { RCLCPP_WARN( - logger_, "There is a large gap between predicted time and measurement time. (%f)", time_gap); + logger_, "There is a large gap between predicted time and measurement time. (%f)", + (time - last_update_time_).seconds()); } // update object diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 017e061674519..5f18dd743af6d 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -47,6 +47,7 @@ BigVehicleTracker::BigVehicleTracker( const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("BigVehicleTracker")), + last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z), tracking_offset_(Eigen::Vector2d::Zero()) { @@ -166,7 +167,12 @@ BigVehicleTracker::BigVehicleTracker( bool BigVehicleTracker::predict(const rclcpp::Time & time) { - return motion_model_.predictState(time); + // predict state vector X t+1 + bool is_predicted = motion_model_.predictState(time); + if (is_predicted) { + last_update_time_ = time; + } + return is_predicted; } autoware_auto_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObject( @@ -330,10 +336,10 @@ bool BigVehicleTracker::measure( } // check time gap - const auto time_gap = motion_model_.getDeltaTime(time); - if (0.01 /*10msec*/ < std::fabs(time_gap)) { + if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { RCLCPP_WARN( - logger_, "There is a large gap between predicted time and measurement time. (%f)", time_gap); + logger_, "There is a large gap between predicted time and measurement time. (%f)", + (time - last_update_time_).seconds()); } // update object diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index c651de1f02514..47fc44f56297a 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -47,6 +47,7 @@ NormalVehicleTracker::NormalVehicleTracker( const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("NormalVehicleTracker")), + last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z), tracking_offset_(Eigen::Vector2d::Zero()) { @@ -166,7 +167,12 @@ NormalVehicleTracker::NormalVehicleTracker( bool NormalVehicleTracker::predict(const rclcpp::Time & time) { - return motion_model_.predictState(time); + // predict state vector X t+1 + bool is_predicted = motion_model_.predictState(time); + if (is_predicted) { + last_update_time_ = time; + } + return is_predicted; } autoware_auto_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingObject( @@ -330,10 +336,10 @@ bool NormalVehicleTracker::measure( } // check time gap - const auto time_gap = motion_model_.getDeltaTime(time); - if (0.01 /*10msec*/ < std::fabs(time_gap)) { + if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { RCLCPP_WARN( - logger_, "There is a large gap between predicted time and measurement time. (%f)", time_gap); + logger_, "There is a large gap between predicted time and measurement time. (%f)", + (time - last_update_time_).seconds()); } // update object diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index 9013fe09e253d..dbfb1c16059f4 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -47,6 +47,7 @@ PedestrianTracker::PedestrianTracker( const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("PedestrianTracker")), + last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object; @@ -143,7 +144,12 @@ PedestrianTracker::PedestrianTracker( bool PedestrianTracker::predict(const rclcpp::Time & time) { - return motion_model_.predictState(time); + // predict state vector X t+1 + bool is_predicted = motion_model_.predictState(time); + if (is_predicted) { + last_update_time_ = time; + } + return is_predicted; } autoware_auto_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObject( @@ -232,10 +238,11 @@ bool PedestrianTracker::measure( } // check time gap - const auto time_gap = motion_model_.getDeltaTime(time); - if (0.01 /*10msec*/ < std::fabs(time_gap)) { + if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { RCLCPP_WARN( - logger_, "There is a large gap between predicted time and measurement time. (%f)", time_gap); + logger_, + "PedestrianTracker: There is a large gap between predicted time and measurement time. (%f)", + (time - last_update_time_).seconds()); } // update object diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp index 425fa099f6c2b..e170062aa1542 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -41,6 +41,7 @@ UnknownTracker::UnknownTracker( const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("UnknownTracker")), + last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object; @@ -133,7 +134,12 @@ UnknownTracker::UnknownTracker( bool UnknownTracker::predict(const rclcpp::Time & time) { - return motion_model_.predictState(time); + // predict state vector X t+1 + bool is_predicted = motion_model_.predictState(time); + if (is_predicted) { + last_update_time_ = time; + } + return is_predicted; } autoware_auto_perception_msgs::msg::DetectedObject UnknownTracker::getUpdatingObject( @@ -190,10 +196,11 @@ bool UnknownTracker::measure( object_ = object; // check time gap - const auto time_gap = motion_model_.getDeltaTime(time); - if (0.01 /*10msec*/ < std::fabs(time_gap)) { + if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { RCLCPP_WARN( - logger_, "There is a large gap between predicted time and measurement time. (%f)", time_gap); + logger_, + "UnknownTracker: There is a large gap between predicted time and measurement time. (%f)", + (time - last_update_time_).seconds()); } // update object From a0184eb8afe356653514f21610013fccd40c8b56 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 8 Mar 2024 11:12:02 +0900 Subject: [PATCH 33/42] fix: header cleanup Signed-off-by: Taekjin LEE --- .../motion_model/bicycle_motion_model.hpp | 2 +- .../motion_model/ctrv_motion_model.hpp | 2 +- .../motion_model/cv_motion_model.hpp | 2 +- .../motion_model/motion_model_base.hpp | 3 +-- .../src/motion_model/bicycle_motion_model.cpp | 19 +++++++++++-------- .../src/motion_model/cv_motion_model.cpp | 4 ---- 6 files changed, 15 insertions(+), 17 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp index 727bf62d016fe..787aee34e1804 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp @@ -20,7 +20,6 @@ #define MULTI_OBJECT_TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ #include "multi_object_tracker/motion_model/motion_model_base.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include #include @@ -31,6 +30,7 @@ #else #include #endif +#include class BicycleMotionModel : public MotionModel { diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp index 78eda6894e356..cce0b6a57632d 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp @@ -20,7 +20,6 @@ #define MULTI_OBJECT_TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ #include "multi_object_tracker/motion_model/motion_model_base.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include #include @@ -31,6 +30,7 @@ #else #include #endif +#include class CTRVMotionModel : public MotionModel { diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp index ae8b8c6cb5f07..c1559978fb492 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp @@ -20,7 +20,6 @@ #define MULTI_OBJECT_TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ #include "multi_object_tracker/motion_model/motion_model_base.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include #include @@ -31,6 +30,7 @@ #else #include #endif +#include class CVMotionModel : public MotionModel { diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/motion_model_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/motion_model_base.hpp index 35fc0bffc2742..24d1fa470360d 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/motion_model_base.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/motion_model/motion_model_base.hpp @@ -19,8 +19,6 @@ #ifndef MULTI_OBJECT_TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ #define MULTI_OBJECT_TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ -#include "multi_object_tracker/utils/utils.hpp" - #include #include #include @@ -30,6 +28,7 @@ #else #include #endif +#include class MotionModel { diff --git a/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp index 10afdce14116b..6d1f90b217d7f 100644 --- a/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp @@ -357,9 +357,10 @@ bool BicycleMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) c if (vel <= 0.01) { q_stddev_yaw_rate = motion_params_.q_stddev_yaw_rate_min; } else { - // uncertainty of the yaw rate is limited by - // centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v - // or maximum slip angle slip_max : w = v*sin(slip_max)/l_r + /* uncertainty of the yaw rate is limited by the following: + * - centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v + * - or maximum slip angle slip_max : w = v*sin(slip_max)/l_r + */ q_stddev_yaw_rate = std::min( motion_params_.q_stddev_acc_lat / vel, vel * std::sin(motion_params_.q_max_slip_angle) / lr_); // [rad/s] @@ -370,11 +371,13 @@ bool BicycleMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) c if (vel <= 0.01) { q_cov_slip_rate = motion_params_.q_cov_slip_rate_min; } else { - // The slip angle rate uncertainty is modeled as follows: - // d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt - // where sin(slip) = w * l_r / v - // d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) - // d(v)/dt and d(w)/t are considered to be uncorrelated + /* The slip angle rate uncertainty is modeled as follows: + * d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt + * where sin(slip) = w * l_r / v + * + * d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) + * d(v)/dt and d(w)/t are considered to be uncorrelated + */ q_cov_slip_rate = std::pow(motion_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); q_cov_slip_rate = std::min(q_cov_slip_rate, motion_params_.q_cov_slip_rate_max); diff --git a/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp index be5d4a20a45a1..aaa90a5ddfd38 100644 --- a/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp +++ b/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp @@ -253,10 +253,6 @@ bool CVMotionModel::getPredictedState( // get yaw from pose const double yaw = tf2::getYaw(pose.orientation); - // const double yaw = std::atan2( - // 2.0 * (pose.orientation.w * pose.orientation.z + pose.orientation.x * pose.orientation.y), - // 1.0 - 2.0 * (pose.orientation.y * pose.orientation.y + pose.orientation.z * - // pose.orientation.z)); // set position pose.position.x = X(IDX::X); From 98c3b25beb7c323a911b0df1533748dd6e1a126d Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 8 Mar 2024 11:31:55 +0900 Subject: [PATCH 34/42] fix: relocate motion models Signed-off-by: Taekjin LEE --- perception/multi_object_tracker/CMakeLists.txt | 12 ++++++------ .../tracker/model/bicycle_tracker.hpp | 2 +- .../tracker/model/big_vehicle_tracker.hpp | 2 +- .../tracker/model/normal_vehicle_tracker.hpp | 2 +- .../tracker/model/pedestrian_tracker.hpp | 2 +- .../tracker/model/unknown_tracker.hpp | 2 +- .../motion_model/bicycle_motion_model.hpp | 8 ++++---- .../{ => tracker}/motion_model/ctrv_motion_model.hpp | 8 ++++---- .../{ => tracker}/motion_model/cv_motion_model.hpp | 8 ++++---- .../{ => tracker}/motion_model/motion_model_base.hpp | 6 +++--- .../motion_model/bicycle_motion_model.cpp | 4 ++-- .../{ => tracker}/motion_model/ctrv_motion_model.cpp | 4 ++-- .../{ => tracker}/motion_model/cv_motion_model.cpp | 4 ++-- .../{ => tracker}/motion_model/motion_model_base.cpp | 2 +- 14 files changed, 33 insertions(+), 33 deletions(-) rename perception/multi_object_tracker/include/multi_object_tracker/{ => tracker}/motion_model/bicycle_motion_model.hpp (91%) rename perception/multi_object_tracker/include/multi_object_tracker/{ => tracker}/motion_model/ctrv_motion_model.hpp (89%) rename perception/multi_object_tracker/include/multi_object_tracker/{ => tracker}/motion_model/cv_motion_model.hpp (88%) rename perception/multi_object_tracker/include/multi_object_tracker/{ => tracker}/motion_model/motion_model_base.hpp (90%) rename perception/multi_object_tracker/src/{ => tracker}/motion_model/bicycle_motion_model.cpp (99%) rename perception/multi_object_tracker/src/{ => tracker}/motion_model/ctrv_motion_model.cpp (98%) rename perception/multi_object_tracker/src/{ => tracker}/motion_model/cv_motion_model.cpp (98%) rename perception/multi_object_tracker/src/{ => tracker}/motion_model/motion_model_base.cpp (97%) diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt index bd7ebaae54694..94687dd6b61c7 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/multi_object_tracker/CMakeLists.txt @@ -22,6 +22,12 @@ include_directories( # Generate exe file set(MULTI_OBJECT_TRACKER_SRC src/multi_object_tracker_core.cpp + src/data_association/data_association.cpp + src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp + src/tracker/motion_model/motion_model_base.cpp + src/tracker/motion_model/bicycle_motion_model.cpp + src/tracker/motion_model/ctrv_motion_model.cpp + src/tracker/motion_model/cv_motion_model.cpp src/tracker/model/tracker_base.cpp src/tracker/model/big_vehicle_tracker.cpp src/tracker/model/normal_vehicle_tracker.cpp @@ -31,12 +37,6 @@ set(MULTI_OBJECT_TRACKER_SRC src/tracker/model/pedestrian_and_bicycle_tracker.cpp src/tracker/model/unknown_tracker.cpp src/tracker/model/pass_through_tracker.cpp - src/data_association/data_association.cpp - src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp - src/motion_model/motion_model_base.cpp - src/motion_model/bicycle_motion_model.cpp - src/motion_model/ctrv_motion_model.cpp - src/motion_model/cv_motion_model.cpp ) ament_auto_add_library(multi_object_tracker_node SHARED diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 22220ed3f2ad4..7e5a27f611dff 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -19,8 +19,8 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ -#include "multi_object_tracker/motion_model/bicycle_motion_model.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" #include diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index 9df39959b13f6..ceb6883a64ae8 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -19,8 +19,8 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ -#include "multi_object_tracker/motion_model/bicycle_motion_model.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" #include diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 2d4e19a8ccf67..3f8fc72e8880c 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -19,8 +19,8 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ -#include "multi_object_tracker/motion_model/bicycle_motion_model.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" #include diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index c2f323ca0190b..66c23ede4a1b4 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -19,8 +19,8 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ -#include "multi_object_tracker/motion_model/ctrv_motion_model.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" #include diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp index cf6f5dc6a3b99..eef2c9f756d4e 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -19,8 +19,8 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ -#include "multi_object_tracker/motion_model/cv_motion_model.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" #include diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp similarity index 91% rename from perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp rename to perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp index 787aee34e1804..5127d0448835c 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/bicycle_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp @@ -16,10 +16,10 @@ // Author: v1.0 Taekjin Lee // -#ifndef MULTI_OBJECT_TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ -#define MULTI_OBJECT_TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ +#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ +#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ -#include "multi_object_tracker/motion_model/motion_model_base.hpp" +#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include #include @@ -105,4 +105,4 @@ class BicycleMotionModel : public MotionModel geometry_msgs::msg::Twist & twist, std::array & twist_cov) const override; }; -#endif // MULTI_OBJECT_TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ +#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp similarity index 89% rename from perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp rename to perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp index cce0b6a57632d..560d165bba1b5 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/ctrv_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp @@ -16,10 +16,10 @@ // Author: v1.0 Taekjin Lee // -#ifndef MULTI_OBJECT_TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ -#define MULTI_OBJECT_TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ +#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ +#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ -#include "multi_object_tracker/motion_model/motion_model_base.hpp" +#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include #include @@ -90,4 +90,4 @@ class CTRVMotionModel : public MotionModel geometry_msgs::msg::Twist & twist, std::array & twist_cov) const override; }; -#endif // MULTI_OBJECT_TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ +#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp similarity index 88% rename from perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp rename to perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp index c1559978fb492..59032706b00d6 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/cv_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp @@ -16,10 +16,10 @@ // Author: v1.0 Taekjin Lee // -#ifndef MULTI_OBJECT_TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ -#define MULTI_OBJECT_TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ +#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ +#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ -#include "multi_object_tracker/motion_model/motion_model_base.hpp" +#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include #include @@ -85,4 +85,4 @@ class CVMotionModel : public MotionModel geometry_msgs::msg::Twist & twist, std::array & twist_cov) const override; }; -#endif // MULTI_OBJECT_TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ +#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/motion_model_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp similarity index 90% rename from perception/multi_object_tracker/include/multi_object_tracker/motion_model/motion_model_base.hpp rename to perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp index 24d1fa470360d..af4b8abf970fa 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/motion_model/motion_model_base.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp @@ -16,8 +16,8 @@ // Author: v1.0 Taekjin Lee // -#ifndef MULTI_OBJECT_TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ -#define MULTI_OBJECT_TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ +#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ +#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ #include #include @@ -61,4 +61,4 @@ class MotionModel geometry_msgs::msg::Twist & twist, std::array & twist_cov) const = 0; }; -#endif // MULTI_OBJECT_TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ +#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ diff --git a/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp similarity index 99% rename from perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp rename to perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp index 6d1f90b217d7f..d992ea926746e 100644 --- a/perception/multi_object_tracker/src/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp @@ -16,9 +16,9 @@ // Author: v1.0 Taekjin Lee // -#include "multi_object_tracker/motion_model/bicycle_motion_model.hpp" +#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" -#include "multi_object_tracker/motion_model/motion_model_base.hpp" +#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/utils/utils.hpp" #include diff --git a/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp similarity index 98% rename from perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp rename to perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp index 6284a4b8e3980..b10fc70d1bba7 100644 --- a/perception/multi_object_tracker/src/motion_model/ctrv_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp @@ -16,9 +16,9 @@ // Author: v1.0 Taekjin Lee // -#include "multi_object_tracker/motion_model/ctrv_motion_model.hpp" +#include "multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" -#include "multi_object_tracker/motion_model/motion_model_base.hpp" +#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/utils/utils.hpp" #include diff --git a/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp similarity index 98% rename from perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp rename to perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp index aaa90a5ddfd38..28ba59f4f19f9 100644 --- a/perception/multi_object_tracker/src/motion_model/cv_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp @@ -16,9 +16,9 @@ // Author: v1.0 Taekjin Lee // -#include "multi_object_tracker/motion_model/cv_motion_model.hpp" +#include "multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" -#include "multi_object_tracker/motion_model/motion_model_base.hpp" +#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/utils/utils.hpp" #include diff --git a/perception/multi_object_tracker/src/motion_model/motion_model_base.cpp b/perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp similarity index 97% rename from perception/multi_object_tracker/src/motion_model/motion_model_base.cpp rename to perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp index 6a945283e1a8c..18448e96d428b 100644 --- a/perception/multi_object_tracker/src/motion_model/motion_model_base.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp @@ -16,7 +16,7 @@ // Author: v1.0 Taekjin Lee // -#include "multi_object_tracker/motion_model/motion_model_base.hpp" +#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" MotionModel::MotionModel() : last_update_time_(rclcpp::Time(0, 0)) { From c313d7bc682867591852f893eadb2b8fd63252e0 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 8 Mar 2024 17:24:13 +0900 Subject: [PATCH 35/42] fix: timestamp const Signed-off-by: Taekjin LEE --- .../tracker/motion_model/motion_model_base.hpp | 5 ++++- .../multi_object_tracker/src/multi_object_tracker_core.cpp | 4 ++-- .../src/tracker/motion_model/motion_model_base.cpp | 2 +- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp index af4b8abf970fa..1aca602ed66a3 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp @@ -45,7 +45,10 @@ class MotionModel virtual ~MotionModel() = default; bool checkInitialized() const { return is_initialized_; } - double getDeltaTime(rclcpp::Time time) const { return (time - last_update_time_).seconds(); } + double getDeltaTime(const rclcpp::Time & time) const + { + return (time - last_update_time_).seconds(); + } void setMaxDeltaTime(const double dt_max) { dt_max_ = dt_max; } double getStateElement(unsigned int idx) const { return ekf_.getXelement(idx); } void getStateVector(Eigen::MatrixXd & X) const { ekf_.getX(X); } 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 d955745bb8c47..5d3f7e41d1190 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -256,7 +256,7 @@ void MultiObjectTracker::onMeasurement( return; } /* tracker prediction */ - rclcpp::Time measurement_time = input_objects_msg->header.stamp; + const rclcpp::Time measurement_time = input_objects_msg->header.stamp; for (auto itr = list_tracker_.begin(); itr != list_tracker_.end(); ++itr) { (*itr)->predict(measurement_time); } @@ -330,7 +330,7 @@ std::shared_ptr MultiObjectTracker::createNewTracker( void MultiObjectTracker::onTimer() { - rclcpp::Time current_time = this->now(); + const rclcpp::Time current_time = this->now(); const auto self_transform = getTransformAnonymous(tf_buffer_, world_frame_id_, "base_link", current_time); if (!self_transform) { 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 18448e96d428b..c096ea36ab7d2 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 @@ -28,7 +28,7 @@ bool MotionModel::initialize( // initialize Kalman filter if (!ekf_.init(X, P)) return false; - // set last update time + // set last_updated_time_ last_update_time_ = time; // set initialized flag From 54e6248ef08f1a0cca40d06de375120a625e56b2 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 8 Mar 2024 17:47:56 +0900 Subject: [PATCH 36/42] fix: exception when object couldn't be predicted Signed-off-by: Taekjin LEE --- .../src/multi_object_tracker_core.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 5d3f7e41d1190..cc662768ce197 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -373,10 +373,10 @@ void MultiObjectTracker::sanitizeTracker( /* delete collision tracker */ for (auto itr1 = list_tracker.begin(); itr1 != list_tracker.end(); ++itr1) { autoware_auto_perception_msgs::msg::TrackedObject object1; - (*itr1)->getTrackedObject(time, object1); + if (!(*itr1)->getTrackedObject(time, object1)) continue; for (auto itr2 = std::next(itr1); itr2 != list_tracker.end(); ++itr2) { autoware_auto_perception_msgs::msg::TrackedObject object2; - (*itr2)->getTrackedObject(time, object2); + if (!(*itr2)->getTrackedObject(time, object2)) continue; const double distance = std::hypot( object1.kinematics.pose_with_covariance.pose.position.x - object2.kinematics.pose_with_covariance.pose.position.x, @@ -457,12 +457,12 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) for (auto itr = list_tracker_.begin(); itr != list_tracker_.end(); ++itr) { if (!shouldTrackerPublish(*itr)) { // for debug purpose autoware_auto_perception_msgs::msg::TrackedObject object; - (*itr)->getTrackedObject(time, object); + if (!(*itr)->getTrackedObject(time, object)) continue; tentative_objects_msg.objects.push_back(object); continue; } autoware_auto_perception_msgs::msg::TrackedObject object; - (*itr)->getTrackedObject(time, object); + if (!(*itr)->getTrackedObject(time, object)) continue; output_msg.objects.push_back(object); } From 0189ebbcd747341d0a553476493c0ed3b85b3a93 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 8 Mar 2024 19:23:02 +0900 Subject: [PATCH 37/42] fix: remove last_update_time_ member Signed-off-by: Taekjin LEE --- .../tracker/model/bicycle_tracker.hpp | 2 +- .../tracker/model/big_vehicle_tracker.hpp | 2 +- .../tracker/model/normal_vehicle_tracker.hpp | 2 +- .../tracker/model/pedestrian_tracker.hpp | 2 +- .../tracker/model/unknown_tracker.hpp | 2 +- .../src/tracker/model/bicycle_tracker.cpp | 18 ++++++++---------- .../src/tracker/model/big_vehicle_tracker.cpp | 18 ++++++++---------- .../tracker/model/normal_vehicle_tracker.cpp | 18 ++++++++---------- .../src/tracker/model/pedestrian_tracker.cpp | 17 +++++++---------- .../src/tracker/model/unknown_tracker.cpp | 17 +++++++---------- .../tracker/motion_model/motion_model_base.cpp | 2 +- 11 files changed, 44 insertions(+), 56 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 7e5a27f611dff..188cc284f6463 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -31,7 +31,7 @@ class BicycleTracker : public Tracker rclcpp::Logger logger_; private: - rclcpp::Time last_update_time_; + // rclcpp::Time last_update_time_; struct EkfParams { diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index ceb6883a64ae8..a0d89071bf2fb 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -31,7 +31,7 @@ class BigVehicleTracker : public Tracker rclcpp::Logger logger_; private: - rclcpp::Time last_update_time_; + // rclcpp::Time last_update_time_; struct EkfParams { double r_cov_x; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 3f8fc72e8880c..810ef2fbb477c 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -31,7 +31,7 @@ class NormalVehicleTracker : public Tracker rclcpp::Logger logger_; private: - rclcpp::Time last_update_time_; + // rclcpp::Time last_update_time_; struct EkfParams { diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index 66c23ede4a1b4..571826bca9d37 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -31,7 +31,7 @@ class PedestrianTracker : public Tracker rclcpp::Logger logger_; private: - rclcpp::Time last_update_time_; + // rclcpp::Time last_update_time_; struct EkfParams { double r_cov_x; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp index eef2c9f756d4e..dc8e7f2255800 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -31,7 +31,7 @@ class UnknownTracker : public Tracker rclcpp::Logger logger_; private: - rclcpp::Time last_update_time_; + // rclcpp::Time last_update_time_; struct EkfParams { double r_cov_x; diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 0977c2e10304f..1cdb4842c3fa7 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -47,7 +47,7 @@ BicycleTracker::BicycleTracker( const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("BicycleTracker")), - last_update_time_(time), + // last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object; @@ -145,12 +145,7 @@ BicycleTracker::BicycleTracker( bool BicycleTracker::predict(const rclcpp::Time & time) { - // predict state vector X t+1 - bool is_predicted = motion_model_.predictState(time); - if (is_predicted) { - last_update_time_ = time; - } - return is_predicted; + return motion_model_.predictState(time); } autoware_auto_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( @@ -244,10 +239,13 @@ bool BicycleTracker::measure( } // check time gap - if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { + const double dt = motion_model_.getDeltaTime(time); + if (0.01 /*10msec*/ < dt) { RCLCPP_WARN( - logger_, "There is a large gap between predicted time and measurement time. (%f)", - (time - last_update_time_).seconds()); + logger_, + "BicycleTracker::measure There is a large gap between predicted time and measurement time. " + "(%f)", + dt); } // update object diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 5f18dd743af6d..7c4ea8a8a2dde 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -47,7 +47,7 @@ BigVehicleTracker::BigVehicleTracker( const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("BigVehicleTracker")), - last_update_time_(time), + // last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z), tracking_offset_(Eigen::Vector2d::Zero()) { @@ -167,12 +167,7 @@ BigVehicleTracker::BigVehicleTracker( bool BigVehicleTracker::predict(const rclcpp::Time & time) { - // predict state vector X t+1 - bool is_predicted = motion_model_.predictState(time); - if (is_predicted) { - last_update_time_ = time; - } - return is_predicted; + return motion_model_.predictState(time); } autoware_auto_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObject( @@ -336,10 +331,13 @@ bool BigVehicleTracker::measure( } // check time gap - if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { + const double dt = motion_model_.getDeltaTime(time); + if (0.01 /*10msec*/ < dt) { RCLCPP_WARN( - logger_, "There is a large gap between predicted time and measurement time. (%f)", - (time - last_update_time_).seconds()); + logger_, + "BigVehicleTracker::measure There is a large gap between predicted time and measurement " + "time. (%f)", + dt); } // update object diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 47fc44f56297a..c9371c5bc54eb 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -47,7 +47,7 @@ NormalVehicleTracker::NormalVehicleTracker( const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("NormalVehicleTracker")), - last_update_time_(time), + // last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z), tracking_offset_(Eigen::Vector2d::Zero()) { @@ -167,12 +167,7 @@ NormalVehicleTracker::NormalVehicleTracker( bool NormalVehicleTracker::predict(const rclcpp::Time & time) { - // predict state vector X t+1 - bool is_predicted = motion_model_.predictState(time); - if (is_predicted) { - last_update_time_ = time; - } - return is_predicted; + return motion_model_.predictState(time); } autoware_auto_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingObject( @@ -336,10 +331,13 @@ bool NormalVehicleTracker::measure( } // check time gap - if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { + const double dt = motion_model_.getDeltaTime(time); + if (0.01 /*10msec*/ < dt) { RCLCPP_WARN( - logger_, "There is a large gap between predicted time and measurement time. (%f)", - (time - last_update_time_).seconds()); + logger_, + "NormalVehicleTracker::measure There is a large gap between predicted time and measurement " + "time. (%f)", + dt); } // update object diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index dbfb1c16059f4..7a450109c5f21 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -47,7 +47,7 @@ PedestrianTracker::PedestrianTracker( const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("PedestrianTracker")), - last_update_time_(time), + // last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object; @@ -144,12 +144,7 @@ PedestrianTracker::PedestrianTracker( bool PedestrianTracker::predict(const rclcpp::Time & time) { - // predict state vector X t+1 - bool is_predicted = motion_model_.predictState(time); - if (is_predicted) { - last_update_time_ = time; - } - return is_predicted; + return motion_model_.predictState(time); } autoware_auto_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObject( @@ -238,11 +233,13 @@ bool PedestrianTracker::measure( } // check time gap - if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { + const double dt = motion_model_.getDeltaTime(time); + if (0.01 /*10msec*/ < dt) { RCLCPP_WARN( logger_, - "PedestrianTracker: There is a large gap between predicted time and measurement time. (%f)", - (time - last_update_time_).seconds()); + "PedestrianTracker::measure There is a large gap between predicted time and measurement " + "time. (%f)", + dt); } // update object diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp index e170062aa1542..1201fb77e32c9 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -41,7 +41,7 @@ UnknownTracker::UnknownTracker( const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("UnknownTracker")), - last_update_time_(time), + // last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object; @@ -134,12 +134,7 @@ UnknownTracker::UnknownTracker( bool UnknownTracker::predict(const rclcpp::Time & time) { - // predict state vector X t+1 - bool is_predicted = motion_model_.predictState(time); - if (is_predicted) { - last_update_time_ = time; - } - return is_predicted; + return motion_model_.predictState(time); } autoware_auto_perception_msgs::msg::DetectedObject UnknownTracker::getUpdatingObject( @@ -196,11 +191,13 @@ bool UnknownTracker::measure( object_ = object; // check time gap - if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { + const double dt = motion_model_.getDeltaTime(time); + if (0.01 /*10msec*/ < dt) { RCLCPP_WARN( logger_, - "UnknownTracker: There is a large gap between predicted time and measurement time. (%f)", - (time - last_update_time_).seconds()); + "UnknownTracker::measure There is a large gap between predicted time and measurement time. " + "(%f)", + dt); } // update object 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 c096ea36ab7d2..b4af422fd2329 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 @@ -28,7 +28,7 @@ bool MotionModel::initialize( // initialize Kalman filter if (!ekf_.init(X, P)) return false; - // set last_updated_time_ + // set last_update_time_ last_update_time_ = time; // set initialized flag From 61c3172289171d4b64163c5cac0bc73c2beadd4d Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 8 Mar 2024 22:46:39 +0900 Subject: [PATCH 38/42] fix: set minimum size, to avoid iou error/overflow Signed-off-by: Taekjin LEE --- .../src/tracker/model/bicycle_tracker.cpp | 21 +++++++++++++++++++ .../src/tracker/model/big_vehicle_tracker.cpp | 8 +++++++ .../tracker/model/normal_vehicle_tracker.cpp | 8 +++++++ .../src/tracker/model/pedestrian_tracker.cpp | 6 ++++++ 4 files changed, 43 insertions(+) diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 1cdb4842c3fa7..1ed8ab2812a32 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -61,6 +61,18 @@ BicycleTracker::BicycleTracker( ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); + // OBJECT SHAPE MODEL + if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + bounding_box_ = { + object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; + } else { + bounding_box_ = {1.0, 0.5, 1.7}; + } + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 0.5); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.8); + // Set motion model parameters { constexpr double q_stddev_acc_long = @@ -211,6 +223,11 @@ bool BicycleTracker::measureWithPose( bool BicycleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { + if (!object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + // do not update shape if the input is not a bounding box + return true; + } + constexpr double gain = 0.1; constexpr double gain_inv = 1.0 - gain; @@ -218,6 +235,10 @@ bool BicycleTracker::measureWithShape( bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x; bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y; bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 0.5); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.8); // update motion model motion_model_.updateExtendedState(bounding_box_.length); diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 7c4ea8a8a2dde..26167ea615810 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -82,6 +82,10 @@ BigVehicleTracker::BigVehicleTracker( bbox_object.shape.dimensions.z}; last_input_bounding_box_ = bounding_box_; } + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 2.5); + bounding_box_.width = std::max(bounding_box_.width, 1.0); + bounding_box_.height = std::max(bounding_box_.height, 1.0); // Set motion model parameters { @@ -305,6 +309,10 @@ bool BigVehicleTracker::measureWithShape( bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; last_input_bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 2.5); + bounding_box_.width = std::max(bounding_box_.width, 1.0); + bounding_box_.height = std::max(bounding_box_.height, 1.0); // update motion model motion_model_.updateExtendedState(bounding_box_.length); diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index c9371c5bc54eb..ebf86612f0c62 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -82,6 +82,10 @@ NormalVehicleTracker::NormalVehicleTracker( bbox_object.shape.dimensions.z}; last_input_bounding_box_ = bounding_box_; } + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 1.5); + bounding_box_.width = std::max(bounding_box_.width, 0.5); + bounding_box_.height = std::max(bounding_box_.height, 1.0); // Set motion model parameters { @@ -305,6 +309,10 @@ bool NormalVehicleTracker::measureWithShape( bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; last_input_bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 1.5); + bounding_box_.width = std::max(bounding_box_.width, 0.5); + bounding_box_.height = std::max(bounding_box_.height, 1.0); // update motion model motion_model_.updateExtendedState(bounding_box_.length); diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index 7a450109c5f21..6f2cfac232dae 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -70,6 +70,12 @@ PedestrianTracker::PedestrianTracker( } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { cylinder_ = {object.shape.dimensions.x, object.shape.dimensions.z}; } + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 0.3); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.8); + cylinder_.width = std::max(cylinder_.width, 0.3); + cylinder_.height = std::max(cylinder_.height, 0.8); // Set motion model parameters { From 42a2ef6ffc8f5317d86db47e2dab6805d2d56692 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 12 Mar 2024 12:52:26 +0900 Subject: [PATCH 39/42] fix: rollback tracking size and offset algorithm Signed-off-by: Taekjin LEE --- .../tracker/model/big_vehicle_tracker.hpp | 11 ++++++++ .../tracker/model/normal_vehicle_tracker.hpp | 11 ++++++++ .../src/tracker/model/big_vehicle_tracker.cpp | 28 +++++++++++++++---- .../tracker/model/normal_vehicle_tracker.cpp | 28 +++++++++++++++---- 4 files changed, 66 insertions(+), 12 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index a0d89071bf2fb..b9550b975d4a4 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -52,6 +52,7 @@ class BigVehicleTracker : public Tracker BoundingBox bounding_box_; BoundingBox last_input_bounding_box_; Eigen::Vector2d tracking_offset_; + int last_nearest_corner_index_; private: BicycleMotionModel motion_model_; @@ -76,6 +77,16 @@ class BigVehicleTracker : public Tracker const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const override; virtual ~BigVehicleTracker() {} + +private: + void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform) + { + Eigen::MatrixXd X_t(DIM, 1); + motion_model_.getStateVector(X_t); + last_nearest_corner_index_ = utils::getNearestCornerOrSurface( + X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, + self_transform); + } }; #endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 810ef2fbb477c..dd91a354994cf 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -53,6 +53,7 @@ class NormalVehicleTracker : public Tracker BoundingBox bounding_box_; BoundingBox last_input_bounding_box_; Eigen::Vector2d tracking_offset_; + int last_nearest_corner_index_; private: BicycleMotionModel motion_model_; @@ -77,6 +78,16 @@ class NormalVehicleTracker : public Tracker const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const override; virtual ~NormalVehicleTracker() {} + +private: + void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform) + { + Eigen::MatrixXd X_t(DIM, 1); + motion_model_.getStateVector(X_t); + last_nearest_corner_index_ = utils::getNearestCornerOrSurface( + X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, + self_transform); + } }; #endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 26167ea615810..30cbe733d0824 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -44,7 +44,7 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; BigVehicleTracker::BigVehicleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/) + const geometry_msgs::msg::Transform & self_transform) : Tracker(time, object.classification), logger_(rclcpp::get_logger("BigVehicleTracker")), // last_update_time_(time), @@ -167,6 +167,9 @@ BigVehicleTracker::BigVehicleTracker( // initialize motion model motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); } + + /* calc nearest corner index*/ + setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step } bool BigVehicleTracker::predict(const rclcpp::Time & time) @@ -316,11 +319,12 @@ bool BigVehicleTracker::measureWithShape( // update motion model motion_model_.updateExtendedState(bounding_box_.length); - // update offset into object position - motion_model_.adjustPosition(gain * tracking_offset_.x(), gain * tracking_offset_.y()); - // update offset - tracking_offset_.x() = gain_inv * tracking_offset_.x(); - tracking_offset_.y() = gain_inv * tracking_offset_.y(); + + // // update offset into object position + // motion_model_.adjustPosition(gain * tracking_offset_.x(), gain * tracking_offset_.y()); + // // update offset + // tracking_offset_.x() = gain_inv * tracking_offset_.x(); + // tracking_offset_.y() = gain_inv * tracking_offset_.y(); return true; } @@ -354,6 +358,9 @@ bool BigVehicleTracker::measure( measureWithPose(updating_object); measureWithShape(updating_object); + /* calc nearest corner index*/ + setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step + return true; } @@ -375,6 +382,15 @@ bool BigVehicleTracker::getTrackedObject( return false; } + // recover bounding box from tracking point + const double dl = bounding_box_.length - last_input_bounding_box_.length; + const double dw = bounding_box_.width - last_input_bounding_box_.width; + const Eigen::Vector2d recovered_pose = utils::recoverFromTrackingPoint( + pose_with_cov.pose.position.x, pose_with_cov.pose.position.y, + motion_model_.getStateElement(IDX::YAW), dw, dl, last_nearest_corner_index_, tracking_offset_); + pose_with_cov.pose.position.x = recovered_pose.x(); + pose_with_cov.pose.position.y = recovered_pose.y(); + // position pose_with_cov.pose.position.z = z_; diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index ebf86612f0c62..b87e759e27bc3 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -44,7 +44,7 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; NormalVehicleTracker::NormalVehicleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/) + const geometry_msgs::msg::Transform & self_transform) : Tracker(time, object.classification), logger_(rclcpp::get_logger("NormalVehicleTracker")), // last_update_time_(time), @@ -167,6 +167,9 @@ NormalVehicleTracker::NormalVehicleTracker( // initialize motion model motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); } + + /* calc nearest corner index*/ + setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step } bool NormalVehicleTracker::predict(const rclcpp::Time & time) @@ -316,11 +319,12 @@ bool NormalVehicleTracker::measureWithShape( // update motion model motion_model_.updateExtendedState(bounding_box_.length); - // update offset into object position - motion_model_.adjustPosition(gain * tracking_offset_.x(), gain * tracking_offset_.y()); - // update offset - tracking_offset_.x() = gain_inv * tracking_offset_.x(); - tracking_offset_.y() = gain_inv * tracking_offset_.y(); + + // // update offset into object position + // motion_model_.adjustPosition(gain * tracking_offset_.x(), gain * tracking_offset_.y()); + // // update offset + // tracking_offset_.x() = gain_inv * tracking_offset_.x(); + // tracking_offset_.y() = gain_inv * tracking_offset_.y(); return true; } @@ -354,6 +358,9 @@ bool NormalVehicleTracker::measure( measureWithPose(updating_object); measureWithShape(updating_object); + /* calc nearest corner index*/ + setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step + return true; } @@ -375,6 +382,15 @@ bool NormalVehicleTracker::getTrackedObject( return false; } + // recover bounding box from tracking point + const double dl = bounding_box_.length - last_input_bounding_box_.length; + const double dw = bounding_box_.width - last_input_bounding_box_.width; + const Eigen::Vector2d recovered_pose = utils::recoverFromTrackingPoint( + pose_with_cov.pose.position.x, pose_with_cov.pose.position.y, + motion_model_.getStateElement(IDX::YAW), dw, dl, last_nearest_corner_index_, tracking_offset_); + pose_with_cov.pose.position.x = recovered_pose.x(); + pose_with_cov.pose.position.y = recovered_pose.y(); + // position pose_with_cov.pose.position.z = z_; From 09201eadb3d1cf950a218ca367e4d9efad5b21d1 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 12 Mar 2024 13:22:18 +0900 Subject: [PATCH 40/42] fix: set minimum pedestrian size Signed-off-by: Taekjin LEE --- .../src/tracker/model/pedestrian_tracker.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index 6f2cfac232dae..3ac45d239cd37 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -219,9 +219,9 @@ bool PedestrianTracker::measureWithShape( // set minimum size bounding_box_.length = std::max(bounding_box_.length, 0.3); bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.8); cylinder_.width = std::max(cylinder_.width, 0.3); - cylinder_.height = std::max(cylinder_.height, 0.3); + cylinder_.height = std::max(cylinder_.height, 0.8); return true; } From 72adba346e0c24f3a4dff8e530b9cf4d94ffe94d Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 12 Mar 2024 16:57:05 +0900 Subject: [PATCH 41/42] fix: adjust minimum sizes to 0.3m Signed-off-by: Taekjin LEE --- .../src/tracker/model/bicycle_tracker.cpp | 8 ++++---- .../src/tracker/model/big_vehicle_tracker.cpp | 12 ++++++------ .../src/tracker/model/normal_vehicle_tracker.cpp | 12 ++++++------ .../src/tracker/model/pedestrian_tracker.cpp | 8 ++++---- 4 files changed, 20 insertions(+), 20 deletions(-) diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 1ed8ab2812a32..7b7927fdc1edb 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -69,9 +69,9 @@ BicycleTracker::BicycleTracker( bounding_box_ = {1.0, 0.5, 1.7}; } // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 0.5); + bounding_box_.length = std::max(bounding_box_.length, 0.3); bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.8); + bounding_box_.height = std::max(bounding_box_.height, 0.3); // Set motion model parameters { @@ -236,9 +236,9 @@ bool BicycleTracker::measureWithShape( bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y; bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 0.5); + bounding_box_.length = std::max(bounding_box_.length, 0.3); bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.8); + bounding_box_.height = std::max(bounding_box_.height, 0.3); // update motion model motion_model_.updateExtendedState(bounding_box_.length); diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 30cbe733d0824..421729753d419 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -83,9 +83,9 @@ BigVehicleTracker::BigVehicleTracker( last_input_bounding_box_ = bounding_box_; } // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 2.5); - bounding_box_.width = std::max(bounding_box_.width, 1.0); - bounding_box_.height = std::max(bounding_box_.height, 1.0); + bounding_box_.length = std::max(bounding_box_.length, 0.3); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.3); // Set motion model parameters { @@ -313,9 +313,9 @@ bool BigVehicleTracker::measureWithShape( last_input_bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 2.5); - bounding_box_.width = std::max(bounding_box_.width, 1.0); - bounding_box_.height = std::max(bounding_box_.height, 1.0); + bounding_box_.length = std::max(bounding_box_.length, 0.3); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.3); // update motion model motion_model_.updateExtendedState(bounding_box_.length); diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index b87e759e27bc3..04532270ece10 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -83,9 +83,9 @@ NormalVehicleTracker::NormalVehicleTracker( last_input_bounding_box_ = bounding_box_; } // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 1.5); - bounding_box_.width = std::max(bounding_box_.width, 0.5); - bounding_box_.height = std::max(bounding_box_.height, 1.0); + bounding_box_.length = std::max(bounding_box_.length, 0.3); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.3); // Set motion model parameters { @@ -313,9 +313,9 @@ bool NormalVehicleTracker::measureWithShape( last_input_bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 1.5); - bounding_box_.width = std::max(bounding_box_.width, 0.5); - bounding_box_.height = std::max(bounding_box_.height, 1.0); + bounding_box_.length = std::max(bounding_box_.length, 0.3); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.3); // update motion model motion_model_.updateExtendedState(bounding_box_.length); diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index 3ac45d239cd37..e01558c75761c 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -73,9 +73,9 @@ PedestrianTracker::PedestrianTracker( // set minimum size bounding_box_.length = std::max(bounding_box_.length, 0.3); bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.8); + bounding_box_.height = std::max(bounding_box_.height, 0.3); cylinder_.width = std::max(cylinder_.width, 0.3); - cylinder_.height = std::max(cylinder_.height, 0.8); + cylinder_.height = std::max(cylinder_.height, 0.3); // Set motion model parameters { @@ -219,9 +219,9 @@ bool PedestrianTracker::measureWithShape( // set minimum size bounding_box_.length = std::max(bounding_box_.length, 0.3); bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.8); + bounding_box_.height = std::max(bounding_box_.height, 0.3); cylinder_.width = std::max(cylinder_.width, 0.3); - cylinder_.height = std::max(cylinder_.height, 0.8); + cylinder_.height = std::max(cylinder_.height, 0.3); return true; } From 08f374138ee8e1ec18f77b0845e942a2d3accee3 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 13 Mar 2024 10:32:51 +0900 Subject: [PATCH 42/42] chore: cleanup unused members Signed-off-by: Taekjin LEE --- .../multi_object_tracker/tracker/model/bicycle_tracker.hpp | 5 +---- .../tracker/model/big_vehicle_tracker.hpp | 5 ++--- .../tracker/model/normal_vehicle_tracker.hpp | 6 ++---- .../tracker/model/pedestrian_tracker.hpp | 3 +-- .../multi_object_tracker/tracker/model/unknown_tracker.hpp | 4 ++-- .../src/tracker/model/bicycle_tracker.cpp | 1 - .../src/tracker/model/big_vehicle_tracker.cpp | 1 - .../src/tracker/model/normal_vehicle_tracker.cpp | 1 - .../src/tracker/model/pedestrian_tracker.cpp | 1 - .../src/tracker/model/unknown_tracker.cpp | 1 - 10 files changed, 8 insertions(+), 20 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 188cc284f6463..2d69334a2f43f 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -31,17 +31,15 @@ class BicycleTracker : public Tracker rclcpp::Logger logger_; private: - // rclcpp::Time last_update_time_; - struct EkfParams { double r_cov_x; double r_cov_y; double r_cov_yaw; } ekf_params_; + double z_; -private: struct BoundingBox { double length; @@ -61,7 +59,6 @@ class BicycleTracker : public Tracker const geometry_msgs::msg::Transform & self_transform); bool predict(const rclcpp::Time & time) override; - bool predict(const double dt, KalmanFilter & ekf) const; bool measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index b9550b975d4a4..8d49138b94a24 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -31,7 +31,6 @@ class BigVehicleTracker : public Tracker rclcpp::Logger logger_; private: - // rclcpp::Time last_update_time_; struct EkfParams { double r_cov_x; @@ -39,10 +38,10 @@ class BigVehicleTracker : public Tracker double r_cov_yaw; double r_cov_vel; } ekf_params_; - double z_; double velocity_deviation_threshold_; -private: + double z_; + struct BoundingBox { double length; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index dd91a354994cf..6ab7e63bce167 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -31,8 +31,6 @@ class NormalVehicleTracker : public Tracker rclcpp::Logger logger_; private: - // rclcpp::Time last_update_time_; - struct EkfParams { double r_cov_x; @@ -40,10 +38,10 @@ class NormalVehicleTracker : public Tracker double r_cov_yaw; double r_cov_vel; } ekf_params_; - double z_; double velocity_deviation_threshold_; -private: + double z_; + struct BoundingBox { double length; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index 571826bca9d37..c81f594ae461a 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -31,16 +31,15 @@ class PedestrianTracker : public Tracker rclcpp::Logger logger_; private: - // rclcpp::Time last_update_time_; struct EkfParams { double r_cov_x; double r_cov_y; double r_cov_yaw; } ekf_params_; + double z_; -private: struct BoundingBox { double length; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp index dc8e7f2255800..73bf7849e13d8 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -13,7 +13,7 @@ // limitations under the License. // // -// v1.0 Yukihiro Saito +// Author: v1.0 Yukihiro Saito // #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ @@ -31,7 +31,6 @@ class UnknownTracker : public Tracker rclcpp::Logger logger_; private: - // rclcpp::Time last_update_time_; struct EkfParams { double r_cov_x; @@ -39,6 +38,7 @@ class UnknownTracker : public Tracker double r_cov_vx; double r_cov_vy; } ekf_params_; + double z_; private: diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 7b7927fdc1edb..12c340516c6c1 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -47,7 +47,6 @@ BicycleTracker::BicycleTracker( const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("BicycleTracker")), - // last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object; diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 421729753d419..3c73b9f19cfbb 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -47,7 +47,6 @@ BigVehicleTracker::BigVehicleTracker( const geometry_msgs::msg::Transform & self_transform) : Tracker(time, object.classification), logger_(rclcpp::get_logger("BigVehicleTracker")), - // last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z), tracking_offset_(Eigen::Vector2d::Zero()) { diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 04532270ece10..19fc5a2f71122 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -47,7 +47,6 @@ NormalVehicleTracker::NormalVehicleTracker( const geometry_msgs::msg::Transform & self_transform) : Tracker(time, object.classification), logger_(rclcpp::get_logger("NormalVehicleTracker")), - // last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z), tracking_offset_(Eigen::Vector2d::Zero()) { diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index e01558c75761c..e1c07388836f6 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -47,7 +47,6 @@ PedestrianTracker::PedestrianTracker( const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("PedestrianTracker")), - // last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object; diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp index 1201fb77e32c9..450bb2d94abb6 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -41,7 +41,6 @@ UnknownTracker::UnknownTracker( const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("UnknownTracker")), - // last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object;