From 7e617ccf965aae77170ea975914682a6db7e2135 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 13 Jun 2024 16:55:20 +0900 Subject: [PATCH] feat: impl. object model to pedestrian --- .../tracker/model/bicycle_tracker.hpp | 7 -- .../tracker/model/pedestrian_tracker.hpp | 8 +- .../tracker/object_model/object_model.hpp | 29 ++++--- .../src/tracker/model/bicycle_tracker.cpp | 7 +- .../src/tracker/model/big_vehicle_tracker.cpp | 7 +- .../tracker/model/normal_vehicle_tracker.cpp | 7 +- .../src/tracker/model/pedestrian_tracker.cpp | 87 +++++++++++-------- 7 files changed, 83 insertions(+), 69 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 0e66bcb62fccf..63ad496b70ed9 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,13 +32,6 @@ class BicycleTracker : public Tracker object_model::ObjectModel object_model_ = object_model::bicycle; - struct EkfParams - { - double r_cov_x; - double r_cov_y; - double r_cov_yaw; - } ekf_params_; - double z_; struct BoundingBox 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 3c9dd5e91dac5..c5be57f656eb5 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 @@ -22,6 +22,7 @@ #include "kalman_filter/kalman_filter.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" #include "multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" +#include "multi_object_tracker/tracker/object_model/object_model.hpp" // cspell: ignore CTRV @@ -31,12 +32,7 @@ class PedestrianTracker : public Tracker autoware_perception_msgs::msg::DetectedObject object_; rclcpp::Logger logger_; - struct EkfParams - { - double r_cov_x; - double r_cov_y; - double r_cov_yaw; - } ekf_params_; + object_model::ObjectModel object_model_ = object_model::pedestrian; double z_; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model.hpp index 1acd40dae40af..8f63dee9f872e 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model.hpp @@ -67,10 +67,14 @@ struct ObjectSizeLimit }; struct MotionProcessNoise { + double vel_long{0.0}; // [m/s] uncertain longitudinal velocity + double vel_lat{0.0}; // [m/s] uncertain lateral velocity + double yaw_rate{0.0}; // [rad/s] uncertain yaw rate + double yaw_rate_min{0.0}; // [rad/s] uncertain yaw rate, minimum + double yaw_rate_max{0.0}; // [rad/s] uncertain yaw rate, maximum double acc_long{0.0}; // [m/s^2] uncertain longitudinal acceleration double acc_lat{0.0}; // [m/s^2] uncertain lateral acceleration - double yaw_rate_min{0.0}; // [rad/s] uncertain yaw change rate, minimum - double yaw_rate_max{0.0}; // [rad/s] uncertain yaw change rate, maximum + double rotate_rate{0.0}; // [rad/s^2] uncertain rotation change rate }; struct MotionProcessLimit { @@ -78,6 +82,7 @@ struct MotionProcessLimit double acc_lat_max{0.0}; // [m/s^2] double vel_long_max{0.0}; // [m/s] double vel_lat_max{0.0}; // [m/s] + double yaw_rate_max{0.0}; // [rad/s] }; struct StateCovariance { @@ -256,17 +261,17 @@ class ObjectModel size_limit.length_max = 2.0; size_limit.width_min = 0.3; size_limit.width_max = 1.0; - size_limit.height_min = 1.0; + size_limit.height_min = 0.6; size_limit.height_max = 2.0; - process_noise.acc_long = const_g * 0.35; - process_noise.acc_lat = const_g * 0.15; - process_noise.yaw_rate_min = 1.5; // deg2rad(1.5); - process_noise.yaw_rate_max = 15.0; // deg2rad(15.0); + process_noise.vel_long = 0.5; + process_noise.vel_lat = 0.5; + process_noise.yaw_rate = deg2rad(20.0); + process_noise.acc_long = const_g * 0.3; + process_noise.rotate_rate = deg2rad(30.0); - process_limit.acc_long_max = const_g; - process_limit.acc_lat_max = const_g; process_limit.vel_long_max = kmph2mps(100.0); + process_limit.yaw_rate_max = deg2rad(30.0); // initial covariance initial_covariance.pos_x = sq(1.0); @@ -276,10 +281,10 @@ class ObjectModel initial_covariance.vel_lat = sq(0.2); // measurement noise model - measurement_covariance.pos_x = sq(0.5); + measurement_covariance.pos_x = sq(0.4); measurement_covariance.pos_y = sq(0.4); - measurement_covariance.yaw = sq(deg2rad(20.0)); - measurement_covariance.vel_long = sq(kmph2mps(10.0)); + measurement_covariance.yaw = sq(deg2rad(30.0)); + break; default: 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 76642894b5d21..262a6cfce8803 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -166,9 +166,10 @@ autoware_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( auto r_cov_y = object_model_.measurement_covariance.pos_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_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const bool is_yaw_available = + object.kinematics.orientation_availability != + autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; // fill covariance matrix using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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 f43c24a488656..58037e858f701 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 @@ -206,9 +206,10 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje } // yaw angle fix - double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - bool is_yaw_available = object.kinematics.orientation_availability != - autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const bool is_yaw_available = + object.kinematics.orientation_availability != + autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; // fill covariance matrix using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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 cfef71828e5c4..080a3cefcc50b 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 @@ -207,9 +207,10 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO } // yaw angle fix - double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - bool is_yaw_available = object.kinematics.orientation_availability != - autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const bool is_yaw_available = + object.kinematics.orientation_availability != + autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; // fill covariance matrix using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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 677b18f289eb9..cfc4ae236f28a 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -55,18 +55,11 @@ PedestrianTracker::PedestrianTracker( // initialize existence probability initializeExistenceProbabilities(channel_index, object.existence_probability); - // Initialize parameters - // measurement noise covariance - float r_stddev_x = 0.4; // [m] - float r_stddev_y = 0.4; // [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [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); - // OBJECT SHAPE MODEL - bounding_box_ = {0.5, 0.5, 1.7}; - cylinder_ = {0.5, 1.7}; + bounding_box_ = { + object_model_.init_size.length, object_model_.init_size.width, + object_model_.init_size.height}; // default value + cylinder_ = {object_model_.init_size.length, object_model_.init_size.height}; // default value if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; @@ -76,21 +69,24 @@ PedestrianTracker::PedestrianTracker( // do not update polygon shape } // set maximum and minimum size - constexpr double max_size = 5.0; - constexpr double min_size = 0.3; - bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); - bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); - bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); - cylinder_.width = std::min(std::max(cylinder_.width, min_size), max_size); - cylinder_.height = std::min(std::max(cylinder_.height, min_size), max_size); + bounding_box_.length = std::clamp( + bounding_box_.length, object_model_.size_limit.length_min, object_model_.size_limit.length_max); + bounding_box_.width = std::clamp( + bounding_box_.width, object_model_.size_limit.width_min, object_model_.size_limit.width_max); + bounding_box_.height = std::clamp( + bounding_box_.height, object_model_.size_limit.height_min, object_model_.size_limit.height_max); + cylinder_.width = std::clamp( + cylinder_.width, object_model_.size_limit.length_min, object_model_.size_limit.length_max); + cylinder_.height = std::clamp( + cylinder_.height, object_model_.size_limit.height_min, object_model_.size_limit.height_max); // 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)] + const double q_stddev_x = object_model_.process_noise.vel_long; + const double q_stddev_y = object_model_.process_noise.vel_lat; + const double q_stddev_yaw = object_model_.process_noise.yaw_rate; + const double q_stddev_vx = object_model_.process_noise.acc_long; + const double q_stddev_wz = object_model_.process_noise.rotate_rate; motion_model_.setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vx, q_stddev_wz); } @@ -167,12 +163,19 @@ autoware_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObje // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { + // measurement noise covariance + auto r_cov_x = object_model_.measurement_covariance.pos_x; + auto r_cov_y = object_model_.measurement_covariance.pos_y; + + // yaw angle fix + const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const bool is_yaw_available = + object.kinematics.orientation_availability != + autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + // fill covariance matrix using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - 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); const double sin_yaw = std::sin(pose_yaw); const double sin_2yaw = std::sin(2.0f * pose_yaw); @@ -180,9 +183,20 @@ autoware_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObje r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y pose_cov[XYZRPY_COV_IDX::Y_Y] = - r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y - pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x + r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y + pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x + pose_cov[XYZRPY_COV_IDX::X_YAW] = 0.0; // x - yaw + pose_cov[XYZRPY_COV_IDX::Y_YAW] = 0.0; // y - yaw + pose_cov[XYZRPY_COV_IDX::YAW_X] = 0.0; // yaw - x + pose_cov[XYZRPY_COV_IDX::YAW_Y] = 0.0; // yaw - y + pose_cov[XYZRPY_COV_IDX::YAW_YAW] = object_model_.measurement_covariance.yaw; // yaw - yaw + if (!is_yaw_available) { + pose_cov[XYZRPY_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value + } + auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance; + twist_cov[XYZRPY_COV_IDX::X_X] = object_model_.measurement_covariance.vel_long; // vel - vel } + return updating_object; } @@ -246,13 +260,16 @@ bool PedestrianTracker::measureWithShape( } // set maximum and minimum size - constexpr double max_size = 5.0; - constexpr double min_size = 0.3; - bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); - bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); - bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); - cylinder_.width = std::min(std::max(cylinder_.width, min_size), max_size); - cylinder_.height = std::min(std::max(cylinder_.height, min_size), max_size); + bounding_box_.length = std::clamp( + bounding_box_.length, object_model_.size_limit.length_min, object_model_.size_limit.length_max); + bounding_box_.width = std::clamp( + bounding_box_.width, object_model_.size_limit.width_min, object_model_.size_limit.width_max); + bounding_box_.height = std::clamp( + bounding_box_.height, object_model_.size_limit.height_min, object_model_.size_limit.height_max); + cylinder_.width = std::clamp( + cylinder_.width, object_model_.size_limit.length_min, object_model_.size_limit.length_max); + cylinder_.height = std::clamp( + cylinder_.height, object_model_.size_limit.height_min, object_model_.size_limit.height_max); return true; }