From d6e50a95e09211e17efa9720c8e7cb6ab402e17b Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 6 Jun 2024 15:28:34 +0900 Subject: [PATCH 01/30] feat: separate filters Signed-off-by: Taekjin LEE --- .../filter/lidar_object_validator.launch.xml | 35 +++++++++++++++++++ 1 file changed, 35 insertions(+) create mode 100644 launch/tier4_perception_launch/launch/object_recognition/detection/filter/lidar_object_validator.launch.xml diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/filter/lidar_object_validator.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/lidar_object_validator.launch.xml new file mode 100644 index 0000000000000..40330d544234f --- /dev/null +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/lidar_object_validator.launch.xml @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 24f66cd4df59bd32e935686592ee8146c060f623 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 7 Jun 2024 09:58:20 +0900 Subject: [PATCH 02/30] fix: object validator to modular Signed-off-by: Taekjin LEE --- .../filter/lidar_object_validator.launch.xml | 35 ------------------- 1 file changed, 35 deletions(-) delete mode 100644 launch/tier4_perception_launch/launch/object_recognition/detection/filter/lidar_object_validator.launch.xml diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/filter/lidar_object_validator.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/lidar_object_validator.launch.xml deleted file mode 100644 index 40330d544234f..0000000000000 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/filter/lidar_object_validator.launch.xml +++ /dev/null @@ -1,35 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - From c5eeaa5a0b6cf03971fa08ee263fcec2776b202d Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 30 May 2024 14:09:01 +0900 Subject: [PATCH 03/30] fix: replace xyzrpy covariance index Signed-off-by: Taekjin LEE --- .../multi_object_tracker/utils/utils.hpp | 91 ------------------ .../src/tracker/model/bicycle_tracker.cpp | 27 +++--- .../src/tracker/model/big_vehicle_tracker.cpp | 43 ++++----- .../tracker/model/normal_vehicle_tracker.cpp | 43 ++++----- .../tracker/model/pass_through_tracker.cpp | 31 +++--- .../src/tracker/model/pedestrian_tracker.cpp | 32 ++++--- .../src/tracker/model/unknown_tracker.cpp | 50 +++++----- .../motion_model/bicycle_motion_model.cpp | 94 ++++++++++--------- .../motion_model/ctrv_motion_model.cpp | 94 ++++++++++--------- .../tracker/motion_model/cv_motion_model.cpp | 66 ++++++------- 10 files changed, 248 insertions(+), 323 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 259a58342c369..5c7a79299cdae 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 @@ -38,45 +38,6 @@ namespace utils { -enum MSG_COV_IDX { - X_X = 0, - X_Y = 1, - X_Z = 2, - X_ROLL = 3, - X_PITCH = 4, - X_YAW = 5, - Y_X = 6, - Y_Y = 7, - Y_Z = 8, - Y_ROLL = 9, - Y_PITCH = 10, - Y_YAW = 11, - Z_X = 12, - Z_Y = 13, - Z_Z = 14, - Z_ROLL = 15, - Z_PITCH = 16, - Z_YAW = 17, - ROLL_X = 18, - ROLL_Y = 19, - ROLL_Z = 20, - ROLL_ROLL = 21, - ROLL_PITCH = 22, - ROLL_YAW = 23, - PITCH_X = 24, - PITCH_Y = 25, - PITCH_Z = 26, - PITCH_ROLL = 27, - PITCH_PITCH = 28, - PITCH_YAW = 29, - YAW_X = 30, - YAW_Y = 31, - YAW_Z = 32, - YAW_ROLL = 33, - YAW_PITCH = 34, - YAW_YAW = 35 -}; - enum BBOX_IDX { FRONT_SURFACE = 0, RIGHT_SURFACE = 1, @@ -155,34 +116,6 @@ inline int getNearestCornerOrSurface( return labels[xgrid][ygrid]; // 0 to 7 + 1(null) value } -/** - * @brief Get the Nearest Corner or Surface from detected object - * @param object: input object - * @param yaw: object yaw angle (after solved front and back uncertainty) - * @param self_transform - * @return nearest corner or surface index - */ -inline int getNearestCornerOrSurfaceFromObject( - const autoware_perception_msgs::msg::DetectedObject & object, const double & yaw, - const geometry_msgs::msg::Transform & self_transform) -{ - // only work for BBOX shape - if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { - return BBOX_IDX::INVALID; - } - - // extract necessary information from input object - double x, y, width, length; - x = object.kinematics.pose_with_covariance.pose.position.x; - y = object.kinematics.pose_with_covariance.pose.position.y; - // yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); //not use raw yaw - // now - width = object.shape.dimensions.y; - length = object.shape.dimensions.x; - - return getNearestCornerOrSurface(x, y, yaw, width, length, self_transform); -} - /** * @brief Calc bounding box center offset caused by shape change * @param dw: width update [m] = w_new - w_old @@ -225,30 +158,6 @@ inline Eigen::Vector2d calcOffsetVectorFromShapeChange( return offset; // do nothing if indx == INVALID or INSIDE } -/** - * @brief post-processing to recover bounding box center from tracking point and offset vector - * @param x: x of tracking point estimated with ekf - * @param y: y of tracking point estimated with ekf - * @param yaw: yaw of tracking point estimated with ekf - * @param dw: diff of width: w_estimated - w_input - * @param dl: diff of length: l_estimated - l_input - * @param indx: closest corner or surface index - * @param tracking_offset: tracking offset between bounding box center and tracking point - */ -inline Eigen::Vector2d recoverFromTrackingPoint( - const double x, const double y, const double yaw, const double dw, const double dl, - const int indx, const Eigen::Vector2d & tracking_offset) -{ - const Eigen::Vector2d tracking_point{x, y}; - const Eigen::Matrix2d R = Eigen::Rotation2Dd(yaw).toRotationMatrix(); - - const Eigen::Vector2d shape_change_offset = calcOffsetVectorFromShapeChange(dw, dl, indx); - - Eigen::Vector2d output_center = tracking_point - R * tracking_offset - R * shape_change_offset; - - return output_center; -} - /** * @brief Convert input object center to tracking point based on nearest corner information * 1. update anchor offset vector, 2. offset input bbox based on tracking_offset vector and 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 05e06107c2247..826488be653d4 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -109,6 +110,7 @@ BicycleTracker::BicycleTracker( // Set initial state { + using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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); @@ -134,13 +136,11 @@ BicycleTracker::BicycleTracker( 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; + pose_cov[XYZRPY_COV_IDX::X_X] = p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; + pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; + pose_cov[XYZRPY_COV_IDX::Y_Y] = p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; + pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; + pose_cov[XYZRPY_COV_IDX::YAW_YAW] = p0_cov_yaw; } if (!object.kinematics.has_twist_covariance) { @@ -148,7 +148,7 @@ BicycleTracker::BicycleTracker( autoware::universe_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]; + vel_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X]; } const double slip = 0.0; @@ -185,12 +185,13 @@ autoware_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { // fill covariance matrix + using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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 + pose_cov[XYZRPY_COV_IDX::X_X] = ekf_params_.r_cov_x; // x - x + pose_cov[XYZRPY_COV_IDX::X_Y] = 0; // x - y + pose_cov[XYZRPY_COV_IDX::Y_X] = 0; // y - x + pose_cov[XYZRPY_COV_IDX::Y_Y] = ekf_params_.r_cov_y; // y - y + pose_cov[XYZRPY_COV_IDX::YAW_YAW] = ekf_params_.r_cov_yaw; // yaw - yaw } return updating_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 4414d8a21ca01..0eeec58e118d7 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 @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -127,6 +128,7 @@ BigVehicleTracker::BigVehicleTracker( // Set initial state { + using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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); @@ -152,13 +154,11 @@ BigVehicleTracker::BigVehicleTracker( 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; + pose_cov[XYZRPY_COV_IDX::X_X] = p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; + pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; + pose_cov[XYZRPY_COV_IDX::Y_Y] = p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; + pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; + pose_cov[XYZRPY_COV_IDX::YAW_YAW] = p0_cov_yaw; } if (!object.kinematics.has_twist_covariance) { @@ -166,7 +166,7 @@ BigVehicleTracker::BigVehicleTracker( autoware::universe_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]; + vel_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X]; } const double slip = 0.0; @@ -243,26 +243,27 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; // fill covariance matrix + using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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 + pose_cov[XYZRPY_COV_IDX::X_X] = + 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 + 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] = 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 + 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[utils::MSG_COV_IDX::X_X] = ekf_params_.r_cov_vel; // vel - vel + twist_cov[XYZRPY_COV_IDX::X_X] = ekf_params_.r_cov_vel; // vel - vel } return updating_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 1fff1a13add65..981fbea5ab3c8 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 @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -128,6 +129,7 @@ NormalVehicleTracker::NormalVehicleTracker( // Set initial state { + using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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); @@ -153,13 +155,11 @@ NormalVehicleTracker::NormalVehicleTracker( 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; + pose_cov[XYZRPY_COV_IDX::X_X] = p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; + pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; + pose_cov[XYZRPY_COV_IDX::Y_Y] = p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; + pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; + pose_cov[XYZRPY_COV_IDX::YAW_YAW] = p0_cov_yaw; } if (!object.kinematics.has_twist_covariance) { @@ -167,7 +167,7 @@ NormalVehicleTracker::NormalVehicleTracker( autoware::universe_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]; + vel_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X]; } const double slip = 0.0; @@ -245,26 +245,27 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; // fill covariance matrix + using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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 + pose_cov[XYZRPY_COV_IDX::X_X] = + 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 + 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] = 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 + 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[utils::MSG_COV_IDX::X_X] = ekf_params_.r_cov_vel; // vel - vel + twist_cov[XYZRPY_COV_IDX::X_X] = ekf_params_.r_cov_vel; // vel - vel } return updating_object; diff --git a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp index 3e4f23cd440bc..b0d202036d79e 100644 --- a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp @@ -16,6 +16,8 @@ // Author: v1.0 Yutaka Shimizu // +#include + #include #include #include @@ -86,25 +88,26 @@ bool PassThroughTracker::measure( bool PassThroughTracker::getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const { + using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; object = object_recognition_utils::toTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X] = 0.0; - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y] = 0.0; - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X] = 0.0; - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y] = 0.0; - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Z_Z] = 0.0; - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = 0.0; - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = 0.0; - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW] = 0.0; + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_X] = 0.0; + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_Y] = 0.0; + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_X] = 0.0; + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y] = 0.0; + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Z_Z] = 0.0; + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::ROLL_ROLL] = 0.0; + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::PITCH_PITCH] = 0.0; + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::YAW_YAW] = 0.0; // twist covariance - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X] = 0.0; - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y] = 0.0; - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Z_Z] = 0.0; - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = 0.0; - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = 0.0; - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW] = 0.0; + object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X] = 0.0; + object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y] = 0.0; + object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::Z_Z] = 0.0; + object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::ROLL_ROLL] = 0.0; + object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::PITCH_PITCH] = 0.0; + object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::YAW_YAW] = 0.0; const double dt = (time - last_update_time_).seconds(); if (0.5 /*500msec*/ < dt) { 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 887810066f038..4b605aa2a8cf6 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -102,6 +103,7 @@ PedestrianTracker::PedestrianTracker( // Set initial state { + using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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); @@ -129,13 +131,11 @@ PedestrianTracker::PedestrianTracker( 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; + pose_cov[XYZRPY_COV_IDX::X_X] = p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; + pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; + pose_cov[XYZRPY_COV_IDX::Y_Y] = p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; + pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; + pose_cov[XYZRPY_COV_IDX::YAW_YAW] = p0_cov_yaw; } if (!object.kinematics.has_twist_covariance) { @@ -146,8 +146,8 @@ PedestrianTracker::PedestrianTracker( 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]; + vel_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X]; + wz_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::YAW_YAW]; } // initialize motion model @@ -168,6 +168,8 @@ autoware_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObje // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { + // 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; @@ -175,12 +177,12 @@ autoware_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObje 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[XYZRPY_COV_IDX::X_X] = + 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 } return updating_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 8e484623ef0aa..af4cd7f3df8ab 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -73,6 +74,7 @@ UnknownTracker::UnknownTracker( // Set initial state { + using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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; @@ -98,12 +100,10 @@ UnknownTracker::UnknownTracker( 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[XYZRPY_COV_IDX::X_X] = p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; + pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; + pose_cov[XYZRPY_COV_IDX::Y_Y] = p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; + pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; } if (!object.kinematics.has_twist_covariance) { @@ -111,24 +111,24 @@ UnknownTracker::UnknownTracker( constexpr double p0_stddev_vy = autoware::universe_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; + twist_cov[XYZRPY_COV_IDX::X_X] = p0_cov_vx; + twist_cov[XYZRPY_COV_IDX::X_Y] = 0.0; + twist_cov[XYZRPY_COV_IDX::Y_X] = 0.0; + twist_cov[XYZRPY_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]; + twist_cov_rotate(0, 0) = twist_cov[XYZRPY_COV_IDX::X_X]; + twist_cov_rotate(0, 1) = twist_cov[XYZRPY_COV_IDX::X_Y]; + twist_cov_rotate(1, 0) = twist_cov[XYZRPY_COV_IDX::Y_X]; + twist_cov_rotate(1, 1) = twist_cov[XYZRPY_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); + twist_cov[XYZRPY_COV_IDX::X_X] = twist_cov_rotated(0, 0); + twist_cov[XYZRPY_COV_IDX::X_Y] = twist_cov_rotated(0, 1); + twist_cov[XYZRPY_COV_IDX::Y_X] = twist_cov_rotated(1, 0); + twist_cov[XYZRPY_COV_IDX::Y_Y] = twist_cov_rotated(1, 1); // initialize motion model motion_model_.initialize(time, x, y, pose_cov, vx, vy, twist_cov); @@ -148,6 +148,8 @@ autoware_perception_msgs::msg::DetectedObject UnknownTracker::getUpdatingObject( // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { + // 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; @@ -155,12 +157,12 @@ autoware_perception_msgs::msg::DetectedObject UnknownTracker::getUpdatingObject( 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[XYZRPY_COV_IDX::X_X] = + 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 } return updating_object; } diff --git a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp index f21855c2356ef..419db77c4849a 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp @@ -23,6 +23,7 @@ #include #include +#include #define EIGEN_MPL2_ONLY #include @@ -31,6 +32,7 @@ // cspell: ignore CTRV // Bicycle CTRV motion model // CTRV : Constant Turn Rate and constant Velocity +using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; BicycleMotionModel::BicycleMotionModel() : MotionModel(), logger_(rclcpp::get_logger("BicycleMotionModel")) @@ -117,9 +119,9 @@ bool BicycleMotionModel::initialize( // 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::X, IDX::X) = pose_cov[XYZRPY_COV_IDX::X_X]; + P(IDX::Y, IDX::Y) = pose_cov[XYZRPY_COV_IDX::Y_Y]; + P(IDX::YAW, IDX::YAW) = pose_cov[XYZRPY_COV_IDX::YAW_YAW]; P(IDX::VEL, IDX::VEL) = vel_cov; P(IDX::SLIP, IDX::SLIP) = slip_cov; @@ -147,10 +149,10 @@ bool BicycleMotionModel::updateStatePose( 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]; + R(0, 0) = pose_cov[XYZRPY_COV_IDX::X_X]; + R(0, 1) = pose_cov[XYZRPY_COV_IDX::X_Y]; + R(1, 0) = pose_cov[XYZRPY_COV_IDX::Y_X]; + R(1, 1) = pose_cov[XYZRPY_COV_IDX::Y_Y]; return ekf_.update(Y, C, R); } @@ -186,15 +188,15 @@ bool BicycleMotionModel::updateStatePoseHead( 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]; + R(0, 0) = pose_cov[XYZRPY_COV_IDX::X_X]; + R(0, 1) = pose_cov[XYZRPY_COV_IDX::X_Y]; + R(1, 0) = pose_cov[XYZRPY_COV_IDX::Y_X]; + R(1, 1) = pose_cov[XYZRPY_COV_IDX::Y_Y]; + R(0, 2) = pose_cov[XYZRPY_COV_IDX::X_YAW]; + R(1, 2) = pose_cov[XYZRPY_COV_IDX::Y_YAW]; + R(2, 0) = pose_cov[XYZRPY_COV_IDX::YAW_X]; + R(2, 1) = pose_cov[XYZRPY_COV_IDX::YAW_Y]; + R(2, 2) = pose_cov[XYZRPY_COV_IDX::YAW_YAW]; return ekf_.update(Y, C, R); } @@ -232,16 +234,16 @@ bool BicycleMotionModel::updateStatePoseHeadVel( 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]; + R(0, 0) = pose_cov[XYZRPY_COV_IDX::X_X]; + R(0, 1) = pose_cov[XYZRPY_COV_IDX::X_Y]; + R(1, 0) = pose_cov[XYZRPY_COV_IDX::Y_X]; + R(1, 1) = pose_cov[XYZRPY_COV_IDX::Y_Y]; + R(0, 2) = pose_cov[XYZRPY_COV_IDX::X_YAW]; + R(1, 2) = pose_cov[XYZRPY_COV_IDX::Y_YAW]; + R(2, 0) = pose_cov[XYZRPY_COV_IDX::YAW_X]; + R(2, 1) = pose_cov[XYZRPY_COV_IDX::YAW_Y]; + R(2, 2) = pose_cov[XYZRPY_COV_IDX::YAW_YAW]; + R(3, 3) = twist_cov[XYZRPY_COV_IDX::X_X]; return ekf_.update(Y, C, R); } @@ -445,14 +447,14 @@ bool BicycleMotionModel::getPredictedState( 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; + pose_cov[XYZRPY_COV_IDX::X_X] = P(IDX::X, IDX::X); + pose_cov[XYZRPY_COV_IDX::X_Y] = P(IDX::X, IDX::Y); + pose_cov[XYZRPY_COV_IDX::Y_X] = P(IDX::Y, IDX::X); + pose_cov[XYZRPY_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); + pose_cov[XYZRPY_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); + pose_cov[XYZRPY_COV_IDX::Z_Z] = zz_cov; + pose_cov[XYZRPY_COV_IDX::ROLL_ROLL] = rr_cov; + pose_cov[XYZRPY_COV_IDX::PITCH_PITCH] = pp_cov; // set twist covariance Eigen::MatrixXd cov_jacob(3, 2); @@ -466,18 +468,18 @@ bool BicycleMotionModel::getPredictedState( 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; + twist_cov[XYZRPY_COV_IDX::X_X] = twist_cov_mat(0, 0); + twist_cov[XYZRPY_COV_IDX::X_Y] = twist_cov_mat(0, 1); + twist_cov[XYZRPY_COV_IDX::X_YAW] = twist_cov_mat(0, 2); + twist_cov[XYZRPY_COV_IDX::Y_X] = twist_cov_mat(1, 0); + twist_cov[XYZRPY_COV_IDX::Y_Y] = twist_cov_mat(1, 1); + twist_cov[XYZRPY_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); + twist_cov[XYZRPY_COV_IDX::YAW_X] = twist_cov_mat(2, 0); + twist_cov[XYZRPY_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); + twist_cov[XYZRPY_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); + twist_cov[XYZRPY_COV_IDX::Z_Z] = vz_cov; + twist_cov[XYZRPY_COV_IDX::ROLL_ROLL] = wx_cov; + twist_cov[XYZRPY_COV_IDX::PITCH_PITCH] = wy_cov; return true; } diff --git a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp index 79ce6e5dbb61c..15741ac26899d 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp @@ -23,6 +23,7 @@ #include #include +#include #define EIGEN_MPL2_ONLY #include @@ -30,6 +31,7 @@ // cspell: ignore CTRV // Constant Turn Rate and constant Velocity (CTRV) motion model +using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; CTRVMotionModel::CTRVMotionModel() : MotionModel(), logger_(rclcpp::get_logger("CTRVMotionModel")) { @@ -88,9 +90,9 @@ bool CTRVMotionModel::initialize( // 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::X, IDX::X) = pose_cov[XYZRPY_COV_IDX::X_X]; + P(IDX::Y, IDX::Y) = pose_cov[XYZRPY_COV_IDX::Y_Y]; + P(IDX::YAW, IDX::YAW) = pose_cov[XYZRPY_COV_IDX::YAW_YAW]; P(IDX::VEL, IDX::VEL) = vel_cov; P(IDX::WZ, IDX::WZ) = wz_cov; @@ -115,10 +117,10 @@ bool CTRVMotionModel::updateStatePose( 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]; + R(0, 0) = pose_cov[XYZRPY_COV_IDX::X_X]; + R(0, 1) = pose_cov[XYZRPY_COV_IDX::X_Y]; + R(1, 0) = pose_cov[XYZRPY_COV_IDX::Y_X]; + R(1, 1) = pose_cov[XYZRPY_COV_IDX::Y_Y]; return ekf_.update(Y, C, R); } @@ -154,15 +156,15 @@ bool CTRVMotionModel::updateStatePoseHead( 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]; + R(0, 0) = pose_cov[XYZRPY_COV_IDX::X_X]; + R(0, 1) = pose_cov[XYZRPY_COV_IDX::X_Y]; + R(1, 0) = pose_cov[XYZRPY_COV_IDX::Y_X]; + R(1, 1) = pose_cov[XYZRPY_COV_IDX::Y_Y]; + R(0, 2) = pose_cov[XYZRPY_COV_IDX::X_YAW]; + R(1, 2) = pose_cov[XYZRPY_COV_IDX::Y_YAW]; + R(2, 0) = pose_cov[XYZRPY_COV_IDX::YAW_X]; + R(2, 1) = pose_cov[XYZRPY_COV_IDX::YAW_Y]; + R(2, 2) = pose_cov[XYZRPY_COV_IDX::YAW_YAW]; return ekf_.update(Y, C, R); } @@ -200,16 +202,16 @@ bool CTRVMotionModel::updateStatePoseHeadVel( 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]; + R(0, 0) = pose_cov[XYZRPY_COV_IDX::X_X]; + R(0, 1) = pose_cov[XYZRPY_COV_IDX::X_Y]; + R(1, 0) = pose_cov[XYZRPY_COV_IDX::Y_X]; + R(1, 1) = pose_cov[XYZRPY_COV_IDX::Y_Y]; + R(0, 2) = pose_cov[XYZRPY_COV_IDX::X_YAW]; + R(1, 2) = pose_cov[XYZRPY_COV_IDX::Y_YAW]; + R(2, 0) = pose_cov[XYZRPY_COV_IDX::YAW_X]; + R(2, 1) = pose_cov[XYZRPY_COV_IDX::YAW_Y]; + R(2, 2) = pose_cov[XYZRPY_COV_IDX::YAW_YAW]; + R(3, 3) = twist_cov[XYZRPY_COV_IDX::X_X]; return ekf_.update(Y, C, R); } @@ -355,32 +357,32 @@ bool CTRVMotionModel::getPredictedState( 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; + pose_cov[XYZRPY_COV_IDX::X_X] = P(IDX::X, IDX::X); + pose_cov[XYZRPY_COV_IDX::X_Y] = P(IDX::X, IDX::Y); + pose_cov[XYZRPY_COV_IDX::Y_X] = P(IDX::Y, IDX::X); + pose_cov[XYZRPY_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); + pose_cov[XYZRPY_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); + pose_cov[XYZRPY_COV_IDX::Z_Z] = zz_cov; + pose_cov[XYZRPY_COV_IDX::ROLL_ROLL] = rr_cov; + pose_cov[XYZRPY_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] = 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; - twist_cov[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; - twist_cov[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; + twist_cov[XYZRPY_COV_IDX::X_X] = P(IDX::VEL, IDX::VEL); + twist_cov[XYZRPY_COV_IDX::X_Y] = 0.0; + twist_cov[XYZRPY_COV_IDX::X_YAW] = P(IDX::VEL, IDX::WZ); + twist_cov[XYZRPY_COV_IDX::Y_X] = 0.0; + twist_cov[XYZRPY_COV_IDX::Y_Y] = vy_cov; + twist_cov[XYZRPY_COV_IDX::Y_YAW] = 0.0; + twist_cov[XYZRPY_COV_IDX::YAW_X] = P(IDX::WZ, IDX::VEL); + twist_cov[XYZRPY_COV_IDX::YAW_Y] = 0.0; + twist_cov[XYZRPY_COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ); + twist_cov[XYZRPY_COV_IDX::Z_Z] = vz_cov; + twist_cov[XYZRPY_COV_IDX::ROLL_ROLL] = wx_cov; + twist_cov[XYZRPY_COV_IDX::PITCH_PITCH] = wy_cov; return true; } diff --git a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp index 8b3e9014f52d7..7800c24ec0b19 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp @@ -23,6 +23,7 @@ #include #include +#include #include @@ -32,6 +33,7 @@ // cspell: ignore CV // Constant Velocity (CV) motion model +using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; CVMotionModel::CVMotionModel() : MotionModel(), logger_(rclcpp::get_logger("CVMotionModel")) { @@ -87,10 +89,10 @@ bool CVMotionModel::initialize( // 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]; + P(IDX::X, IDX::X) = pose_cov[XYZRPY_COV_IDX::X_X]; + P(IDX::Y, IDX::Y) = pose_cov[XYZRPY_COV_IDX::Y_Y]; + P(IDX::VX, IDX::VX) = twist_cov[XYZRPY_COV_IDX::X_X]; + P(IDX::VY, IDX::VY) = twist_cov[XYZRPY_COV_IDX::Y_Y]; return MotionModel::initialize(time, X, P); } @@ -113,10 +115,10 @@ bool CVMotionModel::updateStatePose( 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]; + R(0, 0) = pose_cov[XYZRPY_COV_IDX::X_X]; + R(0, 1) = pose_cov[XYZRPY_COV_IDX::X_Y]; + R(1, 0) = pose_cov[XYZRPY_COV_IDX::Y_X]; + R(1, 1) = pose_cov[XYZRPY_COV_IDX::Y_Y]; return ekf_.update(Y, C, R); } @@ -142,14 +144,14 @@ bool CVMotionModel::updateStatePoseVel( 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]; + R(0, 0) = pose_cov[XYZRPY_COV_IDX::X_X]; + R(0, 1) = pose_cov[XYZRPY_COV_IDX::X_Y]; + R(1, 0) = pose_cov[XYZRPY_COV_IDX::Y_X]; + R(1, 1) = pose_cov[XYZRPY_COV_IDX::Y_Y]; + R(2, 2) = twist_cov[XYZRPY_COV_IDX::X_X]; + R(2, 3) = twist_cov[XYZRPY_COV_IDX::X_Y]; + R(3, 2) = twist_cov[XYZRPY_COV_IDX::Y_X]; + R(3, 3) = twist_cov[XYZRPY_COV_IDX::Y_Y]; return ekf_.update(Y, C, R); } @@ -272,14 +274,14 @@ bool CVMotionModel::getPredictedState( 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; + pose_cov[XYZRPY_COV_IDX::X_X] = P(IDX::X, IDX::X); + pose_cov[XYZRPY_COV_IDX::X_Y] = P(IDX::X, IDX::Y); + pose_cov[XYZRPY_COV_IDX::Y_X] = P(IDX::Y, IDX::X); + pose_cov[XYZRPY_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); + pose_cov[XYZRPY_COV_IDX::Z_Z] = zz_cov; + pose_cov[XYZRPY_COV_IDX::ROLL_ROLL] = rr_cov; + pose_cov[XYZRPY_COV_IDX::PITCH_PITCH] = pp_cov; + pose_cov[XYZRPY_COV_IDX::YAW_YAW] = yaw_cov; // set twist covariance constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative @@ -294,14 +296,14 @@ bool CVMotionModel::getPredictedState( twist_cov_rotate(1, 1) = P(IDX::VY, IDX::VY); 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; - twist_cov[utils::MSG_COV_IDX::YAW_YAW] = wz_cov; + twist_cov[XYZRPY_COV_IDX::X_X] = twist_cov_rotated(0, 0); + twist_cov[XYZRPY_COV_IDX::X_Y] = twist_cov_rotated(0, 1); + twist_cov[XYZRPY_COV_IDX::Y_X] = twist_cov_rotated(1, 0); + twist_cov[XYZRPY_COV_IDX::Y_Y] = twist_cov_rotated(1, 1); + twist_cov[XYZRPY_COV_IDX::Z_Z] = vz_cov; + twist_cov[XYZRPY_COV_IDX::ROLL_ROLL] = wx_cov; + twist_cov[XYZRPY_COV_IDX::PITCH_PITCH] = wy_cov; + twist_cov[XYZRPY_COV_IDX::YAW_YAW] = wz_cov; return true; } From 68d9964b7ab2475aa638a9d19efb2cfb9d004306 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 4 Jun 2024 15:54:29 +0900 Subject: [PATCH 04/30] chore: refactor tracker class members Signed-off-by: Taekjin LEE --- .../multi_object_tracker/tracker/model/bicycle_tracker.hpp | 3 --- .../multi_object_tracker/tracker/model/big_vehicle_tracker.hpp | 3 --- .../tracker/model/normal_vehicle_tracker.hpp | 3 --- .../multi_object_tracker/tracker/model/pedestrian_tracker.hpp | 3 --- .../multi_object_tracker/tracker/model/unknown_tracker.hpp | 3 --- 5 files changed, 15 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 36176b8202e72..8e0bfa91342df 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 @@ -30,7 +30,6 @@ class BicycleTracker : public Tracker autoware_perception_msgs::msg::DetectedObject object_; rclcpp::Logger logger_; -private: struct EkfParams { double r_cov_x; @@ -48,9 +47,7 @@ class BicycleTracker : public Tracker }; BoundingBox bounding_box_; -private: BicycleMotionModel motion_model_; - const char DIM = motion_model_.DIM; using IDX = BicycleMotionModel::IDX; public: 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 8eec03c5dc847..4e4e27a918e89 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 @@ -30,7 +30,6 @@ class BigVehicleTracker : public Tracker autoware_perception_msgs::msg::DetectedObject object_; rclcpp::Logger logger_; -private: struct EkfParams { double r_cov_x; @@ -51,9 +50,7 @@ class BigVehicleTracker : public Tracker BoundingBox bounding_box_; Eigen::Vector2d tracking_offset_; -private: BicycleMotionModel motion_model_; - const char DIM = motion_model_.DIM; using IDX = BicycleMotionModel::IDX; public: 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 107a3ef194afb..04a7fef055219 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 @@ -30,7 +30,6 @@ class NormalVehicleTracker : public Tracker autoware_perception_msgs::msg::DetectedObject object_; rclcpp::Logger logger_; -private: struct EkfParams { double r_cov_x; @@ -51,9 +50,7 @@ class NormalVehicleTracker : public Tracker BoundingBox bounding_box_; Eigen::Vector2d tracking_offset_; -private: BicycleMotionModel motion_model_; - const char DIM = motion_model_.DIM; using IDX = BicycleMotionModel::IDX; public: 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 e5d718c4b15ee..6fb384c419373 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 @@ -32,7 +32,6 @@ class PedestrianTracker : public Tracker autoware_perception_msgs::msg::DetectedObject object_; rclcpp::Logger logger_; -private: struct EkfParams { double r_cov_x; @@ -56,9 +55,7 @@ class PedestrianTracker : public Tracker BoundingBox bounding_box_; Cylinder cylinder_; -private: CTRVMotionModel motion_model_; - const char DIM = motion_model_.DIM; using IDX = CTRVMotionModel::IDX; public: 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 43a1f2fd14842..ee0b46d05133b 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 @@ -30,7 +30,6 @@ class UnknownTracker : public Tracker autoware_perception_msgs::msg::DetectedObject object_; rclcpp::Logger logger_; -private: struct EkfParams { double r_cov_x; @@ -41,9 +40,7 @@ class UnknownTracker : public Tracker double z_; -private: CVMotionModel motion_model_; - const char DIM = motion_model_.DIM; using IDX = CVMotionModel::IDX; public: From 6a752a88c7d1495c5946245cd2a69b6dfe9089d4 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 4 Jun 2024 15:58:18 +0900 Subject: [PATCH 05/30] chore: refactor Signed-off-by: Taekjin LEE --- .../multi_object_tracker/utils/utils.hpp | 46 ++++++++----------- .../src/tracker/model/bicycle_tracker.cpp | 2 +- .../src/tracker/model/big_vehicle_tracker.cpp | 2 +- .../tracker/model/normal_vehicle_tracker.cpp | 2 +- .../src/tracker/model/pedestrian_tracker.cpp | 4 +- 5 files changed, 24 insertions(+), 32 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 5c7a79299cdae..164c6fec3861a 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 @@ -93,7 +93,8 @@ inline int getNearestCornerOrSurface( // (left) | | (right) // -- // x- (rear) - int xgrid, ygrid; + int xgrid; + int ygrid; const int labels[3][3] = { {BBOX_IDX::FRONT_L_CORNER, BBOX_IDX::FRONT_SURFACE, BBOX_IDX::FRONT_R_CORNER}, {BBOX_IDX::LEFT_SURFACE, BBOX_IDX::INSIDE, BBOX_IDX::RIGHT_SURFACE}, @@ -183,9 +184,8 @@ inline void calcAnchorPointOffset( } // current object width and height - double w_n, l_n; - l_n = input_object.shape.dimensions.x; - w_n = input_object.shape.dimensions.y; + const double w_n = input_object.shape.dimensions.y; + const double l_n = input_object.shape.dimensions.x; // update offset const Eigen::Vector2d offset = calcOffsetVectorFromShapeChange(w_n - w, l_n - l, indx); @@ -213,27 +213,19 @@ inline bool convertConvexHullToBoundingBox( } // look for bounding box boundary - double max_x = 0; - double max_y = 0; - double min_x = 0; - double min_y = 0; - double max_z = 0; - for (size_t i = 0; i < input_object.shape.footprint.points.size(); ++i) { - const double foot_x = input_object.shape.footprint.points.at(i).x; - const double foot_y = input_object.shape.footprint.points.at(i).y; - const double foot_z = input_object.shape.footprint.points.at(i).z; - max_x = std::max(max_x, foot_x); - max_y = std::max(max_y, foot_y); - min_x = std::min(min_x, foot_x); - min_y = std::min(min_y, foot_y); - max_z = std::max(max_z, foot_z); + float max_x = 0; + float max_y = 0; + float min_x = 0; + float min_y = 0; + float max_z = 0; + for (const auto & point : input_object.shape.footprint.points) { + max_x = std::max(max_x, point.x); + max_y = std::max(max_y, point.y); + min_x = std::min(min_x, point.x); + min_y = std::min(min_y, point.y); + max_z = std::max(max_z, point.z); } - // calc bounding box state - const double length = max_x - min_x; - const double width = max_y - min_y; - const double height = max_z; - // calc new center const Eigen::Vector2d center{ input_object.kinematics.pose_with_covariance.pose.position.x, @@ -248,10 +240,10 @@ inline bool convertConvexHullToBoundingBox( output_object.kinematics.pose_with_covariance.pose.position.x = new_center.x(); output_object.kinematics.pose_with_covariance.pose.position.y = new_center.y(); - output_object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; - output_object.shape.dimensions.x = length; - output_object.shape.dimensions.y = width; - output_object.shape.dimensions.z = height; + output_object.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + output_object.shape.dimensions.x = max_x - min_x; + output_object.shape.dimensions.y = max_y - min_y; + output_object.shape.dimensions.z = max_z; return true; } 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 826488be653d4..2234e889fc73a 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -116,7 +116,7 @@ BicycleTracker::BicycleTracker( 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; + double vel_cov = 10.0; const double & length = bounding_box_.length; if (object.kinematics.has_twist) { 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 0eeec58e118d7..271900183f6c3 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 @@ -134,7 +134,7 @@ BigVehicleTracker::BigVehicleTracker( 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; + double vel_cov = 10.0; const double & length = bounding_box_.length; if (object.kinematics.has_twist) { 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 981fbea5ab3c8..f4e9699a67429 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 @@ -135,7 +135,7 @@ NormalVehicleTracker::NormalVehicleTracker( 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; + double vel_cov = 10.0; const double & length = bounding_box_.length; if (object.kinematics.has_twist) { 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 4b605aa2a8cf6..4c5965d8b9296 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -110,8 +110,8 @@ PedestrianTracker::PedestrianTracker( auto pose_cov = object.kinematics.pose_with_covariance.covariance; double vel = 0.0; double wz = 0.0; - double vel_cov; - double wz_cov; + double vel_cov = 10.0; + double wz_cov = 10.0; if (object.kinematics.has_twist) { vel = object.kinematics.twist_with_covariance.twist.linear.x; From a75daa82134ed2e56365c886636061abae6ca0e7 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 4 Jun 2024 16:46:52 +0900 Subject: [PATCH 06/30] chore: refactoring destructor Signed-off-by: Taekjin LEE --- .../multi_object_tracker/tracker/model/bicycle_tracker.hpp | 3 +-- .../multi_object_tracker/tracker/model/big_vehicle_tracker.hpp | 3 +-- .../tracker/model/normal_vehicle_tracker.hpp | 3 +-- .../tracker/model/pass_through_tracker.hpp | 3 +-- .../multi_object_tracker/tracker/model/pedestrian_tracker.hpp | 3 +-- .../multi_object_tracker/tracker/model/tracker_base.hpp | 2 +- .../multi_object_tracker/tracker/model/unknown_tracker.hpp | 3 +-- 7 files changed, 7 insertions(+), 13 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 8e0bfa91342df..c870f1176dacb 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 @@ -67,8 +67,7 @@ class BicycleTracker : public Tracker bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; - virtual ~BicycleTracker() {} + autoware_auto_perception_msgs::msg::TrackedObject & object) const override; }; #endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ 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 4e4e27a918e89..8701e6b7a6ddf 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 @@ -70,8 +70,7 @@ class BigVehicleTracker : public Tracker bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; - virtual ~BigVehicleTracker() {} + autoware_auto_perception_msgs::msg::TrackedObject & object) const override; }; #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 04a7fef055219..a3218490cf273 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 @@ -70,8 +70,7 @@ class NormalVehicleTracker : public Tracker bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; - virtual ~NormalVehicleTracker() {} + autoware_auto_perception_msgs::msg::TrackedObject & object) const override; }; #endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp index 605b3ae462cec..c469173e14fa8 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp @@ -42,8 +42,7 @@ class PassThroughTracker : public Tracker const geometry_msgs::msg::Transform & self_transform) override; bool getTrackedObject( const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; - virtual ~PassThroughTracker() {} + autoware_auto_perception_msgs::msg::TrackedObject & object) const override; }; #endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_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 6fb384c419373..ecef63cc38f14 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 @@ -75,8 +75,7 @@ class PedestrianTracker : public Tracker bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; - virtual ~PedestrianTracker() {} + autoware_auto_perception_msgs::msg::TrackedObject & object) const override; }; #endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp index 56670e9b54b7e..7d4e7a6f15b5c 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp @@ -54,7 +54,7 @@ class Tracker const rclcpp::Time & time, const std::vector & classification, const size_t & channel_size); - virtual ~Tracker() {} + virtual ~Tracker() = default; void initializeExistenceProbabilities( const uint & channel_index, const float & existence_probability); 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 ee0b46d05133b..563281eadfb1b 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 @@ -60,8 +60,7 @@ class UnknownTracker : public Tracker bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; - virtual ~UnknownTracker() {} + autoware_auto_perception_msgs::msg::TrackedObject & object) const override; }; #endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ From 0f9089b6085eae274ba821a233e14320b8874a25 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 4 Jun 2024 17:22:48 +0900 Subject: [PATCH 07/30] chore: refactoring getUpdatingObject Signed-off-by: Taekjin LEE --- .../tracker/model/bicycle_tracker.hpp | 12 +++++---- .../tracker/model/big_vehicle_tracker.hpp | 12 +++++---- .../tracker/model/normal_vehicle_tracker.hpp | 12 +++++---- .../tracker/model/pedestrian_tracker.hpp | 12 +++++---- .../multi_object_tracker/utils/utils.hpp | 4 +-- .../src/tracker/model/bicycle_tracker.cpp | 6 ++--- .../src/tracker/model/big_vehicle_tracker.cpp | 26 +++++++------------ .../tracker/model/normal_vehicle_tracker.cpp | 26 +++++++------------ .../src/tracker/model/pedestrian_tracker.cpp | 6 ++--- 9 files changed, 56 insertions(+), 60 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 c870f1176dacb..5916067582bd1 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 @@ -60,14 +60,16 @@ class BicycleTracker : public Tracker bool measure( const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - autoware_perception_msgs::msg::DetectedObject getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); - bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object); - bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); + 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; + +private: + autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform) const; }; #endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ 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 8701e6b7a6ddf..1bfbb2e3597e2 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 @@ -63,14 +63,16 @@ class BigVehicleTracker : public Tracker bool measure( const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - autoware_perception_msgs::msg::DetectedObject getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); - bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object); - bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); + 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; + +private: + autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & 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 a3218490cf273..56c4324d0752e 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 @@ -63,14 +63,16 @@ class NormalVehicleTracker : public Tracker bool measure( const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - autoware_perception_msgs::msg::DetectedObject getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); - bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object); - bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); + 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; + +private: + autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); }; #endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_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 ecef63cc38f14..8475a4750eb16 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 @@ -68,14 +68,16 @@ class PedestrianTracker : public Tracker bool measure( const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - autoware_perception_msgs::msg::DetectedObject getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); - bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object); - bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); + 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; + +private: + autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform) const; }; #endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ 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 164c6fec3861a..304152a4e5c70 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 @@ -93,8 +93,8 @@ inline int getNearestCornerOrSurface( // (left) | | (right) // -- // x- (rear) - int xgrid; - int ygrid; + int xgrid = 0; + int ygrid = 0; const int labels[3][3] = { {BBOX_IDX::FRONT_L_CORNER, BBOX_IDX::FRONT_SURFACE, BBOX_IDX::FRONT_R_CORNER}, {BBOX_IDX::LEFT_SURFACE, BBOX_IDX::INSIDE, BBOX_IDX::RIGHT_SURFACE}, 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 2234e889fc73a..c0d72430a9eaf 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,9 @@ bool BicycleTracker::predict(const rclcpp::Time & time) return motion_model_.predictState(time); } -autoware_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/) +autoware_auto_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & /*self_transform*/) const { autoware_perception_msgs::msg::DetectedObject updating_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 271900183f6c3..ce59ebf00eb86 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 @@ -219,22 +219,15 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { // measurement noise covariance - float r_cov_x; - float r_cov_y; - using Label = autoware_perception_msgs::msg::ObjectClassification; + float r_cov_x = static_cast(ekf_params_.r_cov_x); + float r_cov_y = static_cast(ekf_params_.r_cov_y); 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 == autoware_auto_perception_msgs::msg::ObjectClassification::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; + r_cov_x = r_stddev_x * r_stddev_x; + r_cov_y = r_stddev_y * r_stddev_y; } // yaw angle fix @@ -315,7 +308,7 @@ bool BigVehicleTracker::measureWithPose( bool BigVehicleTracker::measureWithShape( const autoware_perception_msgs::msg::DetectedObject & object) { - if (!object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + 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 false; } @@ -323,9 +316,10 @@ bool BigVehicleTracker::measureWithShape( // check object size abnormality constexpr double size_max = 40.0; // [m] constexpr double size_min = 1.0; // [m] - if (object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max) { - return false; - } else if (object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min) { + bool isSizeValid = + (object.shape.dimensions.x <= size_max && object.shape.dimensions.y <= size_max && + object.shape.dimensions.x >= size_min && object.shape.dimensions.y >= size_min); + if (!isSizeValid) { return 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 f4e9699a67429..38b549b5008a3 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 @@ -221,22 +221,15 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { // measurement noise covariance - float r_cov_x; - float r_cov_y; - using Label = autoware_perception_msgs::msg::ObjectClassification; + float r_cov_x = static_cast(ekf_params_.r_cov_x); + float r_cov_y = static_cast(ekf_params_.r_cov_y); 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 (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; + r_cov_x = r_stddev_x * r_stddev_x; + r_cov_y = r_stddev_y * r_stddev_y; } // yaw angle fix @@ -317,7 +310,7 @@ bool NormalVehicleTracker::measureWithPose( bool NormalVehicleTracker::measureWithShape( const autoware_perception_msgs::msg::DetectedObject & object) { - if (!object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + 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 false; } @@ -325,9 +318,10 @@ bool NormalVehicleTracker::measureWithShape( // check object size abnormality constexpr double size_max = 30.0; // [m] constexpr double size_min = 1.0; // [m] - if (object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max) { - return false; - } else if (object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min) { + bool isSizeValid = + (object.shape.dimensions.x <= size_max && object.shape.dimensions.y <= size_max && + object.shape.dimensions.x >= size_min && object.shape.dimensions.y >= size_min); + if (!isSizeValid) { return 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 4c5965d8b9296..becd973abbc06 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -160,9 +160,9 @@ bool PedestrianTracker::predict(const rclcpp::Time & time) return motion_model_.predictState(time); } -autoware_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/) +autoware_auto_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & /*self_transform*/) const { autoware_perception_msgs::msg::DetectedObject updating_object = object; From 4b0e6cf233123b92fd138fa2669a41983d2ab8d2 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 4 Jun 2024 17:53:31 +0900 Subject: [PATCH 08/30] chore: refactoring object size checker Signed-off-by: Taekjin LEE --- .../src/tracker/model/big_vehicle_tracker.cpp | 4 ++-- .../src/tracker/model/normal_vehicle_tracker.cpp | 4 ++-- .../src/tracker/model/pedestrian_tracker.cpp | 13 ++++++------- 3 files changed, 10 insertions(+), 11 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 ce59ebf00eb86..4ef0778a629be 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 @@ -316,10 +316,10 @@ bool BigVehicleTracker::measureWithShape( // check object size abnormality constexpr double size_max = 40.0; // [m] constexpr double size_min = 1.0; // [m] - bool isSizeValid = + bool is_size_valid = (object.shape.dimensions.x <= size_max && object.shape.dimensions.y <= size_max && object.shape.dimensions.x >= size_min && object.shape.dimensions.y >= size_min); - if (!isSizeValid) { + if (!is_size_valid) { return 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 38b549b5008a3..92ae8e3c9a988 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,10 +318,10 @@ bool NormalVehicleTracker::measureWithShape( // check object size abnormality constexpr double size_max = 30.0; // [m] constexpr double size_min = 1.0; // [m] - bool isSizeValid = + bool is_size_valid = (object.shape.dimensions.x <= size_max && object.shape.dimensions.y <= size_max && object.shape.dimensions.x >= size_min && object.shape.dimensions.y >= size_min); - if (!isSizeValid) { + if (!is_size_valid) { return 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 becd973abbc06..50a0b3ae218c7 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -218,15 +218,14 @@ bool PedestrianTracker::measureWithShape( // check bound box size abnormality constexpr double size_max = 30.0; // [m] constexpr double size_min = 0.1; // [m] - if ( - object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max || - object.shape.dimensions.z > size_max) { - return false; - } else if ( - object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min || - object.shape.dimensions.z < size_min) { + bool is_size_valid = + (object.shape.dimensions.x <= size_max && object.shape.dimensions.y <= size_max && + object.shape.dimensions.z <= size_max && object.shape.dimensions.x >= size_min && + object.shape.dimensions.y >= size_min && object.shape.dimensions.z >= size_min); + if (!is_size_valid) { return false; } + // update bounding box size 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; From b963c092c2ec7b0548e694ba5d37e3962fcfc28a Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 4 Jun 2024 19:29:08 +0900 Subject: [PATCH 09/30] fix: debugger delay calculation order is fixed Signed-off-by: Taekjin LEE --- perception/multi_object_tracker/src/debugger/debugger.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/multi_object_tracker/src/debugger/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp index a3ece17eab112..e692ae76468e4 100644 --- a/perception/multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/multi_object_tracker/src/debugger/debugger.cpp @@ -98,7 +98,7 @@ void TrackerDebugger::checkDelay(diagnostic_updater::DiagnosticStatusWrapper & s stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Measurement time is not set."); return; } - const double & delay = pipeline_latency_ms_; // [s] + const double & delay = pipeline_latency_ms_ / 1e3; // [s] if (delay == 0.0) { stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Detection delay is not calculated."); From 34057df3c9f6cb426a35fd6b0871589bf480c4ab Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 5 Jun 2024 09:06:08 +0900 Subject: [PATCH 10/30] fix: bicycle size update bug fix Signed-off-by: Taekjin LEE --- .../src/tracker/model/bicycle_tracker.cpp | 21 ++++++++----------- .../src/tracker/model/big_vehicle_tracker.cpp | 12 +++++------ .../tracker/model/normal_vehicle_tracker.cpp | 17 +++++++-------- 3 files changed, 21 insertions(+), 29 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 c0d72430a9eaf..cb703de3e2e18 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -170,7 +170,7 @@ autoware_auto_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingOb const autoware_auto_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & /*self_transform*/) const { - autoware_perception_msgs::msg::DetectedObject updating_object; + autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; // OBJECT SHAPE MODEL // convert to bounding box if input is convex shape @@ -178,8 +178,6 @@ autoware_auto_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingOb if (!utils::convertConvexHullToBoundingBox(object, updating_object)) { updating_object = object; } - } else { - updating_object = object; } // UNCERTAINTY MODEL @@ -230,8 +228,7 @@ bool BicycleTracker::measureWithPose(const autoware_perception_msgs::msg::Detect bool BicycleTracker::measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object) { - autoware_perception_msgs::msg::DetectedObject bbox_object; - if (!object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + 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 false; } @@ -240,21 +237,21 @@ bool BicycleTracker::measureWithShape(const autoware_perception_msgs::msg::Detec constexpr double size_max = 30.0; // [m] constexpr double size_min = 0.1; // [m] if ( - bbox_object.shape.dimensions.x > size_max || bbox_object.shape.dimensions.y > size_max || - bbox_object.shape.dimensions.z > size_max) { + object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max || + object.shape.dimensions.z > size_max) { return false; } else if ( - bbox_object.shape.dimensions.x < size_min || bbox_object.shape.dimensions.y < size_min || - bbox_object.shape.dimensions.z < size_min) { + object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min || + object.shape.dimensions.z < size_min) { return false; } // update object size constexpr double gain = 0.1; constexpr double gain_inv = 1.0 - gain; - bounding_box_.length = gain_inv * bounding_box_.length + gain * bbox_object.shape.dimensions.x; - bounding_box_.width = gain_inv * bounding_box_.width + gain * bbox_object.shape.dimensions.y; - bounding_box_.height = gain_inv * bounding_box_.height + gain * bbox_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; // set maximum and minimum size constexpr double max_size = 5.0; 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 4ef0778a629be..c3df3dac81c32 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 @@ -190,11 +190,6 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje { autoware_perception_msgs::msg::DetectedObject updating_object = object; - // current (predicted) state - 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 autoware_perception_msgs::msg::DetectedObject bbox_object; @@ -205,10 +200,13 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje "BigVehicleTracker::getUpdatingObject: Failed to convert convex hull to bounding box."); bbox_object = object; } - } else { - bbox_object = object; } + // current (predicted) state + 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); + // get offset measurement const int nearest_corner_index = utils::getNearestCornerOrSurface( tracked_x, tracked_y, tracked_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 92ae8e3c9a988..b44b08c2951e7 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 @@ -191,26 +191,23 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO { autoware_perception_msgs::msg::DetectedObject updating_object = object; - // current (predicted) state - 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 - autoware_perception_msgs::msg::DetectedObject bbox_object; - if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + autoware_auto_perception_msgs::msg::DetectedObject bbox_object = object; + if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { RCLCPP_WARN( logger_, "NormalVehicleTracker::getUpdatingObject: Failed to convert convex hull to bounding box."); bbox_object = object; } - - } else { - bbox_object = object; } + // current (predicted) state + 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); + // get offset measurement const int nearest_corner_index = utils::getNearestCornerOrSurface( tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform); From daa4e1679dec44a71cf6de05bd122fa69c04cadc Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 5 Jun 2024 09:15:37 +0900 Subject: [PATCH 11/30] fix: renamed message package Signed-off-by: Taekjin LEE --- .../tracker/model/bicycle_tracker.hpp | 10 +++++----- .../tracker/model/big_vehicle_tracker.hpp | 10 +++++----- .../tracker/model/normal_vehicle_tracker.hpp | 10 +++++----- .../tracker/model/pass_through_tracker.hpp | 2 +- .../tracker/model/pedestrian_tracker.hpp | 10 +++++----- .../tracker/model/unknown_tracker.hpp | 2 +- .../include/multi_object_tracker/utils/utils.hpp | 2 +- .../src/tracker/model/bicycle_tracker.cpp | 8 ++++---- .../src/tracker/model/big_vehicle_tracker.cpp | 4 ++-- .../src/tracker/model/normal_vehicle_tracker.cpp | 6 +++--- .../src/tracker/model/pedestrian_tracker.cpp | 4 ++-- 11 files changed, 34 insertions(+), 34 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 5916067582bd1..a41368d59151b 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 @@ -60,15 +60,15 @@ class BicycleTracker : public Tracker bool measure( const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); - bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); + bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object); + bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, - autoware_auto_perception_msgs::msg::TrackedObject & object) const override; + autoware_perception_msgs::msg::TrackedObject & object) const override; private: - autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( - const autoware_auto_perception_msgs::msg::DetectedObject & object, + autoware_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & self_transform) const; }; 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 1bfbb2e3597e2..8a82fb128b212 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 @@ -63,15 +63,15 @@ class BigVehicleTracker : public Tracker bool measure( const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); - bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); + bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object); + bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, - autoware_auto_perception_msgs::msg::TrackedObject & object) const override; + autoware_perception_msgs::msg::TrackedObject & object) const override; private: - autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( - const autoware_auto_perception_msgs::msg::DetectedObject & object, + autoware_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & self_transform); }; 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 56c4324d0752e..3a51c61349452 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 @@ -63,15 +63,15 @@ class NormalVehicleTracker : public Tracker bool measure( const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); - bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); + bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object); + bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, - autoware_auto_perception_msgs::msg::TrackedObject & object) const override; + autoware_perception_msgs::msg::TrackedObject & object) const override; private: - autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( - const autoware_auto_perception_msgs::msg::DetectedObject & object, + autoware_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & self_transform); }; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp index c469173e14fa8..3d9bffb18a9db 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp @@ -42,7 +42,7 @@ class PassThroughTracker : public Tracker const geometry_msgs::msg::Transform & self_transform) override; bool getTrackedObject( const rclcpp::Time & time, - autoware_auto_perception_msgs::msg::TrackedObject & object) const override; + autoware_perception_msgs::msg::TrackedObject & object) const override; }; #endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_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 8475a4750eb16..386fd74d9b942 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 @@ -68,15 +68,15 @@ class PedestrianTracker : public Tracker bool measure( const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); - bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); + bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object); + bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, - autoware_auto_perception_msgs::msg::TrackedObject & object) const override; + autoware_perception_msgs::msg::TrackedObject & object) const override; private: - autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( - const autoware_auto_perception_msgs::msg::DetectedObject & object, + autoware_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & self_transform) const; }; 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 563281eadfb1b..e0467087acd70 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 @@ -60,7 +60,7 @@ class UnknownTracker : public Tracker bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, - autoware_auto_perception_msgs::msg::TrackedObject & object) const override; + autoware_perception_msgs::msg::TrackedObject & object) const override; }; #endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ 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 304152a4e5c70..a30980e12e987 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 @@ -240,7 +240,7 @@ inline bool convertConvexHullToBoundingBox( output_object.kinematics.pose_with_covariance.pose.position.x = new_center.x(); output_object.kinematics.pose_with_covariance.pose.position.y = new_center.y(); - output_object.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + output_object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; output_object.shape.dimensions.x = max_x - min_x; output_object.shape.dimensions.y = max_y - min_y; output_object.shape.dimensions.z = max_z; 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 cb703de3e2e18..41a170157da5a 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -166,11 +166,11 @@ bool BicycleTracker::predict(const rclcpp::Time & time) return motion_model_.predictState(time); } -autoware_auto_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( - const autoware_auto_perception_msgs::msg::DetectedObject & object, +autoware_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( + const autoware_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & /*self_transform*/) const { - autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; + autoware_perception_msgs::msg::DetectedObject updating_object = object; // OBJECT SHAPE MODEL // convert to bounding box if input is convex shape @@ -228,7 +228,7 @@ bool BicycleTracker::measureWithPose(const autoware_perception_msgs::msg::Detect bool BicycleTracker::measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object) { - if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { // do not update shape if the input is not a bounding box return 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 c3df3dac81c32..01796dc617bae 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 @@ -220,7 +220,7 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje float r_cov_x = static_cast(ekf_params_.r_cov_x); float r_cov_y = static_cast(ekf_params_.r_cov_y); const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::CAR) { + if (label == autoware_perception_msgs::msg::ObjectClassification::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] @@ -306,7 +306,7 @@ bool BigVehicleTracker::measureWithPose( bool BigVehicleTracker::measureWithShape( const autoware_perception_msgs::msg::DetectedObject & object) { - if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { // do not update shape if the input is not a bounding box return 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 b44b08c2951e7..50fe14421ac60 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 @@ -193,8 +193,8 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO // OBJECT SHAPE MODEL // convert to bounding box if input is convex shape - autoware_auto_perception_msgs::msg::DetectedObject bbox_object = object; - if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + autoware_perception_msgs::msg::DetectedObject bbox_object = object; + if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { RCLCPP_WARN( logger_, @@ -307,7 +307,7 @@ bool NormalVehicleTracker::measureWithPose( bool NormalVehicleTracker::measureWithShape( const autoware_perception_msgs::msg::DetectedObject & object) { - if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { // do not update shape if the input is not a bounding box return 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 50a0b3ae218c7..202a9cb25f809 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -160,8 +160,8 @@ bool PedestrianTracker::predict(const rclcpp::Time & time) return motion_model_.predictState(time); } -autoware_auto_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObject( - const autoware_auto_perception_msgs::msg::DetectedObject & object, +autoware_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObject( + const autoware_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & /*self_transform*/) const { autoware_perception_msgs::msg::DetectedObject updating_object = object; From 8cf586ac75f7f2d951f36e19427b1a03879d6b4c Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 10 Jun 2024 16:23:12 +0900 Subject: [PATCH 12/30] chore: refactoring Signed-off-by: Taekjin LEE --- .../multi_object_tracker/src/multi_object_tracker_core.cpp | 2 +- .../src/tracker/model/big_vehicle_tracker.cpp | 6 +++--- .../src/tracker/model/normal_vehicle_tracker.cpp | 4 ++-- .../src/tracker/motion_model/bicycle_motion_model.cpp | 3 +-- .../src/tracker/motion_model/ctrv_motion_model.cpp | 2 +- .../src/tracker/motion_model/cv_motion_model.cpp | 2 +- 6 files changed, 9 insertions(+), 10 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 54255e706e35a..8cc552b417676 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -207,7 +207,7 @@ void MultiObjectTracker::onTrigger() { const rclcpp::Time current_time = this->now(); // get objects from the input manager and run process - std::vector> objects_list; + ObjectsList objects_list; const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list); if (!is_objects_ready) return; 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 01796dc617bae..3222bf4c66b3a 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 @@ -192,7 +192,7 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje // OBJECT SHAPE MODEL // convert to bounding box if input is convex shape - autoware_perception_msgs::msg::DetectedObject bbox_object; + autoware_perception_msgs::msg::DetectedObject bbox_object = object; if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { RCLCPP_WARN( @@ -217,8 +217,8 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { // measurement noise covariance - float r_cov_x = static_cast(ekf_params_.r_cov_x); - float r_cov_y = static_cast(ekf_params_.r_cov_y); + auto r_cov_x = static_cast(ekf_params_.r_cov_x); + auto r_cov_y = static_cast(ekf_params_.r_cov_y); const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); if (label == autoware_perception_msgs::msg::ObjectClassification::CAR) { // if label is changed, enlarge the measurement noise 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 50fe14421ac60..9f4158a6282c7 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 @@ -218,8 +218,8 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { // measurement noise covariance - float r_cov_x = static_cast(ekf_params_.r_cov_x); - float r_cov_y = static_cast(ekf_params_.r_cov_y); + auto r_cov_x = static_cast(ekf_params_.r_cov_x); + auto r_cov_y = static_cast(ekf_params_.r_cov_y); const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); if (utils::isLargeVehicleLabel(label)) { // if label is changed, enlarge the measurement noise covariance diff --git a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp index 419db77c4849a..5d45b1fc89678 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp @@ -34,8 +34,7 @@ // CTRV : Constant Turn Rate and constant Velocity using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; -BicycleMotionModel::BicycleMotionModel() -: MotionModel(), logger_(rclcpp::get_logger("BicycleMotionModel")) +BicycleMotionModel::BicycleMotionModel() : logger_(rclcpp::get_logger("BicycleMotionModel")) { // Initialize motion parameters setDefaultParams(); diff --git a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp index 15741ac26899d..dc1f235e1623a 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp @@ -33,7 +33,7 @@ // Constant Turn Rate and constant Velocity (CTRV) motion model using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; -CTRVMotionModel::CTRVMotionModel() : MotionModel(), logger_(rclcpp::get_logger("CTRVMotionModel")) +CTRVMotionModel::CTRVMotionModel() : logger_(rclcpp::get_logger("CTRVMotionModel")) { // Initialize motion parameters setDefaultParams(); diff --git a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp index 7800c24ec0b19..33cb945777b0d 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp @@ -35,7 +35,7 @@ // Constant Velocity (CV) motion model using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; -CVMotionModel::CVMotionModel() : MotionModel(), logger_(rclcpp::get_logger("CVMotionModel")) +CVMotionModel::CVMotionModel() : logger_(rclcpp::get_logger("CVMotionModel")) { // Initialize motion parameters setDefaultParams(); From 74f08daa1bf48269bc6f0f1df4c096e540a22e0d Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 10 Jun 2024 19:02:29 +0900 Subject: [PATCH 13/30] feat: object parameter class Signed-off-by: Taekjin LEE --- .../object_model/object_model_base.hpp | 169 ++++++++++++++++++ 1 file changed, 169 insertions(+) create mode 100644 perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model_base.hpp diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model_base.hpp new file mode 100644 index 0000000000000..bb075f59f20cf --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model_base.hpp @@ -0,0 +1,169 @@ +// 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__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ +#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ + +#include + +namespace +{ + constexpr double const_g = 9.81; + double deg2rad(const double deg) + { + return deg * M_PI / 180.0; + } +} // namespace + +namespace object_model +{ + +enum class ObjectModelType +{ + NORMAL_VEHICLE, + BIG_VEHICLE, + BICYCLE, + PEDESTRIAN, + STATIC, + UNKNOWN +}; +struct ObjectSize +{ + double length; // [m] + double width; // [m] + double height; // [m] +}; +struct ObjectSizeLimit +{ + double length_min; // [m] + double length_max; // [m] + double width_min; // [m] + double width_max; // [m] + double height_min; // [m] + double height_max; // [m] +}; +struct MotionProcessNoise +{ + double acc_long; // [m/s^2] + double acc_lat; // [m/s^2] + double yaw_rate_min; // [rad/s] + double yaw_rate_max; // [rad/s] + double slip_rate_min; + double slip_rate_max; + +}; +struct MotionProcessLimit +{ + double slip_angle_max; + double acc_long_max; + double acc_lat_max; + double vel_long_max; +}; +struct StateCovariance +{ + double pos_x; // [m^2] + double pos_y; // [m^2] + double yaw; // [rad^2] + double yaw_rate; // [rad^2/s^2] + double acc_long; // [m^2/s^4] + double acc_lat; // [m^2/s^4] + double vel_long; // [m^2/s^2] + double vel_lat; // [m^2/s^2] +}; +struct BicycleModelState +{ + double slip_angle; // [rad^2] + double slip_rate; // [rad^2/s^2] + double wheel_pos_ratio_front; // [-] + double wheel_pos_ratio_rear; // [-] + double wheel_pos_front_min; // [m] + double wheel_pos_rear_min; // [m] +}; + +class ObjectModel +{ +public: + ObjectSize size; + ObjectSizeLimit size_limit; + MotionProcessNoise process_noise; + MotionProcessLimit process_limit; + StateCovariance initial_covariance; + StateCovariance state_covariance; + BicycleModelState bicycle_model_state; + + ObjectModel(const ObjectModelType & type) + { + switch (type) + { + case ObjectModelType::NORMAL_VEHICLE: + size.length = 3.0; + size.width = 2.0; + size.height = 1.8; + size_limit.length_min = 2.0; + size_limit.length_max = 5.0; + size_limit.width_min = 1.5; + size_limit.width_max = 3.0; + size_limit.height_min = 1.5; + 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; + process_noise.yaw_rate_max = 15.0; + process_noise.slip_rate_min = 0.3; + process_noise.slip_rate_max = 10.0; + + process_limit.slip_angle_max = 30; + process_limit.acc_long_max = 2.0; + process_limit.acc_lat_max = 2.0; + process_limit.vel_long_max = 30.0; + + // initial covariance + initial_covariance.pos_x = 0.5; + initial_covariance.pos_y = 0.4; + initial_covariance.yaw = deg2rad(20.0); + initial_covariance.yaw_rate = deg2rad(10.0); + initial_covariance.acc_long = 1.0; + initial_covariance.acc_lat = 1.0; + initial_covariance.vel_long = 1.0; + initial_covariance.vel_lat = 1.0; + + // noise model + state_covariance.pos_x = 0.5; + state_covariance.pos_y = 0.4; + state_covariance.yaw = 0.1; + state_covariance.yaw_rate = 0.1; + state_covariance.acc_long = 0.1; + state_covariance.acc_lat = 0.1; + state_covariance.vel_long = 0.1; + state_covariance.vel_lat = 0.1; + break; + } + } + virtual ~ObjectModel() = default; +}; + + + +ObjectModel normal_vehicle(ObjectModelType::NORMAL_VEHICLE); + + + +} // namespace object_model + +#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ \ No newline at end of file From 80806cee96abc07c937e0dd3e53fb6c49ff108f1 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 10 Jun 2024 10:06:58 +0000 Subject: [PATCH 14/30] style(pre-commit): autofix Signed-off-by: Taekjin LEE --- .../object_model/object_model_base.hpp | 178 ++++++++---------- 1 file changed, 82 insertions(+), 96 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model_base.hpp index bb075f59f20cf..52e5b6108a0d9 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model_base.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model_base.hpp @@ -16,56 +16,47 @@ // Author: v1.0 Taekjin Lee // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ +#ifndef MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_BASE_HPP_ +#define MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_BASE_HPP_ #include namespace { - constexpr double const_g = 9.81; - double deg2rad(const double deg) - { - return deg * M_PI / 180.0; - } +constexpr double const_g = 9.81; +double deg2rad(const double deg) +{ + return deg * M_PI / 180.0; +} } // namespace namespace object_model { -enum class ObjectModelType -{ - NORMAL_VEHICLE, - BIG_VEHICLE, - BICYCLE, - PEDESTRIAN, - STATIC, - UNKNOWN -}; +enum class ObjectModelType { NORMAL_VEHICLE, BIG_VEHICLE, BICYCLE, PEDESTRIAN, STATIC, UNKNOWN }; struct ObjectSize { - double length; // [m] - double width; // [m] - double height; // [m] + double length; // [m] + double width; // [m] + double height; // [m] }; struct ObjectSizeLimit { - double length_min; // [m] - double length_max; // [m] - double width_min; // [m] - double width_max; // [m] - double height_min; // [m] - double height_max; // [m] + double length_min; // [m] + double length_max; // [m] + double width_min; // [m] + double width_max; // [m] + double height_min; // [m] + double height_max; // [m] }; struct MotionProcessNoise { - double acc_long; // [m/s^2] - double acc_lat; // [m/s^2] - double yaw_rate_min; // [rad/s] - double yaw_rate_max; // [rad/s] - double slip_rate_min; + double acc_long; // [m/s^2] + double acc_lat; // [m/s^2] + double yaw_rate_min; // [rad/s] + double yaw_rate_max; // [rad/s] + double slip_rate_min; double slip_rate_max; - }; struct MotionProcessLimit { @@ -76,23 +67,23 @@ struct MotionProcessLimit }; struct StateCovariance { - double pos_x; // [m^2] - double pos_y; // [m^2] - double yaw; // [rad^2] - double yaw_rate; // [rad^2/s^2] - double acc_long; // [m^2/s^4] - double acc_lat; // [m^2/s^4] - double vel_long; // [m^2/s^2] - double vel_lat; // [m^2/s^2] + double pos_x; // [m^2] + double pos_y; // [m^2] + double yaw; // [rad^2] + double yaw_rate; // [rad^2/s^2] + double acc_long; // [m^2/s^4] + double acc_lat; // [m^2/s^4] + double vel_long; // [m^2/s^2] + double vel_lat; // [m^2/s^2] }; struct BicycleModelState { - double slip_angle; // [rad^2] - double slip_rate; // [rad^2/s^2] - double wheel_pos_ratio_front; // [-] - double wheel_pos_ratio_rear; // [-] - double wheel_pos_front_min; // [m] - double wheel_pos_rear_min; // [m] + double slip_angle; // [rad^2] + double slip_rate; // [rad^2/s^2] + double wheel_pos_ratio_front; // [-] + double wheel_pos_ratio_rear; // [-] + double wheel_pos_front_min; // [m] + double wheel_pos_rear_min; // [m] }; class ObjectModel @@ -106,64 +97,59 @@ class ObjectModel StateCovariance state_covariance; BicycleModelState bicycle_model_state; - ObjectModel(const ObjectModelType & type) + ObjectModel(const ObjectModelType & type) { - switch (type) - { - case ObjectModelType::NORMAL_VEHICLE: - size.length = 3.0; - size.width = 2.0; - size.height = 1.8; - size_limit.length_min = 2.0; - size_limit.length_max = 5.0; - size_limit.width_min = 1.5; - size_limit.width_max = 3.0; - size_limit.height_min = 1.5; - 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; - process_noise.yaw_rate_max = 15.0; - process_noise.slip_rate_min = 0.3; - process_noise.slip_rate_max = 10.0; - - process_limit.slip_angle_max = 30; - process_limit.acc_long_max = 2.0; - process_limit.acc_lat_max = 2.0; - process_limit.vel_long_max = 30.0; - - // initial covariance - initial_covariance.pos_x = 0.5; - initial_covariance.pos_y = 0.4; - initial_covariance.yaw = deg2rad(20.0); - initial_covariance.yaw_rate = deg2rad(10.0); - initial_covariance.acc_long = 1.0; - initial_covariance.acc_lat = 1.0; - initial_covariance.vel_long = 1.0; - initial_covariance.vel_lat = 1.0; - - // noise model - state_covariance.pos_x = 0.5; - state_covariance.pos_y = 0.4; - state_covariance.yaw = 0.1; - state_covariance.yaw_rate = 0.1; - state_covariance.acc_long = 0.1; - state_covariance.acc_lat = 0.1; - state_covariance.vel_long = 0.1; - state_covariance.vel_lat = 0.1; - break; + switch (type) { + case ObjectModelType::NORMAL_VEHICLE: + size.length = 3.0; + size.width = 2.0; + size.height = 1.8; + size_limit.length_min = 2.0; + size_limit.length_max = 5.0; + size_limit.width_min = 1.5; + size_limit.width_max = 3.0; + size_limit.height_min = 1.5; + 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; + process_noise.yaw_rate_max = 15.0; + process_noise.slip_rate_min = 0.3; + process_noise.slip_rate_max = 10.0; + + process_limit.slip_angle_max = 30; + process_limit.acc_long_max = 2.0; + process_limit.acc_lat_max = 2.0; + process_limit.vel_long_max = 30.0; + + // initial covariance + initial_covariance.pos_x = 0.5; + initial_covariance.pos_y = 0.4; + initial_covariance.yaw = deg2rad(20.0); + initial_covariance.yaw_rate = deg2rad(10.0); + initial_covariance.acc_long = 1.0; + initial_covariance.acc_lat = 1.0; + initial_covariance.vel_long = 1.0; + initial_covariance.vel_lat = 1.0; + + // noise model + state_covariance.pos_x = 0.5; + state_covariance.pos_y = 0.4; + state_covariance.yaw = 0.1; + state_covariance.yaw_rate = 0.1; + state_covariance.acc_long = 0.1; + state_covariance.acc_lat = 0.1; + state_covariance.vel_long = 0.1; + state_covariance.vel_lat = 0.1; + break; } } virtual ~ObjectModel() = default; }; - - ObjectModel normal_vehicle(ObjectModelType::NORMAL_VEHICLE); - - } // namespace object_model -#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ \ No newline at end of file +#endif // MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_BASE_HPP_ From b316282d5bb173850d7d3f06829f7f62d42c7ca4 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 13 Jun 2024 11:21:21 +0900 Subject: [PATCH 15/30] chore: refactor includes Signed-off-by: Taekjin LEE --- .../data_association/data_association.hpp | 11 ++++++----- .../debugger/debug_object.hpp | 3 ++- .../multi_object_tracker/debugger/debugger.hpp | 2 ++ .../processor/input_manager.hpp | 2 +- .../tracker/model/bicycle_tracker.hpp | 3 +-- .../tracker/model/big_vehicle_tracker.hpp | 3 +-- .../tracker/model/multiple_vehicle_tracker.hpp | 2 +- .../tracker/model/normal_vehicle_tracker.hpp | 3 +-- .../tracker/model/pass_through_tracker.hpp | 3 +-- .../model/pedestrian_and_bicycle_tracker.hpp | 3 +-- .../tracker/model/pedestrian_tracker.hpp | 3 +-- .../tracker/model/tracker_base.hpp | 8 ++++---- .../tracker/model/unknown_tracker.hpp | 3 +-- .../motion_model/bicycle_motion_model.hpp | 2 +- .../tracker/motion_model/ctrv_motion_model.hpp | 2 +- .../tracker/motion_model/cv_motion_model.hpp | 2 +- .../tracker/motion_model/motion_model_base.hpp | 3 ++- .../src/debugger/debug_object.cpp | 2 +- .../src/multi_object_tracker_core.cpp | 17 +++++++++-------- .../src/tracker/model/bicycle_tracker.cpp | 17 ++++++++--------- .../src/tracker/model/big_vehicle_tracker.cpp | 17 ++++++++--------- .../tracker/model/normal_vehicle_tracker.cpp | 17 ++++++++--------- .../src/tracker/model/pass_through_tracker.cpp | 17 ++++++++--------- .../src/tracker/model/pedestrian_tracker.cpp | 17 ++++++++--------- .../src/tracker/model/unknown_tracker.cpp | 15 +++++++-------- .../motion_model/bicycle_motion_model.cpp | 9 ++++----- .../tracker/motion_model/ctrv_motion_model.cpp | 9 ++++----- .../tracker/motion_model/cv_motion_model.cpp | 13 ++++++------- 28 files changed, 99 insertions(+), 109 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp b/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp index 56f4c3ac45e52..27fdb7cf420c3 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp @@ -19,12 +19,8 @@ #ifndef MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ #define MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ -#include -#include -#include -#include - #define EIGEN_MPL2_ONLY + #include "multi_object_tracker/data_association/solver/gnn_solver.hpp" #include "multi_object_tracker/tracker/tracker.hpp" @@ -33,6 +29,11 @@ #include +#include +#include +#include +#include + class DataAssociation { private: diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp index 7c1b814258bcf..1a00e4902b57b 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp @@ -16,14 +16,15 @@ #define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ #include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "tier4_autoware_utils/ros/uuid_helper.hpp" #include #include -#include "unique_identifier_msgs/msg/uuid.hpp" #include #include #include +#include #include #include diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp index 3019f16ada7c9..6bf2e581fcdf1 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp @@ -16,6 +16,8 @@ #define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ #include "multi_object_tracker/debugger/debug_object.hpp" +#include "tier4_autoware_utils/ros/debug_publisher.hpp" +#include "tier4_autoware_utils/ros/published_time_publisher.hpp" #include #include diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp index 29b0c18ae766f..a7ff266a17018 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp @@ -17,7 +17,7 @@ #include "rclcpp/rclcpp.hpp" -#include "autoware_perception_msgs/msg/detected_objects.hpp" +#include #include #include 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 a41368d59151b..e9fdb09d91be6 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,11 +19,10 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ +#include "kalman_filter/kalman_filter.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" #include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" -#include - class BicycleTracker : public Tracker { private: 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 8a82fb128b212..19518b8cb118f 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,11 +19,10 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ +#include "kalman_filter/kalman_filter.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" #include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" -#include - class BigVehicleTracker : public Tracker { private: diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp index b3d088da54721..75aec0b06d6b8 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp @@ -19,11 +19,11 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ +#include "kalman_filter/kalman_filter.hpp" #include "multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" #include "multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" -#include #include class MultipleVehicleTracker : public Tracker 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 3a51c61349452..606c7bf013865 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,11 +19,10 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ +#include "kalman_filter/kalman_filter.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" #include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" -#include - class NormalVehicleTracker : public Tracker { private: diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp index 3d9bffb18a9db..cd661dab52c6e 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp @@ -19,10 +19,9 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ +#include "kalman_filter/kalman_filter.hpp" #include "tracker_base.hpp" -#include - class PassThroughTracker : public Tracker { private: diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp index ad81c504d3719..3c3ac038b085e 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp @@ -19,12 +19,11 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ +#include "kalman_filter/kalman_filter.hpp" #include "multi_object_tracker/tracker/model/bicycle_tracker.hpp" #include "multi_object_tracker/tracker/model/pedestrian_tracker.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" -#include - class PedestrianAndBicycleTracker : public Tracker { 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 386fd74d9b942..3c9dd5e91dac5 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,11 +19,10 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ +#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 - // cspell: ignore CTRV class PedestrianTracker : public Tracker diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp index 7d4e7a6f15b5c..3db1c3869769d 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp @@ -26,10 +26,10 @@ #include #include -#include "autoware_perception_msgs/msg/detected_object.hpp" -#include "autoware_perception_msgs/msg/tracked_object.hpp" -#include "geometry_msgs/msg/point.hpp" -#include "unique_identifier_msgs/msg/uuid.hpp" +#include +#include +#include +#include #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 e0467087acd70..ca8ecba160bd8 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,11 +19,10 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ +#include "kalman_filter/kalman_filter.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" #include "multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" -#include - class UnknownTracker : public Tracker { private: diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp index 5127d0448835c..472b8f2a309f3 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp @@ -19,10 +19,10 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ +#include "kalman_filter/kalman_filter.hpp" #include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include -#include #include #ifdef ROS_DISTRO_GALACTIC diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp index 6b071eddec7a9..657048079c46c 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp @@ -19,10 +19,10 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ +#include "kalman_filter/kalman_filter.hpp" #include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include -#include #include #ifdef ROS_DISTRO_GALACTIC diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp index 59032706b00d6..421e413d7aab7 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp @@ -19,10 +19,10 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ +#include "kalman_filter/kalman_filter.hpp" #include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include -#include #include #ifdef ROS_DISTRO_GALACTIC 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 1aca602ed66a3..e740612e96d4f 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 @@ -19,8 +19,9 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ +#include "kalman_filter/kalman_filter.hpp" + #include -#include #include #ifdef ROS_DISTRO_GALACTIC diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp index fe6d3d5c75564..811330bc5b3df 100644 --- a/perception/multi_object_tracker/src/debugger/debug_object.cpp +++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp @@ -14,7 +14,7 @@ #include "multi_object_tracker/debugger/debug_object.hpp" -#include "autoware_perception_msgs/msg/tracked_object.hpp" +#include #include 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 8cc552b417676..97c3d93d191b2 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -13,6 +13,15 @@ // limitations under the License. // // +#define EIGEN_MPL2_ONLY + +#include "multi_object_tracker/multi_object_tracker_core.hpp" + +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include +#include #include @@ -28,14 +37,6 @@ #include #include -#define EIGEN_MPL2_ONLY -#include "multi_object_tracker/multi_object_tracker_core.hpp" -#include "multi_object_tracker/utils/utils.hpp" - -#include -#include -#include - namespace { // Function to get the transform between two frames 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 41a170157da5a..4364cd3193e95 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -15,15 +15,19 @@ // // Author: v1.0 Yukihiro Saito // +#define EIGEN_MPL2_ONLY #include "multi_object_tracker/tracker/model/bicycle_tracker.hpp" #include "multi_object_tracker/utils/utils.hpp" +#include "object_recognition_utils/object_recognition_utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include "tier4_autoware_utils/math/normalization.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "tier4_autoware_utils/ros/msg_covariance.hpp" -#include -#include -#include -#include +#include +#include #include #include @@ -35,11 +39,6 @@ #else #include #endif -#include "object_recognition_utils/object_recognition_utils.hpp" - -#define EIGEN_MPL2_ONLY -#include -#include using Label = autoware_perception_msgs::msg::ObjectClassification; 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 3222bf4c66b3a..8e6739ab64d2a 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 @@ -15,15 +15,19 @@ // // Author: v1.0 Yukihiro Saito // +#define EIGEN_MPL2_ONLY #include "multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" #include "multi_object_tracker/utils/utils.hpp" +#include "object_recognition_utils/object_recognition_utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include "tier4_autoware_utils/math/normalization.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "tier4_autoware_utils/ros/msg_covariance.hpp" -#include -#include -#include -#include +#include +#include #include #include @@ -35,11 +39,6 @@ #else #include #endif -#include "object_recognition_utils/object_recognition_utils.hpp" - -#define EIGEN_MPL2_ONLY -#include -#include using Label = autoware_perception_msgs::msg::ObjectClassification; 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 9f4158a6282c7..433fffe0c747e 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 @@ -15,15 +15,19 @@ // // Author: v1.0 Yukihiro Saito // +#define EIGEN_MPL2_ONLY #include "multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" #include "multi_object_tracker/utils/utils.hpp" +#include "object_recognition_utils/object_recognition_utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include "tier4_autoware_utils/math/normalization.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "tier4_autoware_utils/ros/msg_covariance.hpp" -#include -#include -#include -#include +#include +#include #include #include @@ -35,11 +39,6 @@ #else #include #endif -#include "object_recognition_utils/object_recognition_utils.hpp" - -#define EIGEN_MPL2_ONLY -#include -#include using Label = autoware_perception_msgs::msg::ObjectClassification; diff --git a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp index b0d202036d79e..13821bf9b7318 100644 --- a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp @@ -15,8 +15,15 @@ // // Author: v1.0 Yutaka Shimizu // +#define EIGEN_MPL2_ONLY +#include "multi_object_tracker/tracker/model/pass_through_tracker.hpp" + +#include "multi_object_tracker/utils/utils.hpp" +#include "object_recognition_utils/object_recognition_utils.hpp" +#include "tier4_autoware_utils/ros/msg_covariance.hpp" -#include +#include +#include #include #include @@ -29,14 +36,6 @@ #include #endif -#define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/pass_through_tracker.hpp" -#include "multi_object_tracker/utils/utils.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" - -#include -#include - PassThroughTracker::PassThroughTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, 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 202a9cb25f809..ddfae8e2f1176 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -15,15 +15,19 @@ // // Author: v1.0 Yukihiro Saito // +#define EIGEN_MPL2_ONLY #include "multi_object_tracker/tracker/model/pedestrian_tracker.hpp" #include "multi_object_tracker/utils/utils.hpp" +#include "object_recognition_utils/object_recognition_utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include "tier4_autoware_utils/math/normalization.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "tier4_autoware_utils/ros/msg_covariance.hpp" -#include -#include -#include -#include +#include +#include #include #include @@ -35,11 +39,6 @@ #else #include #endif -#include "object_recognition_utils/object_recognition_utils.hpp" - -#define EIGEN_MPL2_ONLY -#include -#include using Label = autoware_perception_msgs::msg::ObjectClassification; 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 af4cd7f3df8ab..ae2cf3fff531d 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -11,15 +11,18 @@ // 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. +#define EIGEN_MPL2_ONLY #include "multi_object_tracker/tracker/model/unknown_tracker.hpp" #include "multi_object_tracker/utils/utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include "tier4_autoware_utils/math/normalization.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "tier4_autoware_utils/ros/msg_covariance.hpp" -#include -#include -#include -#include +#include +#include #include #include @@ -33,10 +36,6 @@ #endif #include "object_recognition_utils/object_recognition_utils.hpp" -#define EIGEN_MPL2_ONLY -#include -#include - UnknownTracker::UnknownTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, diff --git a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp index 5d45b1fc89678..7793e77de09ca 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp @@ -15,17 +15,16 @@ // // Author: v1.0 Taekjin Lee // +#define EIGEN_MPL2_ONLY #include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" #include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/utils/utils.hpp" +#include "tier4_autoware_utils/math/normalization.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "tier4_autoware_utils/ros/msg_covariance.hpp" -#include -#include -#include - -#define EIGEN_MPL2_ONLY #include #include diff --git a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp index dc1f235e1623a..a534002cd651d 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp @@ -15,17 +15,16 @@ // // Author: v1.0 Taekjin Lee // +#define EIGEN_MPL2_ONLY #include "multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" #include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/utils/utils.hpp" +#include "tier4_autoware_utils/math/normalization.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "tier4_autoware_utils/ros/msg_covariance.hpp" -#include -#include -#include - -#define EIGEN_MPL2_ONLY #include #include diff --git a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp index 33cb945777b0d..cf22e3256dca9 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp @@ -15,22 +15,21 @@ // // Author: v1.0 Taekjin Lee // +#define EIGEN_MPL2_ONLY #include "multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" #include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/utils/utils.hpp" +#include "tier4_autoware_utils/math/normalization.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "tier4_autoware_utils/ros/msg_covariance.hpp" -#include -#include -#include - -#include - -#define EIGEN_MPL2_ONLY #include #include +#include + // cspell: ignore CV // Constant Velocity (CV) motion model using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; From 9fb235c61e058013cf714104b14d4d37d8294d22 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 13 Jun 2024 15:06:16 +0900 Subject: [PATCH 16/30] feat: initial impl. of object model Signed-off-by: Taekjin LEE --- .../tracker/model/normal_vehicle_tracker.hpp | 3 + .../tracker/object_model/object_model.hpp | 296 ++++++++++++++++++ .../object_model/object_model_base.hpp | 155 --------- .../tracker/model/normal_vehicle_tracker.cpp | 100 +++--- 4 files changed, 341 insertions(+), 213 deletions(-) create mode 100644 perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model.hpp delete mode 100644 perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model_base.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 606c7bf013865..257b146649954 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 @@ -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/bicycle_motion_model.hpp" +#include "multi_object_tracker/tracker/object_model/object_model.hpp" class NormalVehicleTracker : public Tracker { @@ -29,6 +30,8 @@ class NormalVehicleTracker : public Tracker autoware_perception_msgs::msg::DetectedObject object_; rclcpp::Logger logger_; + object_model::ObjectModel object_model_ = object_model::normal_vehicle; + struct EkfParams { double r_cov_x; 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 new file mode 100644 index 0000000000000..8b45cd3bc3aec --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model.hpp @@ -0,0 +1,296 @@ +// 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__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ +#define MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ + +#include + +namespace +{ +constexpr double const_g = 9.81; + +template +constexpr T sq(const T x) +{ + return x * x; +} + +template +constexpr T deg2rad(const T deg) +{ + return deg * static_cast(M_PI) / static_cast(180.0); +} + +// cspell: ignore kmph +template +constexpr T kmph2mps(const T kmph) +{ + return kmph * static_cast(1000.0) / static_cast(3600.0); +} + +} // namespace + +namespace object_model +{ + +enum class ObjectModelType { NormalVehicle, BigVehicle, Bicycle, Pedestrian, Unknown }; +struct ObjectSize +{ + double length{0.0}; // [m] + double width{0.0}; // [m] + double height{0.0}; // [m] +}; +struct ObjectSizeLimit +{ + double length_min{0.0}; // [m] + double length_max{0.0}; // [m] + double width_min{0.0}; // [m] + double width_max{0.0}; // [m] + double height_min{0.0}; // [m] + double height_max{0.0}; // [m] +}; +struct MotionProcessNoise +{ + 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 +}; +struct MotionProcessLimit +{ + double acc_long_max{0.0}; // [m/s^2] + 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] +}; +struct StateCovariance +{ + double pos_x{0.0}; // [m^2] + double pos_y{0.0}; // [m^2] + double yaw{0.0}; // [rad^2] + double yaw_rate{0.0}; // [rad^2/s^2] + double vel_long{0.0}; // [m^2/s^2] + double vel_lat{0.0}; // [m^2/s^2] + double acc_long{0.0}; // [m^2/s^4] + double acc_lat{0.0}; // [m^2/s^4] +}; +struct BicycleModelState +{ + double slip_angle_max{0.0}; // [rad] max slip angle + double slip_rate_cov_min{0.0}; // [rad/s] uncertain slip angle change rate, minimum + double slip_rate_cov_max{0.0}; // [rad/s] uncertain slip angle change rate, maximum + double wheel_pos_ratio_front{0.0}; // [-] + double wheel_pos_ratio_rear{0.0}; // [-] + double wheel_pos_front_min{0.0}; // [m] + double wheel_pos_rear_min{0.0}; // [m] +}; + +class ObjectModel +{ +public: + ObjectSize init_size; + ObjectSizeLimit size_limit; + MotionProcessNoise process_noise; + MotionProcessLimit process_limit; + StateCovariance initial_covariance; + StateCovariance measurement_covariance; + BicycleModelState bicycle_state; + + explicit ObjectModel(const ObjectModelType & type) + { + switch (type) { + case ObjectModelType::NormalVehicle: + init_size.length = 3.0; + init_size.width = 2.0; + init_size.height = 1.8; + size_limit.length_min = 1.0; + size_limit.length_max = 20.0; + size_limit.width_min = 1.0; + size_limit.width_max = 5.0; + size_limit.height_min = 1.0; + size_limit.height_max = 5.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_limit.acc_long_max = const_g; + process_limit.acc_lat_max = const_g; + process_limit.vel_long_max = kmph2mps(100.0); + + // initial covariance + initial_covariance.pos_x = sq(1.0); + initial_covariance.pos_y = sq(0.3); + initial_covariance.yaw = sq(deg2rad(25.0)); + initial_covariance.vel_long = sq(kmph2mps(1000.0)); + initial_covariance.vel_lat = sq(0.2); + + // measurement noise model + measurement_covariance.pos_x = sq(0.5); + measurement_covariance.pos_y = sq(0.4); + measurement_covariance.yaw = sq(deg2rad(20.0)); + measurement_covariance.vel_long = sq(1.0); + + // bicycle motion model + bicycle_state.slip_angle_max = 30.0; // deg2rad(30.0); + bicycle_state.slip_rate_cov_min = 0.3; // sq(deg2rad(0.3)); + bicycle_state.slip_rate_cov_max = 10.0; // sq(deg2rad(10.0)); + bicycle_state.wheel_pos_ratio_front = 0.3; + bicycle_state.wheel_pos_ratio_rear = 0.25; + bicycle_state.wheel_pos_front_min = 1.0; + bicycle_state.wheel_pos_rear_min = 1.0; + break; + + case ObjectModelType::BigVehicle: + init_size.length = 10.0; + init_size.width = 2.5; + init_size.height = 3.0; + size_limit.length_min = 1.0; + size_limit.length_max = 20.0; + size_limit.width_min = 1.0; + size_limit.width_max = 5.0; + size_limit.height_min = 1.0; + size_limit.height_max = 5.0; + + process_noise.acc_long = const_g * 0.35; + process_noise.acc_lat = const_g * 0.15; + process_noise.yaw_rate_min = deg2rad(1.5); + process_noise.yaw_rate_max = deg2rad(15.0); + + process_limit.acc_long_max = const_g; + process_limit.acc_lat_max = const_g; + process_limit.vel_long_max = kmph2mps(100.0); + + // initial covariance + initial_covariance.pos_x = sq(1.0); + initial_covariance.pos_y = sq(0.3); + initial_covariance.yaw = sq(deg2rad(25.0)); + initial_covariance.vel_long = sq(kmph2mps(1000.0)); + initial_covariance.vel_lat = sq(0.2); + + // measurement noise model + measurement_covariance.pos_x = sq(0.5); + measurement_covariance.pos_y = sq(0.4); + measurement_covariance.yaw = sq(deg2rad(20.0)); + measurement_covariance.vel_long = sq(1.0); + + // bicycle motion model + bicycle_state.slip_angle_max = deg2rad(30.0); + bicycle_state.slip_rate_cov_min = sq(deg2rad(0.3)); + bicycle_state.slip_rate_cov_max = sq(deg2rad(10.0)); + bicycle_state.wheel_pos_ratio_front = 0.3; + bicycle_state.wheel_pos_ratio_rear = 0.25; + bicycle_state.wheel_pos_front_min = 1.0; + bicycle_state.wheel_pos_rear_min = 1.0; + break; + + case ObjectModelType::Bicycle: + init_size.length = 2.0; + init_size.width = 0.7; + init_size.height = 1.0; + size_limit.length_min = 1.0; + size_limit.length_max = 20.0; + size_limit.width_min = 1.0; + size_limit.width_max = 5.0; + size_limit.height_min = 1.0; + size_limit.height_max = 5.0; + + process_noise.acc_long = const_g * 0.35; + process_noise.acc_lat = const_g * 0.15; + process_noise.yaw_rate_min = deg2rad(1.5); + process_noise.yaw_rate_max = deg2rad(15.0); + + process_limit.acc_long_max = const_g; + process_limit.acc_lat_max = const_g; + process_limit.vel_long_max = kmph2mps(100.0); + + // initial covariance + initial_covariance.pos_x = sq(1.0); + initial_covariance.pos_y = sq(0.3); + initial_covariance.yaw = sq(deg2rad(25.0)); + initial_covariance.vel_long = sq(kmph2mps(1000.0)); + initial_covariance.vel_lat = sq(0.2); + + // measurement noise model + measurement_covariance.pos_x = sq(0.5); + measurement_covariance.pos_y = sq(0.4); + measurement_covariance.yaw = sq(deg2rad(20.0)); + measurement_covariance.vel_long = sq(1.0); + + // bicycle motion model + bicycle_state.slip_angle_max = deg2rad(30.0); + bicycle_state.slip_rate_cov_min = sq(deg2rad(0.3)); + bicycle_state.slip_rate_cov_max = sq(deg2rad(10.0)); + bicycle_state.wheel_pos_ratio_front = 0.3; + bicycle_state.wheel_pos_ratio_rear = 0.25; + bicycle_state.wheel_pos_front_min = 1.0; + bicycle_state.wheel_pos_rear_min = 1.0; + break; + + case ObjectModelType::Pedestrian: + init_size.length = 0.5; + init_size.width = 0.5; + init_size.height = 1.7; + size_limit.length_min = 0.3; + 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_max = 2.0; + + process_noise.acc_long = const_g * 0.35; + process_noise.acc_lat = const_g * 0.15; + process_noise.yaw_rate_min = deg2rad(1.5); + process_noise.yaw_rate_max = deg2rad(15.0); + + process_limit.acc_long_max = const_g; + process_limit.acc_lat_max = const_g; + process_limit.vel_long_max = kmph2mps(100.0); + + // initial covariance + initial_covariance.pos_x = sq(1.0); + initial_covariance.pos_y = sq(0.3); + initial_covariance.yaw = sq(deg2rad(25.0)); + initial_covariance.vel_long = sq(kmph2mps(1000.0)); + initial_covariance.vel_lat = sq(0.2); + + // measurement noise model + measurement_covariance.pos_x = sq(0.5); + measurement_covariance.pos_y = sq(0.4); + measurement_covariance.yaw = sq(deg2rad(20.0)); + measurement_covariance.vel_long = sq(1.0); + break; + + default: + break; + } + } + virtual ~ObjectModel() = default; +}; + +// create static objects by using ObjectModel class +static const ObjectModel normal_vehicle(ObjectModelType::NormalVehicle); +static const ObjectModel big_vehicle(ObjectModelType::BigVehicle); +static const ObjectModel bicycle(ObjectModelType::Bicycle); +static const ObjectModel pedestrian(ObjectModelType::Pedestrian); + +} // namespace object_model + +#endif // MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model_base.hpp deleted file mode 100644 index 52e5b6108a0d9..0000000000000 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model_base.hpp +++ /dev/null @@ -1,155 +0,0 @@ -// 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__TRACKER__OBJECT_MODEL__OBJECT_MODEL_BASE_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_BASE_HPP_ - -#include - -namespace -{ -constexpr double const_g = 9.81; -double deg2rad(const double deg) -{ - return deg * M_PI / 180.0; -} -} // namespace - -namespace object_model -{ - -enum class ObjectModelType { NORMAL_VEHICLE, BIG_VEHICLE, BICYCLE, PEDESTRIAN, STATIC, UNKNOWN }; -struct ObjectSize -{ - double length; // [m] - double width; // [m] - double height; // [m] -}; -struct ObjectSizeLimit -{ - double length_min; // [m] - double length_max; // [m] - double width_min; // [m] - double width_max; // [m] - double height_min; // [m] - double height_max; // [m] -}; -struct MotionProcessNoise -{ - double acc_long; // [m/s^2] - double acc_lat; // [m/s^2] - double yaw_rate_min; // [rad/s] - double yaw_rate_max; // [rad/s] - double slip_rate_min; - double slip_rate_max; -}; -struct MotionProcessLimit -{ - double slip_angle_max; - double acc_long_max; - double acc_lat_max; - double vel_long_max; -}; -struct StateCovariance -{ - double pos_x; // [m^2] - double pos_y; // [m^2] - double yaw; // [rad^2] - double yaw_rate; // [rad^2/s^2] - double acc_long; // [m^2/s^4] - double acc_lat; // [m^2/s^4] - double vel_long; // [m^2/s^2] - double vel_lat; // [m^2/s^2] -}; -struct BicycleModelState -{ - double slip_angle; // [rad^2] - double slip_rate; // [rad^2/s^2] - double wheel_pos_ratio_front; // [-] - double wheel_pos_ratio_rear; // [-] - double wheel_pos_front_min; // [m] - double wheel_pos_rear_min; // [m] -}; - -class ObjectModel -{ -public: - ObjectSize size; - ObjectSizeLimit size_limit; - MotionProcessNoise process_noise; - MotionProcessLimit process_limit; - StateCovariance initial_covariance; - StateCovariance state_covariance; - BicycleModelState bicycle_model_state; - - ObjectModel(const ObjectModelType & type) - { - switch (type) { - case ObjectModelType::NORMAL_VEHICLE: - size.length = 3.0; - size.width = 2.0; - size.height = 1.8; - size_limit.length_min = 2.0; - size_limit.length_max = 5.0; - size_limit.width_min = 1.5; - size_limit.width_max = 3.0; - size_limit.height_min = 1.5; - 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; - process_noise.yaw_rate_max = 15.0; - process_noise.slip_rate_min = 0.3; - process_noise.slip_rate_max = 10.0; - - process_limit.slip_angle_max = 30; - process_limit.acc_long_max = 2.0; - process_limit.acc_lat_max = 2.0; - process_limit.vel_long_max = 30.0; - - // initial covariance - initial_covariance.pos_x = 0.5; - initial_covariance.pos_y = 0.4; - initial_covariance.yaw = deg2rad(20.0); - initial_covariance.yaw_rate = deg2rad(10.0); - initial_covariance.acc_long = 1.0; - initial_covariance.acc_lat = 1.0; - initial_covariance.vel_long = 1.0; - initial_covariance.vel_lat = 1.0; - - // noise model - state_covariance.pos_x = 0.5; - state_covariance.pos_y = 0.4; - state_covariance.yaw = 0.1; - state_covariance.yaw_rate = 0.1; - state_covariance.acc_long = 0.1; - state_covariance.acc_lat = 0.1; - state_covariance.vel_long = 0.1; - state_covariance.vel_lat = 0.1; - break; - } - } - virtual ~ObjectModel() = default; -}; - -ObjectModel normal_vehicle(ObjectModelType::NORMAL_VEHICLE); - -} // namespace object_model - -#endif // MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_BASE_HPP_ 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 433fffe0c747e..f71fb89a4dceb 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 @@ -56,17 +56,6 @@ NormalVehicleTracker::NormalVehicleTracker( // initialize existence probability initializeExistenceProbabilities(channel_index, object.existence_probability); - // 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 = autoware::universe_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); - // velocity deviation threshold // if the predicted velocity is close to the observed velocity, // the observed velocity is used as the measurement. @@ -83,7 +72,9 @@ NormalVehicleTracker::NormalVehicleTracker( logger_, "NormalVehicleTracker::NormalVehicleTracker: Failed to convert convex hull to bounding " "box."); - bounding_box_ = {3.0, 2.0, 1.8}; // default value + bounding_box_ = { + object_model_.init_size.length, object_model_.init_size.width, + object_model_.init_size.height}; // default value } else { bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, @@ -91,28 +82,26 @@ NormalVehicleTracker::NormalVehicleTracker( } } // set maximum and minimum size - constexpr double max_size = 20.0; - constexpr double min_size = 1.0; - 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); + 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); // 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 + const double q_stddev_acc_long = object_model_.process_noise.acc_long; + const double q_stddev_acc_lat = object_model_.process_noise.acc_lat; + const double q_stddev_yaw_rate_min = object_model_.process_noise.yaw_rate_min; + const double q_stddev_yaw_rate_max = object_model_.process_noise.yaw_rate_max; + const double q_stddev_slip_rate_min = object_model_.bicycle_state.slip_rate_cov_min; + const double q_stddev_slip_rate_max = object_model_.bicycle_state.slip_rate_cov_max; + const double q_max_slip_angle = object_model_.bicycle_state.slip_angle_max; + const double lf_ratio = object_model_.bicycle_state.wheel_pos_ratio_front; + const double lf_min = object_model_.bicycle_state.wheel_pos_front_min; + const double lr_ratio = object_model_.bicycle_state.wheel_pos_ratio_rear; + const double lr_min = object_model_.bicycle_state.wheel_pos_rear_min; 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, @@ -121,8 +110,8 @@ NormalVehicleTracker::NormalVehicleTracker( // Set motion limits { - constexpr double max_vel = autoware::universe_utils::kmph2mps(100); // [m/s] maximum velocity - constexpr double max_slip = 30; // [deg] maximum slip angle + const double max_vel = object_model_.process_limit.vel_long_max; + const double max_slip = object_model_.bicycle_state.slip_angle_max; motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle } @@ -143,13 +132,9 @@ NormalVehicleTracker::NormalVehicleTracker( 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 = - autoware::universe_utils::deg2rad(25); // in map coordinate [rad] - constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x; - constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y; - constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw; + const auto & p0_cov_x = object_model_.initial_covariance.pos_x; + const auto & p0_cov_y = object_model_.initial_covariance.pos_y; + const auto & p0_cov_yaw = object_model_.initial_covariance.yaw; const double cos_yaw = std::cos(yaw); const double sin_yaw = std::sin(yaw); @@ -162,9 +147,7 @@ NormalVehicleTracker::NormalVehicleTracker( } if (!object.kinematics.has_twist_covariance) { - constexpr double p0_stddev_vel = - autoware::universe_utils::kmph2mps(1000); // in object coordinate [m/s] - vel_cov = std::pow(p0_stddev_vel, 2.0); + vel_cov = object_model_.initial_covariance.vel_long; } else { vel_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X]; } @@ -217,8 +200,8 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { // measurement noise covariance - auto r_cov_x = static_cast(ekf_params_.r_cov_x); - auto r_cov_y = static_cast(ekf_params_.r_cov_y); + auto r_cov_x = object_model_.measurement_covariance.pos_x; + auto r_cov_y = object_model_.measurement_covariance.pos_y; const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); if (utils::isLargeVehicleLabel(label)) { // if label is changed, enlarge the measurement noise covariance @@ -243,18 +226,18 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO 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 - 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] = ekf_params_.r_cov_yaw; // yaw - yaw + 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] = ekf_params_.r_cov_vel; // vel - vel + twist_cov[XYZRPY_COV_IDX::X_X] = object_model_.measurement_covariance.vel_long; // vel - vel } return updating_object; @@ -330,11 +313,12 @@ bool NormalVehicleTracker::measureWithShape( bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; // set maximum and minimum size - constexpr double max_size = 20.0; - constexpr double min_size = 1.0; - 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); + 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); // update motion model motion_model_.updateExtendedState(bounding_box_.length); From cbb1a58fabe7e597ee48197ba3338b4e62c125dc Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 13 Jun 2024 16:11:56 +0900 Subject: [PATCH 17/30] feat: impl. object model to bicycle, big, normal Signed-off-by: Taekjin LEE --- .../tracker/model/bicycle_tracker.hpp | 3 + .../tracker/model/big_vehicle_tracker.hpp | 10 +- .../tracker/model/normal_vehicle_tracker.hpp | 7 - .../tracker/object_model/object_model.hpp | 90 +++++++------ .../src/tracker/model/bicycle_tracker.cpp | 121 ++++++++--------- .../src/tracker/model/big_vehicle_tracker.cpp | 123 ++++++++---------- .../tracker/model/normal_vehicle_tracker.cpp | 25 ++-- 7 files changed, 176 insertions(+), 203 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 e9fdb09d91be6..0e66bcb62fccf 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 @@ -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/bicycle_motion_model.hpp" +#include "multi_object_tracker/tracker/object_model/object_model.hpp" class BicycleTracker : public Tracker { @@ -29,6 +30,8 @@ class BicycleTracker : public Tracker autoware_perception_msgs::msg::DetectedObject object_; rclcpp::Logger logger_; + object_model::ObjectModel object_model_ = object_model::bicycle; + struct EkfParams { double r_cov_x; 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 19518b8cb118f..7112e6a08ade1 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 @@ -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/bicycle_motion_model.hpp" +#include "multi_object_tracker/tracker/object_model/object_model.hpp" class BigVehicleTracker : public Tracker { @@ -29,13 +30,8 @@ class BigVehicleTracker : 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; - double r_cov_vel; - } ekf_params_; + object_model::ObjectModel object_model_ = object_model::big_vehicle; + double velocity_deviation_threshold_; double z_; 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 257b146649954..1165cecab258b 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,13 +32,6 @@ class NormalVehicleTracker : public Tracker object_model::ObjectModel object_model_ = object_model::normal_vehicle; - struct EkfParams - { - double r_cov_x; - double r_cov_y; - double r_cov_yaw; - double r_cov_vel; - } ekf_params_; double velocity_deviation_threshold_; 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 8b45cd3bc3aec..1acd40dae40af 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 @@ -92,6 +92,7 @@ struct StateCovariance }; struct BicycleModelState { + double init_slip_angle_cov{0.0}; // [rad^2/s^2] initial slip angle covariance double slip_angle_max{0.0}; // [rad] max slip angle double slip_rate_cov_min{0.0}; // [rad/s] uncertain slip angle change rate, minimum double slip_rate_cov_max{0.0}; // [rad/s] uncertain slip angle change rate, maximum @@ -133,7 +134,7 @@ class ObjectModel process_limit.acc_long_max = const_g; process_limit.acc_lat_max = const_g; - process_limit.vel_long_max = kmph2mps(100.0); + process_limit.vel_long_max = kmph2mps(140.0); // initial covariance initial_covariance.pos_x = sq(1.0); @@ -149,6 +150,7 @@ class ObjectModel measurement_covariance.vel_long = sq(1.0); // bicycle motion model + bicycle_state.init_slip_angle_cov = sq(deg2rad(5.0)); bicycle_state.slip_angle_max = 30.0; // deg2rad(30.0); bicycle_state.slip_rate_cov_min = 0.3; // sq(deg2rad(0.3)); bicycle_state.slip_rate_cov_max = 10.0; // sq(deg2rad(10.0)); @@ -159,28 +161,28 @@ class ObjectModel break; case ObjectModelType::BigVehicle: - init_size.length = 10.0; - init_size.width = 2.5; - init_size.height = 3.0; + init_size.length = 6.0; + init_size.width = 2.0; + init_size.height = 2.0; size_limit.length_min = 1.0; - size_limit.length_max = 20.0; + size_limit.length_max = 35.0; size_limit.width_min = 1.0; - size_limit.width_max = 5.0; + size_limit.width_max = 10.0; size_limit.height_min = 1.0; - size_limit.height_max = 5.0; + size_limit.height_max = 10.0; process_noise.acc_long = const_g * 0.35; process_noise.acc_lat = const_g * 0.15; - process_noise.yaw_rate_min = deg2rad(1.5); - process_noise.yaw_rate_max = deg2rad(15.0); + process_noise.yaw_rate_min = 1.5; // deg2rad(1.5); + process_noise.yaw_rate_max = 15.0; // deg2rad(15.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.vel_long_max = kmph2mps(140.0); // initial covariance - initial_covariance.pos_x = sq(1.0); - initial_covariance.pos_y = sq(0.3); + initial_covariance.pos_x = sq(1.5); + initial_covariance.pos_y = sq(0.5); initial_covariance.yaw = sq(deg2rad(25.0)); initial_covariance.vel_long = sq(kmph2mps(1000.0)); initial_covariance.vel_lat = sq(0.2); @@ -189,41 +191,42 @@ class ObjectModel measurement_covariance.pos_x = sq(0.5); measurement_covariance.pos_y = sq(0.4); measurement_covariance.yaw = sq(deg2rad(20.0)); - measurement_covariance.vel_long = sq(1.0); + measurement_covariance.vel_long = sq(kmph2mps(10.0)); // bicycle motion model - bicycle_state.slip_angle_max = deg2rad(30.0); - bicycle_state.slip_rate_cov_min = sq(deg2rad(0.3)); - bicycle_state.slip_rate_cov_max = sq(deg2rad(10.0)); + bicycle_state.init_slip_angle_cov = sq(deg2rad(5.0)); + bicycle_state.slip_angle_max = 30.0; // deg2rad(30.0); + bicycle_state.slip_rate_cov_min = 0.3; // sq(deg2rad(0.3)); + bicycle_state.slip_rate_cov_max = 10.0; // sq(deg2rad(10.0)); bicycle_state.wheel_pos_ratio_front = 0.3; bicycle_state.wheel_pos_ratio_rear = 0.25; - bicycle_state.wheel_pos_front_min = 1.0; - bicycle_state.wheel_pos_rear_min = 1.0; + bicycle_state.wheel_pos_front_min = 1.5; + bicycle_state.wheel_pos_rear_min = 1.5; break; case ObjectModelType::Bicycle: - init_size.length = 2.0; - init_size.width = 0.7; - init_size.height = 1.0; - size_limit.length_min = 1.0; - size_limit.length_max = 20.0; - size_limit.width_min = 1.0; - size_limit.width_max = 5.0; - size_limit.height_min = 1.0; - size_limit.height_max = 5.0; + init_size.length = 1.0; + init_size.width = 0.5; + init_size.height = 1.7; + size_limit.length_min = 0.5; + size_limit.length_max = 8.0; + size_limit.width_min = 0.5; + size_limit.width_max = 3.0; + size_limit.height_min = 0.5; + size_limit.height_max = 2.5; process_noise.acc_long = const_g * 0.35; process_noise.acc_lat = const_g * 0.15; - process_noise.yaw_rate_min = deg2rad(1.5); - process_noise.yaw_rate_max = deg2rad(15.0); + process_noise.yaw_rate_min = 5.0; // deg2rad(5.0); + process_noise.yaw_rate_max = 15.0; // deg2rad(15.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.vel_long_max = kmph2mps(120.0); // initial covariance - initial_covariance.pos_x = sq(1.0); - initial_covariance.pos_y = sq(0.3); + initial_covariance.pos_x = sq(0.8); + initial_covariance.pos_y = sq(0.5); initial_covariance.yaw = sq(deg2rad(25.0)); initial_covariance.vel_long = sq(kmph2mps(1000.0)); initial_covariance.vel_lat = sq(0.2); @@ -231,17 +234,18 @@ class ObjectModel // measurement noise model measurement_covariance.pos_x = sq(0.5); measurement_covariance.pos_y = sq(0.4); - measurement_covariance.yaw = sq(deg2rad(20.0)); - measurement_covariance.vel_long = sq(1.0); + measurement_covariance.yaw = sq(deg2rad(30.0)); + measurement_covariance.vel_long = sq(kmph2mps(10.0)); // bicycle motion model - bicycle_state.slip_angle_max = deg2rad(30.0); - bicycle_state.slip_rate_cov_min = sq(deg2rad(0.3)); - bicycle_state.slip_rate_cov_max = sq(deg2rad(10.0)); + bicycle_state.init_slip_angle_cov = sq(deg2rad(5.0)); + bicycle_state.slip_angle_max = 30.0; // deg2rad(30.0); + bicycle_state.slip_rate_cov_min = 1.0; // sq(deg2rad(1.0)); + bicycle_state.slip_rate_cov_max = 10.0; // sq(deg2rad(10.0)); bicycle_state.wheel_pos_ratio_front = 0.3; - bicycle_state.wheel_pos_ratio_rear = 0.25; - bicycle_state.wheel_pos_front_min = 1.0; - bicycle_state.wheel_pos_rear_min = 1.0; + bicycle_state.wheel_pos_ratio_rear = 0.3; + bicycle_state.wheel_pos_front_min = 0.3; + bicycle_state.wheel_pos_rear_min = 0.3; break; case ObjectModelType::Pedestrian: @@ -257,8 +261,8 @@ class ObjectModel process_noise.acc_long = const_g * 0.35; process_noise.acc_lat = const_g * 0.15; - process_noise.yaw_rate_min = deg2rad(1.5); - process_noise.yaw_rate_max = deg2rad(15.0); + process_noise.yaw_rate_min = 1.5; // deg2rad(1.5); + process_noise.yaw_rate_max = 15.0; // deg2rad(15.0); process_limit.acc_long_max = const_g; process_limit.acc_lat_max = const_g; @@ -275,7 +279,7 @@ class ObjectModel measurement_covariance.pos_x = sq(0.5); measurement_covariance.pos_y = sq(0.4); measurement_covariance.yaw = sq(deg2rad(20.0)); - measurement_covariance.vel_long = sq(1.0); + measurement_covariance.vel_long = sq(kmph2mps(10.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 4364cd3193e95..36abba3839d21 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -55,45 +55,36 @@ BicycleTracker::BicycleTracker( // initialize existence probability initializeExistenceProbabilities(channel_index, object.existence_probability); - // Initialize parameters - // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty - double r_stddev_x = 0.5; // in vehicle coordinate [m] - double r_stddev_y = 0.4; // in vehicle coordinate [m] - double r_stddev_yaw = autoware::universe_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); - // OBJECT SHAPE MODEL 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}; } else { - bounding_box_ = {1.0, 0.5, 1.7}; + bounding_box_ = { + object_model_.init_size.length, object_model_.init_size.width, + object_model_.init_size.height}; // default value } // 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); + 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); // 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 + const double q_stddev_acc_long = object_model_.process_noise.acc_long; + const double q_stddev_acc_lat = object_model_.process_noise.acc_lat; + const double q_stddev_yaw_rate_min = object_model_.process_noise.yaw_rate_min; + const double q_stddev_yaw_rate_max = object_model_.process_noise.yaw_rate_max; + const double q_stddev_slip_rate_min = object_model_.bicycle_state.slip_rate_cov_min; + const double q_stddev_slip_rate_max = object_model_.bicycle_state.slip_rate_cov_max; + const double q_max_slip_angle = object_model_.bicycle_state.slip_angle_max; + const double lf_ratio = object_model_.bicycle_state.wheel_pos_ratio_front; + const double lf_min = object_model_.bicycle_state.wheel_pos_front_min; + const double lr_ratio = object_model_.bicycle_state.wheel_pos_ratio_rear; + const double lr_min = object_model_.bicycle_state.wheel_pos_rear_min; 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, @@ -102,8 +93,8 @@ BicycleTracker::BicycleTracker( // Set motion limits { - constexpr double max_vel = autoware::universe_utils::kmph2mps(80); // [m/s] maximum velocity - constexpr double max_slip = 30; // [deg] maximum slip angle + const double max_vel = object_model_.process_limit.vel_long_max; + const double max_slip = object_model_.bicycle_state.slip_angle_max; motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle } @@ -113,24 +104,13 @@ BicycleTracker::BicycleTracker( 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 = 10.0; - const double & length = bounding_box_.length; - - if (object.kinematics.has_twist) { - vel = object.kinematics.twist_with_covariance.twist.linear.x; - } + auto pose_cov = object.kinematics.pose_with_covariance.covariance; 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 = - autoware::universe_utils::deg2rad(25); // in map coordinate [rad] - constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x; - constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y; - constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw; + const auto & p0_cov_x = object_model_.initial_covariance.pos_x; + const auto & p0_cov_y = object_model_.initial_covariance.pos_y; + const auto & p0_cov_yaw = object_model_.initial_covariance.yaw; const double cos_yaw = std::cos(yaw); const double sin_yaw = std::sin(yaw); @@ -142,18 +122,18 @@ BicycleTracker::BicycleTracker( pose_cov[XYZRPY_COV_IDX::YAW_YAW] = p0_cov_yaw; } - if (!object.kinematics.has_twist_covariance) { - constexpr double p0_stddev_vel = - autoware::universe_utils::kmph2mps(1000); // in object coordinate [m/s] - vel_cov = std::pow(p0_stddev_vel, 2.0); - } else { + double vel = 0.0; + double vel_cov = object_model_.initial_covariance.vel_long; + if (object.kinematics.has_twist) { + vel = object.kinematics.twist_with_covariance.twist.linear.x; + } + if (object.kinematics.has_twist_covariance) { vel_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X]; } const double slip = 0.0; - const double p0_stddev_slip = - autoware::universe_utils::deg2rad(5); // in object coordinate [rad/s] - const double slip_cov = std::pow(p0_stddev_slip, 2.0); + const double slip_cov = object_model_.bicycle_state.init_slip_angle_cov; + const double & length = bounding_box_.length; // initialize motion model motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); @@ -181,14 +161,37 @@ autoware_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( // 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 + 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; + // fill covariance matrix using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; - pose_cov[XYZRPY_COV_IDX::X_X] = ekf_params_.r_cov_x; // x - x - pose_cov[XYZRPY_COV_IDX::X_Y] = 0; // x - y - pose_cov[XYZRPY_COV_IDX::Y_X] = 0; // y - x - pose_cov[XYZRPY_COV_IDX::Y_Y] = ekf_params_.r_cov_y; // y - y - pose_cov[XYZRPY_COV_IDX::YAW_YAW] = ekf_params_.r_cov_yaw; // yaw - yaw + 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[XYZRPY_COV_IDX::X_X] = + 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 + 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; 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 8e6739ab64d2a..758926c929bcb 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 @@ -56,17 +56,6 @@ BigVehicleTracker::BigVehicleTracker( // initialize existence probability initializeExistenceProbabilities(channel_index, object.existence_probability); - // 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 = autoware::universe_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); - // velocity deviation threshold // if the predicted velocity is close to the observed velocity, // the observed velocity is used as the measurement. @@ -82,7 +71,9 @@ BigVehicleTracker::BigVehicleTracker( RCLCPP_WARN( logger_, "BigVehicleTracker::BigVehicleTracker: Failed to convert convex hull to bounding box."); - bounding_box_ = {6.0, 2.0, 2.0}; // default value + bounding_box_ = { + object_model_.init_size.length, object_model_.init_size.width, + object_model_.init_size.height}; // default value } else { bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, @@ -90,28 +81,26 @@ BigVehicleTracker::BigVehicleTracker( } } // set maximum and minimum size - constexpr double max_size = 30.0; - constexpr double min_size = 1.0; - 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); + 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); // 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 + const double q_stddev_acc_long = object_model_.process_noise.acc_long; + const double q_stddev_acc_lat = object_model_.process_noise.acc_lat; + const double q_stddev_yaw_rate_min = object_model_.process_noise.yaw_rate_min; + const double q_stddev_yaw_rate_max = object_model_.process_noise.yaw_rate_max; + const double q_stddev_slip_rate_min = object_model_.bicycle_state.slip_rate_cov_min; + const double q_stddev_slip_rate_max = object_model_.bicycle_state.slip_rate_cov_max; + const double q_max_slip_angle = object_model_.bicycle_state.slip_angle_max; + const double lf_ratio = object_model_.bicycle_state.wheel_pos_ratio_front; + const double lf_min = object_model_.bicycle_state.wheel_pos_front_min; + const double lr_ratio = object_model_.bicycle_state.wheel_pos_ratio_rear; + const double lr_min = object_model_.bicycle_state.wheel_pos_rear_min; 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, @@ -120,8 +109,8 @@ BigVehicleTracker::BigVehicleTracker( // Set motion limits { - constexpr double max_vel = autoware::universe_utils::kmph2mps(100); // [m/s] maximum velocity - constexpr double max_slip = 30; // [deg] maximum slip angle + const double max_vel = object_model_.process_limit.vel_long_max; + const double max_slip = object_model_.bicycle_state.slip_angle_max; motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle } @@ -131,24 +120,13 @@ BigVehicleTracker::BigVehicleTracker( 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 = 10.0; - const double & length = bounding_box_.length; - - if (object.kinematics.has_twist) { - vel = object.kinematics.twist_with_covariance.twist.linear.x; - } + auto pose_cov = object.kinematics.pose_with_covariance.covariance; 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 = - autoware::universe_utils::deg2rad(25); // in map coordinate [rad] - constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x; - constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y; - constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw; + const auto & p0_cov_x = object_model_.initial_covariance.pos_x; + const auto & p0_cov_y = object_model_.initial_covariance.pos_y; + const auto & p0_cov_yaw = object_model_.initial_covariance.yaw; const double cos_yaw = std::cos(yaw); const double sin_yaw = std::sin(yaw); @@ -160,18 +138,18 @@ BigVehicleTracker::BigVehicleTracker( pose_cov[XYZRPY_COV_IDX::YAW_YAW] = p0_cov_yaw; } - if (!object.kinematics.has_twist_covariance) { - constexpr double p0_stddev_vel = - autoware::universe_utils::kmph2mps(1000); // in object coordinate [m/s] - vel_cov = std::pow(p0_stddev_vel, 2.0); - } else { + double vel = 0.0; + double vel_cov = object_model_.initial_covariance.vel_long; + if (object.kinematics.has_twist) { + vel = object.kinematics.twist_with_covariance.twist.linear.x; + } + if (object.kinematics.has_twist_covariance) { vel_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X]; } const double slip = 0.0; - const double p0_stddev_slip = - autoware::universe_utils::deg2rad(5); // in object coordinate [rad/s] - const double slip_cov = std::pow(p0_stddev_slip, 2.0); + const double slip_cov = object_model_.bicycle_state.init_slip_angle_cov; + const double & length = bounding_box_.length; // initialize motion model motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); @@ -216,8 +194,8 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { // measurement noise covariance - auto r_cov_x = static_cast(ekf_params_.r_cov_x); - auto r_cov_y = static_cast(ekf_params_.r_cov_y); + auto r_cov_x = object_model_.measurement_covariance.pos_x; + auto r_cov_y = object_model_.measurement_covariance.pos_y; const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); if (label == autoware_perception_msgs::msg::ObjectClassification::CAR) { // if label is changed, enlarge the measurement noise covariance @@ -242,18 +220,18 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::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 - 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] = ekf_params_.r_cov_yaw; // yaw - yaw + 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] = ekf_params_.r_cov_vel; // vel - vel + twist_cov[XYZRPY_COV_IDX::X_X] = object_model_.measurement_covariance.vel_long; // vel - vel } return updating_object; @@ -311,7 +289,7 @@ bool BigVehicleTracker::measureWithShape( } // check object size abnormality - constexpr double size_max = 40.0; // [m] + constexpr double size_max = 35.0; // [m] constexpr double size_min = 1.0; // [m] bool is_size_valid = (object.shape.dimensions.x <= size_max && object.shape.dimensions.y <= size_max && @@ -329,11 +307,12 @@ bool BigVehicleTracker::measureWithShape( bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; // set maximum and minimum size - constexpr double max_size = 30.0; - constexpr double min_size = 1.0; - 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); + 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); // 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 f71fb89a4dceb..eb8324b5cf055 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 @@ -121,15 +121,8 @@ NormalVehicleTracker::NormalVehicleTracker( 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 = 10.0; - const double & length = bounding_box_.length; - - if (object.kinematics.has_twist) { - vel = object.kinematics.twist_with_covariance.twist.linear.x; - } + auto pose_cov = object.kinematics.pose_with_covariance.covariance; if (!object.kinematics.has_position_covariance) { // initial state covariance const auto & p0_cov_x = object_model_.initial_covariance.pos_x; @@ -146,16 +139,18 @@ NormalVehicleTracker::NormalVehicleTracker( pose_cov[XYZRPY_COV_IDX::YAW_YAW] = p0_cov_yaw; } - if (!object.kinematics.has_twist_covariance) { - vel_cov = object_model_.initial_covariance.vel_long; - } else { + double vel = 0.0; + double vel_cov = object_model_.initial_covariance.vel_long; + if (object.kinematics.has_twist) { + vel = object.kinematics.twist_with_covariance.twist.linear.x; + } + if (object.kinematics.has_twist_covariance) { vel_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X]; } const double slip = 0.0; - const double p0_stddev_slip = - autoware::universe_utils::deg2rad(5); // in object coordinate [rad/s] - const double slip_cov = std::pow(p0_stddev_slip, 2.0); + const double slip_cov = object_model_.bicycle_state.init_slip_angle_cov; + const double & length = bounding_box_.length; // initialize motion model motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); @@ -295,7 +290,7 @@ bool NormalVehicleTracker::measureWithShape( } // check object size abnormality - constexpr double size_max = 30.0; // [m] + constexpr double size_max = 35.0; // [m] constexpr double size_min = 1.0; // [m] bool is_size_valid = (object.shape.dimensions.x <= size_max && object.shape.dimensions.y <= size_max && From cb895a9c55a9746e7d4618906e3ad08d91773a9b Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 13 Jun 2024 16:55:20 +0900 Subject: [PATCH 18/30] feat: impl. object model to pedestrian Signed-off-by: Taekjin LEE --- .../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 36abba3839d21..a412d9058bc47 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 758926c929bcb..e3180c5698a4f 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 eb8324b5cf055..3b2bcd5473992 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 ddfae8e2f1176..01c0994317e02 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 = autoware::universe_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 = autoware::universe_utils::deg2rad(20); // [rad/s] - constexpr double q_stddev_vx = 9.8 * 0.3; // [m/(s*s)] - constexpr double q_stddev_wz = autoware::universe_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; } From 716d1b880aff7949987d3fcfd31b57176adc4b0c Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 13 Jun 2024 17:20:50 +0900 Subject: [PATCH 19/30] feat: impl. cont Signed-off-by: Taekjin LEE --- .../tracker/object_model/object_model.hpp | 14 ++--- .../src/tracker/model/bicycle_tracker.cpp | 4 +- .../src/tracker/model/big_vehicle_tracker.cpp | 4 +- .../tracker/model/normal_vehicle_tracker.cpp | 4 +- .../src/tracker/model/pedestrian_tracker.cpp | 54 ++++++++----------- .../motion_model/ctrv_motion_model.cpp | 10 ++-- 6 files changed, 40 insertions(+), 50 deletions(-) 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 8f63dee9f872e..47ed52f4155e0 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 @@ -74,7 +74,7 @@ struct MotionProcessNoise 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 rotate_rate{0.0}; // [rad/s^2] uncertain rotation change rate + double acc_turn{0.0}; // [rad/s^2] uncertain rotation acceleration }; struct MotionProcessLimit { @@ -268,17 +268,17 @@ class ObjectModel 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_noise.acc_turn = deg2rad(30.0); 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); - initial_covariance.pos_y = sq(0.3); - initial_covariance.yaw = sq(deg2rad(25.0)); - initial_covariance.vel_long = sq(kmph2mps(1000.0)); - initial_covariance.vel_lat = sq(0.2); + initial_covariance.pos_x = sq(2.0); + initial_covariance.pos_y = sq(2.0); + initial_covariance.yaw = sq(deg2rad(1000.0)); + initial_covariance.vel_long = sq(kmph2mps(120.0)); + initial_covariance.yaw_rate = sq(deg2rad(360.0)); // measurement noise model measurement_covariance.pos_x = sq(0.4); 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 a412d9058bc47..9968b9c3e6e01 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -176,10 +176,10 @@ autoware_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( 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); + const double sin_2yaw = std::sin(2.0 * pose_yaw); pose_cov[XYZRPY_COV_IDX::X_X] = 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::X_Y] = 0.5 * (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 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 e3180c5698a4f..4765a12e6bdc0 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 @@ -216,10 +216,10 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje 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); + const double sin_2yaw = std::sin(2.0 * pose_yaw); pose_cov[XYZRPY_COV_IDX::X_X] = 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::X_Y] = 0.5 * (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 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 3b2bcd5473992..247ad0f8c64c7 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 @@ -217,10 +217,10 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO 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); + const double sin_2yaw = std::sin(2.0 * pose_yaw); pose_cov[XYZRPY_COV_IDX::X_X] = 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::X_Y] = 0.5 * (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 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 01c0994317e02..733d57dcf3d67 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -86,15 +86,16 @@ PedestrianTracker::PedestrianTracker( 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; + const double q_stddev_wz = object_model_.process_noise.acc_turn; motion_model_.setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vx, q_stddev_wz); } // Set motion limits - motion_model_.setMotionLimits( - autoware::universe_utils::kmph2mps(100), /* [m/s] maximum velocity */ - 30.0 /* [deg/s] maximum turn rate */ - ); + { + const double max_vel = object_model_.process_limit.vel_long_max; + const double max_turn_rate = object_model_.process_limit.yaw_rate_max; + motion_model_.setMotionLimits(max_vel, max_turn_rate); // maximum velocity and slip angle + } // Set initial state { @@ -102,26 +103,13 @@ PedestrianTracker::PedestrianTracker( 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 = 10.0; - double wz_cov = 10.0; - - if (object.kinematics.has_twist) { - vel = object.kinematics.twist_with_covariance.twist.linear.x; - wz = object.kinematics.twist_with_covariance.twist.angular.z; - } + auto pose_cov = object.kinematics.pose_with_covariance.covariance; 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 = - autoware::universe_utils::deg2rad(1000); // in map coordinate [rad] - constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x; - constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y; - constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw; + const auto & p0_cov_x = object_model_.initial_covariance.pos_x; + const auto & p0_cov_y = object_model_.initial_covariance.pos_y; + const auto & p0_cov_yaw = object_model_.initial_covariance.yaw; const double cos_yaw = std::cos(yaw); const double sin_yaw = std::sin(yaw); @@ -133,14 +121,16 @@ PedestrianTracker::PedestrianTracker( pose_cov[XYZRPY_COV_IDX::YAW_YAW] = p0_cov_yaw; } - if (!object.kinematics.has_twist_covariance) { - constexpr double p0_stddev_vel = - autoware::universe_utils::kmph2mps(120); // in object coordinate [m/s] - constexpr double p0_stddev_wz = - autoware::universe_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 { + double vel = 0.0; + double wz = 0.0; + if (object.kinematics.has_twist) { + vel = object.kinematics.twist_with_covariance.twist.linear.x; + wz = object.kinematics.twist_with_covariance.twist.angular.z; + } + + double vel_cov = object_model_.initial_covariance.vel_long; + double wz_cov = object_model_.initial_covariance.yaw_rate; + if (object.kinematics.has_twist_covariance) { vel_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X]; wz_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::YAW_YAW]; } @@ -178,10 +168,10 @@ autoware_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObje 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); + const double sin_2yaw = std::sin(2.0 * pose_yaw); pose_cov[XYZRPY_COV_IDX::X_X] = 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::X_Y] = 0.5 * (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 diff --git a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp index a534002cd651d..130669f517914 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp @@ -296,11 +296,11 @@ bool CTRVMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) cons 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); + const double q_cov_x = motion_params_.q_cov_x * dt * dt; + const double q_cov_y = motion_params_.q_cov_y * dt * dt; + const double q_cov_yaw = motion_params_.q_cov_yaw * dt * dt; + const double q_cov_vel = motion_params_.q_cov_vel * dt * dt; + const double q_cov_wz = motion_params_.q_cov_wz * 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. From 2e86d0930971032cb51a76706976e975f5b53e5a Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 13 Jun 2024 08:23:00 +0000 Subject: [PATCH 20/30] style(pre-commit): autofix Signed-off-by: Taekjin LEE --- .../multi_object_tracker/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 +- 4 files changed, 4 insertions(+), 4 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 9968b9c3e6e01..dafd7db5dc9fc 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -178,7 +178,7 @@ autoware_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( const double sin_yaw = std::sin(pose_yaw); const double sin_2yaw = std::sin(2.0 * pose_yaw); pose_cov[XYZRPY_COV_IDX::X_X] = - r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (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 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 4765a12e6bdc0..6887b26f0f37b 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 @@ -218,7 +218,7 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje const double sin_yaw = std::sin(pose_yaw); const double sin_2yaw = std::sin(2.0 * pose_yaw); pose_cov[XYZRPY_COV_IDX::X_X] = - r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (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 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 247ad0f8c64c7..da46148165451 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 @@ -219,7 +219,7 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO const double sin_yaw = std::sin(pose_yaw); const double sin_2yaw = std::sin(2.0 * pose_yaw); pose_cov[XYZRPY_COV_IDX::X_X] = - r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (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 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 733d57dcf3d67..a32287d050b0b 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -170,7 +170,7 @@ autoware_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObje const double sin_yaw = std::sin(pose_yaw); const double sin_2yaw = std::sin(2.0 * pose_yaw); pose_cov[XYZRPY_COV_IDX::X_X] = - r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (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 From 9bdd911fa03ecf04778bb2093742fd810fe331f0 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 13 Jun 2024 17:47:49 +0900 Subject: [PATCH 21/30] chore: fix missing refactoring Signed-off-by: Taekjin LEE --- .../src/tracker/model/bicycle_tracker.cpp | 11 ++++++----- .../src/tracker/model/big_vehicle_tracker.cpp | 3 +-- .../src/tracker/model/normal_vehicle_tracker.cpp | 3 +-- .../src/tracker/model/pedestrian_tracker.cpp | 11 +++++++---- 4 files changed, 15 insertions(+), 13 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 dafd7db5dc9fc..9ae9258ca37c8 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -257,11 +257,12 @@ bool BicycleTracker::measureWithShape(const autoware_perception_msgs::msg::Detec bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; // 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); + 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); // 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 6887b26f0f37b..100c274e7da59 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 @@ -299,10 +299,9 @@ bool BigVehicleTracker::measureWithShape( return false; } + // update object size constexpr double gain = 0.5; constexpr double gain_inv = 1.0 - gain; - - // update object size 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; 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 da46148165451..a3bc44562cd55 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 @@ -300,10 +300,9 @@ bool NormalVehicleTracker::measureWithShape( return false; } + // update object size constexpr double gain = 0.5; constexpr double gain_inv = 1.0 - gain; - - // update object size 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; 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 a32287d050b0b..a6fb52d8a0173 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -228,22 +228,25 @@ bool PedestrianTracker::measureWithShape( if (!is_size_valid) { return false; } - // update bounding box size 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_perception_msgs::msg::Shape::CYLINDER) { // check cylinder size abnormality constexpr double size_max = 30.0; // [m] constexpr double size_min = 0.1; // [m] - if (object.shape.dimensions.x > size_max || object.shape.dimensions.z > size_max) { - return false; - } else if (object.shape.dimensions.x < size_min || object.shape.dimensions.z < size_min) { + bool is_size_valid = + (object.shape.dimensions.x <= size_max && object.shape.dimensions.z <= size_max && + object.shape.dimensions.x >= size_min && object.shape.dimensions.z >= size_min); + if (!is_size_valid) { return false; } + // update cylinder size cylinder_.width = gain_inv * cylinder_.width + gain * object.shape.dimensions.x; cylinder_.height = gain_inv * cylinder_.height + gain * object.shape.dimensions.z; + } else { // do not update polygon shape return false; From ad97751b06cf022dadb998df913ea6f581365444 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 13 Jun 2024 18:17:05 +0900 Subject: [PATCH 22/30] fix: align unit of angles, yaw rates Signed-off-by: Taekjin LEE --- .../motion_model/bicycle_motion_model.hpp | 2 + .../tracker/object_model/object_model.hpp | 26 +++++----- .../src/tracker/model/bicycle_tracker.cpp | 4 +- .../src/tracker/model/big_vehicle_tracker.cpp | 4 +- .../tracker/model/normal_vehicle_tracker.cpp | 4 +- .../motion_model/bicycle_motion_model.cpp | 51 ++++++++++--------- 6 files changed, 49 insertions(+), 42 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp index 472b8f2a309f3..f6ce2842388c6 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp @@ -47,6 +47,8 @@ class BicycleMotionModel : public MotionModel { double q_stddev_acc_long; double q_stddev_acc_lat; + double q_cov_acc_long; + double q_cov_acc_lat; double q_stddev_yaw_rate_min; double q_stddev_yaw_rate_max; double q_cov_slip_rate_min; 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 47ed52f4155e0..4892f72023170 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 @@ -99,8 +99,8 @@ struct BicycleModelState { double init_slip_angle_cov{0.0}; // [rad^2/s^2] initial slip angle covariance double slip_angle_max{0.0}; // [rad] max slip angle - double slip_rate_cov_min{0.0}; // [rad/s] uncertain slip angle change rate, minimum - double slip_rate_cov_max{0.0}; // [rad/s] uncertain slip angle change rate, maximum + double slip_rate_stddev_min{0.0}; // [rad/s] uncertain slip angle change rate, minimum + double slip_rate_stddev_max{0.0}; // [rad/s] uncertain slip angle change rate, maximum double wheel_pos_ratio_front{0.0}; // [-] double wheel_pos_ratio_rear{0.0}; // [-] double wheel_pos_front_min{0.0}; // [m] @@ -156,9 +156,9 @@ class ObjectModel // bicycle motion model bicycle_state.init_slip_angle_cov = sq(deg2rad(5.0)); - bicycle_state.slip_angle_max = 30.0; // deg2rad(30.0); - bicycle_state.slip_rate_cov_min = 0.3; // sq(deg2rad(0.3)); - bicycle_state.slip_rate_cov_max = 10.0; // sq(deg2rad(10.0)); + bicycle_state.slip_angle_max = deg2rad(30.0); + bicycle_state.slip_rate_stddev_min = deg2rad(0.3); + bicycle_state.slip_rate_stddev_max = deg2rad(10.0); bicycle_state.wheel_pos_ratio_front = 0.3; bicycle_state.wheel_pos_ratio_rear = 0.25; bicycle_state.wheel_pos_front_min = 1.0; @@ -178,8 +178,8 @@ class ObjectModel 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.yaw_rate_min = deg2rad(1.5); + process_noise.yaw_rate_max = deg2rad(15.0); process_limit.acc_long_max = const_g; process_limit.acc_lat_max = const_g; @@ -200,9 +200,9 @@ class ObjectModel // bicycle motion model bicycle_state.init_slip_angle_cov = sq(deg2rad(5.0)); - bicycle_state.slip_angle_max = 30.0; // deg2rad(30.0); - bicycle_state.slip_rate_cov_min = 0.3; // sq(deg2rad(0.3)); - bicycle_state.slip_rate_cov_max = 10.0; // sq(deg2rad(10.0)); + bicycle_state.slip_angle_max = deg2rad(30.0); + bicycle_state.slip_rate_stddev_min = deg2rad(0.3); + bicycle_state.slip_rate_stddev_max = deg2rad(10.0); bicycle_state.wheel_pos_ratio_front = 0.3; bicycle_state.wheel_pos_ratio_rear = 0.25; bicycle_state.wheel_pos_front_min = 1.5; @@ -244,9 +244,9 @@ class ObjectModel // bicycle motion model bicycle_state.init_slip_angle_cov = sq(deg2rad(5.0)); - bicycle_state.slip_angle_max = 30.0; // deg2rad(30.0); - bicycle_state.slip_rate_cov_min = 1.0; // sq(deg2rad(1.0)); - bicycle_state.slip_rate_cov_max = 10.0; // sq(deg2rad(10.0)); + bicycle_state.slip_angle_max = deg2rad(30.0); + bicycle_state.slip_rate_stddev_min = deg2rad(1.0); + bicycle_state.slip_rate_stddev_max = deg2rad(10.0); bicycle_state.wheel_pos_ratio_front = 0.3; bicycle_state.wheel_pos_ratio_rear = 0.3; bicycle_state.wheel_pos_front_min = 0.3; 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 9ae9258ca37c8..255361097666f 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -78,8 +78,8 @@ BicycleTracker::BicycleTracker( const double q_stddev_acc_lat = object_model_.process_noise.acc_lat; const double q_stddev_yaw_rate_min = object_model_.process_noise.yaw_rate_min; const double q_stddev_yaw_rate_max = object_model_.process_noise.yaw_rate_max; - const double q_stddev_slip_rate_min = object_model_.bicycle_state.slip_rate_cov_min; - const double q_stddev_slip_rate_max = object_model_.bicycle_state.slip_rate_cov_max; + const double q_stddev_slip_rate_min = object_model_.bicycle_state.slip_rate_stddev_min; + const double q_stddev_slip_rate_max = object_model_.bicycle_state.slip_rate_stddev_max; const double q_max_slip_angle = object_model_.bicycle_state.slip_angle_max; const double lf_ratio = object_model_.bicycle_state.wheel_pos_ratio_front; const double lf_min = object_model_.bicycle_state.wheel_pos_front_min; 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 100c274e7da59..06a8fee8c7cb2 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 @@ -94,8 +94,8 @@ BigVehicleTracker::BigVehicleTracker( const double q_stddev_acc_lat = object_model_.process_noise.acc_lat; const double q_stddev_yaw_rate_min = object_model_.process_noise.yaw_rate_min; const double q_stddev_yaw_rate_max = object_model_.process_noise.yaw_rate_max; - const double q_stddev_slip_rate_min = object_model_.bicycle_state.slip_rate_cov_min; - const double q_stddev_slip_rate_max = object_model_.bicycle_state.slip_rate_cov_max; + const double q_stddev_slip_rate_min = object_model_.bicycle_state.slip_rate_stddev_min; + const double q_stddev_slip_rate_max = object_model_.bicycle_state.slip_rate_stddev_max; const double q_max_slip_angle = object_model_.bicycle_state.slip_angle_max; const double lf_ratio = object_model_.bicycle_state.wheel_pos_ratio_front; const double lf_min = object_model_.bicycle_state.wheel_pos_front_min; 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 a3bc44562cd55..518f0dfa8ebeb 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 @@ -95,8 +95,8 @@ NormalVehicleTracker::NormalVehicleTracker( const double q_stddev_acc_lat = object_model_.process_noise.acc_lat; const double q_stddev_yaw_rate_min = object_model_.process_noise.yaw_rate_min; const double q_stddev_yaw_rate_max = object_model_.process_noise.yaw_rate_max; - const double q_stddev_slip_rate_min = object_model_.bicycle_state.slip_rate_cov_min; - const double q_stddev_slip_rate_max = object_model_.bicycle_state.slip_rate_cov_max; + const double q_stddev_slip_rate_min = object_model_.bicycle_state.slip_rate_stddev_min; + const double q_stddev_slip_rate_max = object_model_.bicycle_state.slip_rate_stddev_max; const double q_max_slip_angle = object_model_.bicycle_state.slip_angle_max; const double lf_ratio = object_model_.bicycle_state.wheel_pos_ratio_front; const double lf_min = object_model_.bicycle_state.wheel_pos_front_min; diff --git a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp index 7793e77de09ca..aae4ed6086944 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp @@ -44,11 +44,15 @@ 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 + constexpr double q_stddev_yaw_rate_min = + tier4_autoware_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate + constexpr double q_stddev_yaw_rate_max = + tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate + constexpr double q_stddev_slip_rate_min = + tier4_autoware_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate + constexpr double q_stddev_slip_rate_max = + tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate + constexpr double q_max_slip_angle = tier4_autoware_utils::deg2rad(30.0); // [rad] 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 @@ -79,13 +83,13 @@ void BicycleMotionModel::setMotionParams( // 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 = autoware::universe_utils::deg2rad(q_stddev_yaw_rate_min); - motion_params_.q_stddev_yaw_rate_max = autoware::universe_utils::deg2rad(q_stddev_yaw_rate_max); - motion_params_.q_cov_slip_rate_min = - std::pow(autoware::universe_utils::deg2rad(q_stddev_slip_rate_min), 2.0); - motion_params_.q_cov_slip_rate_max = - std::pow(autoware::universe_utils::deg2rad(q_stddev_slip_rate_max), 2.0); - motion_params_.q_max_slip_angle = autoware::universe_utils::deg2rad(q_max_slip_angle); + motion_params_.q_cov_acc_long = q_stddev_acc_long * q_stddev_acc_long; + motion_params_.q_cov_acc_lat = q_stddev_acc_lat * q_stddev_acc_lat; + motion_params_.q_stddev_yaw_rate_min = q_stddev_yaw_rate_min; + motion_params_.q_stddev_yaw_rate_max = q_stddev_yaw_rate_max; + motion_params_.q_cov_slip_rate_min = q_stddev_slip_rate_min * q_stddev_slip_rate_min; + motion_params_.q_cov_slip_rate_max = q_stddev_slip_rate_max * q_stddev_slip_rate_max; + motion_params_.q_max_slip_angle = q_max_slip_angle; constexpr double minimum_wheel_pos = 0.01; // minimum of 0.01m if (lf_min < minimum_wheel_pos || lr_min < minimum_wheel_pos) { @@ -354,10 +358,8 @@ bool BicycleMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) c 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 { + double q_stddev_yaw_rate = motion_params_.q_stddev_yaw_rate_min; + if (vel > 0.01) { /* 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 @@ -365,8 +367,9 @@ bool BicycleMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) c 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); + q_stddev_yaw_rate = std::clamp( + q_stddev_yaw_rate, motion_params_.q_stddev_yaw_rate_min, + motion_params_.q_stddev_yaw_rate_max); } double q_cov_slip_rate{0.0}; if (vel <= 0.01) { @@ -384,11 +387,13 @@ bool BicycleMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) c 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; + const double dt2 = dt * dt; + const double dt4 = dt2 * dt2; + const double q_cov_x = 0.5 * motion_params_.q_cov_acc_long * dt4; + const double q_cov_y = 0.5 * motion_params_.q_cov_acc_lat * dt4; + const double q_cov_yaw = q_stddev_yaw_rate * q_stddev_yaw_rate * dt2; + const double q_cov_vel = motion_params_.q_cov_acc_long * dt2; + const double q_cov_slip = q_cov_slip_rate * dt2; Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(DIM, DIM); // Rotate the covariance matrix according to the vehicle yaw From 724dc8e7d7b99e3806db9d577e522a0ba1f7c98f Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 18 Jun 2024 15:43:41 +0900 Subject: [PATCH 23/30] fix: rename tier4_autoware_utils to autoware_universe_utils Signed-off-by: Taekjin LEE --- .../debugger/debug_object.hpp | 2 +- .../multi_object_tracker/debugger/debugger.hpp | 4 ++-- .../src/tracker/model/bicycle_tracker.cpp | 12 ++++++------ .../src/tracker/model/big_vehicle_tracker.cpp | 12 ++++++------ .../tracker/model/normal_vehicle_tracker.cpp | 12 ++++++------ .../src/tracker/model/pass_through_tracker.cpp | 4 ++-- .../src/tracker/model/pedestrian_tracker.cpp | 12 ++++++------ .../src/tracker/model/unknown_tracker.cpp | 12 ++++++------ .../motion_model/bicycle_motion_model.cpp | 18 +++++++++--------- .../tracker/motion_model/ctrv_motion_model.cpp | 8 ++++---- .../tracker/motion_model/cv_motion_model.cpp | 8 ++++---- 11 files changed, 52 insertions(+), 52 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp index 1a00e4902b57b..5af7d083ef3f5 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp @@ -16,7 +16,7 @@ #define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ #include "multi_object_tracker/tracker/model/tracker_base.hpp" -#include "tier4_autoware_utils/ros/uuid_helper.hpp" +#include "autoware/universe_utils/ros/uuid_helper.hpp" #include #include diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp index 6bf2e581fcdf1..e44d0996d19a4 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp @@ -16,8 +16,8 @@ #define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ #include "multi_object_tracker/debugger/debug_object.hpp" -#include "tier4_autoware_utils/ros/debug_publisher.hpp" -#include "tier4_autoware_utils/ros/published_time_publisher.hpp" +#include "autoware/universe_utils/ros/debug_publisher.hpp" +#include "autoware/universe_utils/ros/published_time_publisher.hpp" #include #include 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 255361097666f..14f881cf9f591 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -21,10 +21,10 @@ #include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include "tier4_autoware_utils/math/normalization.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" -#include "tier4_autoware_utils/ros/msg_covariance.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/math/normalization.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" #include #include @@ -100,7 +100,7 @@ BicycleTracker::BicycleTracker( // Set initial state { - using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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); @@ -172,7 +172,7 @@ autoware_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; // fill covariance matrix - using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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); 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 06a8fee8c7cb2..9986fcd6beced 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 @@ -21,10 +21,10 @@ #include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include "tier4_autoware_utils/math/normalization.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" -#include "tier4_autoware_utils/ros/msg_covariance.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/math/normalization.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" #include #include @@ -116,7 +116,7 @@ BigVehicleTracker::BigVehicleTracker( // Set initial state { - using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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); @@ -212,7 +212,7 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; // fill covariance matrix - using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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); 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 518f0dfa8ebeb..cf96148adb6b8 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 @@ -21,10 +21,10 @@ #include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include "tier4_autoware_utils/math/normalization.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" -#include "tier4_autoware_utils/ros/msg_covariance.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/math/normalization.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" #include #include @@ -117,7 +117,7 @@ NormalVehicleTracker::NormalVehicleTracker( // Set initial state { - using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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); @@ -213,7 +213,7 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; // fill covariance matrix - using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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); diff --git a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp index 13821bf9b7318..1c6ddce6a479b 100644 --- a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp @@ -20,7 +20,7 @@ #include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include "tier4_autoware_utils/ros/msg_covariance.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" #include #include @@ -87,7 +87,7 @@ bool PassThroughTracker::measure( bool PassThroughTracker::getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const { - using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; object = object_recognition_utils::toTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); 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 a6fb52d8a0173..ed3998d5d4e49 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -21,10 +21,10 @@ #include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include "tier4_autoware_utils/math/normalization.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" -#include "tier4_autoware_utils/ros/msg_covariance.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/math/normalization.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" #include #include @@ -99,7 +99,7 @@ PedestrianTracker::PedestrianTracker( // Set initial state { - using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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); @@ -164,7 +164,7 @@ autoware_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObje autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; // fill covariance matrix - using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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); 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 ae2cf3fff531d..cfed66cb95892 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -16,10 +16,10 @@ #include "multi_object_tracker/tracker/model/unknown_tracker.hpp" #include "multi_object_tracker/utils/utils.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include "tier4_autoware_utils/math/normalization.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" -#include "tier4_autoware_utils/ros/msg_covariance.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/math/normalization.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" #include #include @@ -73,7 +73,7 @@ UnknownTracker::UnknownTracker( // Set initial state { - using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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; @@ -148,7 +148,7 @@ autoware_perception_msgs::msg::DetectedObject UnknownTracker::getUpdatingObject( // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { // fill covariance matrix - using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using autoware_universe_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; diff --git a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp index aae4ed6086944..b07454947aa0f 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp @@ -21,9 +21,9 @@ #include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/utils/utils.hpp" -#include "tier4_autoware_utils/math/normalization.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" -#include "tier4_autoware_utils/ros/msg_covariance.hpp" +#include "autoware/universe_utils/math/normalization.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" #include #include @@ -31,7 +31,7 @@ // cspell: ignore CTRV // Bicycle CTRV motion model // CTRV : Constant Turn Rate and constant Velocity -using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; +using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; BicycleMotionModel::BicycleMotionModel() : logger_(rclcpp::get_logger("BicycleMotionModel")) { @@ -45,14 +45,14 @@ void BicycleMotionModel::setDefaultParams() 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 = - tier4_autoware_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate + autoware_universe_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate constexpr double q_stddev_yaw_rate_max = - tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate + autoware_universe_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate constexpr double q_stddev_slip_rate_min = - tier4_autoware_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate + autoware_universe_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate constexpr double q_stddev_slip_rate_max = - tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate - constexpr double q_max_slip_angle = tier4_autoware_utils::deg2rad(30.0); // [rad] max slip angle + autoware_universe_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate + constexpr double q_max_slip_angle = autoware_universe_utils::deg2rad(30.0); // [rad] 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 diff --git a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp index 130669f517914..cc9965841d3f6 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp @@ -21,16 +21,16 @@ #include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/utils/utils.hpp" -#include "tier4_autoware_utils/math/normalization.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" -#include "tier4_autoware_utils/ros/msg_covariance.hpp" +#include "autoware/universe_utils/math/normalization.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" #include #include // cspell: ignore CTRV // Constant Turn Rate and constant Velocity (CTRV) motion model -using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; +using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; CTRVMotionModel::CTRVMotionModel() : logger_(rclcpp::get_logger("CTRVMotionModel")) { diff --git a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp index cf22e3256dca9..f59090f6ab8ae 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp @@ -21,9 +21,9 @@ #include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/utils/utils.hpp" -#include "tier4_autoware_utils/math/normalization.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" -#include "tier4_autoware_utils/ros/msg_covariance.hpp" +#include "autoware/universe_utils/math/normalization.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" #include #include @@ -32,7 +32,7 @@ // cspell: ignore CV // Constant Velocity (CV) motion model -using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; +using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; CVMotionModel::CVMotionModel() : logger_(rclcpp::get_logger("CVMotionModel")) { From b63596d2f98883ec216f317a26d3c3e644df8d67 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 18 Jun 2024 15:49:19 +0900 Subject: [PATCH 24/30] fix: missing unit conversion Signed-off-by: Taekjin LEE --- .../tracker/object_model/object_model.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 4892f72023170..6a771344bb36b 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 @@ -134,8 +134,8 @@ class ObjectModel 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.yaw_rate_min = deg2rad(1.5); + process_noise.yaw_rate_max = deg2rad(15.0); process_limit.acc_long_max = const_g; process_limit.acc_lat_max = const_g; @@ -222,8 +222,8 @@ class ObjectModel process_noise.acc_long = const_g * 0.35; process_noise.acc_lat = const_g * 0.15; - process_noise.yaw_rate_min = 5.0; // deg2rad(5.0); - process_noise.yaw_rate_max = 15.0; // deg2rad(15.0); + process_noise.yaw_rate_min = deg2rad(5.0); + process_noise.yaw_rate_max = deg2rad(15.0); process_limit.acc_long_max = const_g; process_limit.acc_lat_max = const_g; From 8cbb2f3b1ed69c6c0f56e540af69ea2c9076602c Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 18 Jun 2024 06:52:10 +0000 Subject: [PATCH 25/30] style(pre-commit): autofix Signed-off-by: Taekjin LEE --- .../include/multi_object_tracker/debugger/debug_object.hpp | 2 +- .../include/multi_object_tracker/debugger/debugger.hpp | 2 +- .../src/tracker/model/bicycle_tracker.cpp | 4 ++-- .../src/tracker/model/big_vehicle_tracker.cpp | 4 ++-- .../src/tracker/model/normal_vehicle_tracker.cpp | 4 ++-- .../src/tracker/model/pass_through_tracker.cpp | 2 +- .../src/tracker/model/pedestrian_tracker.cpp | 4 ++-- .../src/tracker/model/unknown_tracker.cpp | 2 +- .../src/tracker/motion_model/bicycle_motion_model.cpp | 7 ++++--- .../src/tracker/motion_model/ctrv_motion_model.cpp | 4 ++-- .../src/tracker/motion_model/cv_motion_model.cpp | 4 ++-- 11 files changed, 20 insertions(+), 19 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp index 5af7d083ef3f5..38b45a902c7c5 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp @@ -15,8 +15,8 @@ #ifndef MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ #define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ -#include "multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/universe_utils/ros/uuid_helper.hpp" +#include "multi_object_tracker/tracker/model/tracker_base.hpp" #include #include diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp index e44d0996d19a4..f24e3207c2b79 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp @@ -15,9 +15,9 @@ #ifndef MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ #define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ -#include "multi_object_tracker/debugger/debug_object.hpp" #include "autoware/universe_utils/ros/debug_publisher.hpp" #include "autoware/universe_utils/ros/published_time_publisher.hpp" +#include "multi_object_tracker/debugger/debug_object.hpp" #include #include 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 14f881cf9f591..1ab1fc74fe17a 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -19,12 +19,12 @@ #include "multi_object_tracker/tracker/model/bicycle_tracker.hpp" -#include "multi_object_tracker/utils/utils.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" +#include "multi_object_tracker/utils/utils.hpp" +#include "object_recognition_utils/object_recognition_utils.hpp" #include #include 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 9986fcd6beced..c5d78c11e5b01 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 @@ -19,12 +19,12 @@ #include "multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" -#include "multi_object_tracker/utils/utils.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" +#include "multi_object_tracker/utils/utils.hpp" +#include "object_recognition_utils/object_recognition_utils.hpp" #include #include 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 cf96148adb6b8..174b7f0ad5462 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 @@ -19,12 +19,12 @@ #include "multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" -#include "multi_object_tracker/utils/utils.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" +#include "multi_object_tracker/utils/utils.hpp" +#include "object_recognition_utils/object_recognition_utils.hpp" #include #include diff --git a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp index 1c6ddce6a479b..60fe5158e9096 100644 --- a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp @@ -18,9 +18,9 @@ #define EIGEN_MPL2_ONLY #include "multi_object_tracker/tracker/model/pass_through_tracker.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" #include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" #include #include 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 ed3998d5d4e49..2abefb1c82968 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -19,12 +19,12 @@ #include "multi_object_tracker/tracker/model/pedestrian_tracker.hpp" -#include "multi_object_tracker/utils/utils.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" +#include "multi_object_tracker/utils/utils.hpp" +#include "object_recognition_utils/object_recognition_utils.hpp" #include #include 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 cfed66cb95892..b79bfd1130f15 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -15,11 +15,11 @@ #include "multi_object_tracker/tracker/model/unknown_tracker.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" +#include "multi_object_tracker/utils/utils.hpp" #include #include diff --git a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp index b07454947aa0f..1f8011110c2db 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp @@ -19,11 +19,11 @@ #include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" +#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" +#include "multi_object_tracker/utils/utils.hpp" #include #include @@ -52,7 +52,8 @@ void BicycleMotionModel::setDefaultParams() autoware_universe_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate constexpr double q_stddev_slip_rate_max = autoware_universe_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate - constexpr double q_max_slip_angle = autoware_universe_utils::deg2rad(30.0); // [rad] max slip angle + constexpr double q_max_slip_angle = + autoware_universe_utils::deg2rad(30.0); // [rad] 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 diff --git a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp index cc9965841d3f6..a0484fcc4f4d5 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp @@ -19,11 +19,11 @@ #include "multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" +#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" +#include "multi_object_tracker/utils/utils.hpp" #include #include diff --git a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp index f59090f6ab8ae..4d5909e4c21e4 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp @@ -19,11 +19,11 @@ #include "multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" +#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" +#include "multi_object_tracker/utils/utils.hpp" #include #include From 0f62e7538da398b8ec688b0a2ccfe111b8ae2e0b Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 18 Jun 2024 16:01:34 +0900 Subject: [PATCH 26/30] fix: sources Signed-off-by: Taekjin LEE --- .../data_association/data_association.hpp | 2 +- .../include/multi_object_tracker/debugger/debugger.hpp | 6 ++---- .../multi_object_tracker/multi_object_tracker_core.hpp | 4 ++-- .../multi_object_tracker/processor/input_manager.hpp | 2 +- .../include/multi_object_tracker/processor/processor.hpp | 4 ++-- .../multi_object_tracker/tracker/model/tracker_base.hpp | 4 ++-- .../include/multi_object_tracker/utils/utils.hpp | 6 +++--- 7 files changed, 13 insertions(+), 15 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp b/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp index 27fdb7cf420c3..c30423db02c26 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp @@ -27,7 +27,7 @@ #include #include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" #include #include diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp index f24e3207c2b79..a1c516147b220 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp @@ -19,14 +19,12 @@ #include "autoware/universe_utils/ros/published_time_publisher.hpp" #include "multi_object_tracker/debugger/debug_object.hpp" -#include -#include #include #include #include -#include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" +#include "autoware_perception_msgs/msg/tracked_objects.hpp" #include #include diff --git a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp index 4c1ccced1bcf7..aff3cbd00eabe 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp @@ -27,8 +27,8 @@ #include -#include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" +#include "autoware_perception_msgs/msg/tracked_objects.hpp" #include #include diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp index a7ff266a17018..29b0c18ae766f 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp @@ -17,7 +17,7 @@ #include "rclcpp/rclcpp.hpp" -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" #include #include diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp index b2f5f41c81f0a..6d0dbcd036e53 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp @@ -19,8 +19,8 @@ #include -#include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" +#include "autoware_perception_msgs/msg/tracked_objects.hpp" #include #include diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp index 3db1c3869769d..44b3884c392e6 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp @@ -26,8 +26,8 @@ #include #include -#include -#include +#include "autoware_perception_msgs/msg/detected_object.hpp" +#include "autoware_perception_msgs/msg/tracked_object.hpp" #include #include 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 a30980e12e987..0bda7870ae2b9 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 @@ -22,9 +22,9 @@ #include #include -#include -#include -#include +#include "autoware_perception_msgs/msg/detected_object.hpp" +#include "autoware_perception_msgs/msg/shape.hpp" +#include "autoware_perception_msgs/msg/tracked_object.hpp" #include #include #include From b139ffd7d89c9ca54a986cd121cc865b9fb5fe18 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 19 Jun 2024 09:19:09 +0900 Subject: [PATCH 27/30] chore: Update include and import statements Signed-off-by: Taekjin LEE --- .../include/multi_object_tracker/debugger/debug_object.hpp | 5 ++--- .../multi_object_tracker/src/debugger/debug_object.cpp | 2 +- perception/multi_object_tracker/src/processor/processor.cpp | 2 +- 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp index 38b45a902c7c5..08c9eb975d8e6 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp @@ -18,11 +18,10 @@ #include "autoware/universe_utils/ros/uuid_helper.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" -#include #include -#include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" +#include "autoware_perception_msgs/msg/tracked_objects.hpp" #include #include #include diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp index 811330bc5b3df..fe6d3d5c75564 100644 --- a/perception/multi_object_tracker/src/debugger/debug_object.cpp +++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp @@ -14,7 +14,7 @@ #include "multi_object_tracker/debugger/debug_object.hpp" -#include +#include "autoware_perception_msgs/msg/tracked_object.hpp" #include diff --git a/perception/multi_object_tracker/src/processor/processor.cpp b/perception/multi_object_tracker/src/processor/processor.cpp index ff14f7849b161..50d1a021c5838 100644 --- a/perception/multi_object_tracker/src/processor/processor.cpp +++ b/perception/multi_object_tracker/src/processor/processor.cpp @@ -17,7 +17,7 @@ #include "multi_object_tracker/tracker/tracker.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include +#include "autoware_perception_msgs/msg/tracked_objects.hpp" #include From 0e84b4d169d8c05063f08f6d8be9c1e7060e2d5b Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 20 Jun 2024 11:29:30 +0900 Subject: [PATCH 28/30] fix: unit convert bug Signed-off-by: Taekjin LEE --- .../src/tracker/motion_model/bicycle_motion_model.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp index 1f8011110c2db..e927ad1bfae28 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp @@ -108,7 +108,7 @@ void BicycleMotionModel::setMotionLimits(const double & max_vel, const double & { // set motion limitations motion_params_.max_vel = max_vel; - motion_params_.max_slip = autoware::universe_utils::deg2rad(max_slip); + motion_params_.max_slip = max_slip; } bool BicycleMotionModel::initialize( From 10fdb99f69bb30ace19d18dbeb75f1f6f2cb1475 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 20 Jun 2024 11:35:28 +0900 Subject: [PATCH 29/30] fix: update to autoware::universe_utils Signed-off-by: Taekjin LEE --- .../src/tracker/model/bicycle_tracker.cpp | 4 ++-- .../src/tracker/model/big_vehicle_tracker.cpp | 4 ++-- .../src/tracker/model/normal_vehicle_tracker.cpp | 4 ++-- .../src/tracker/model/pass_through_tracker.cpp | 2 +- .../src/tracker/model/pedestrian_tracker.cpp | 4 ++-- .../src/tracker/model/unknown_tracker.cpp | 4 ++-- .../tracker/motion_model/bicycle_motion_model.cpp | 12 ++++++------ .../src/tracker/motion_model/ctrv_motion_model.cpp | 2 +- .../src/tracker/motion_model/cv_motion_model.cpp | 2 +- 9 files changed, 19 insertions(+), 19 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 1ab1fc74fe17a..a438c830596d7 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -100,7 +100,7 @@ BicycleTracker::BicycleTracker( // Set initial state { - using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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); @@ -172,7 +172,7 @@ autoware_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; // fill covariance matrix - using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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); 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 c5d78c11e5b01..90d33e65b46bd 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 @@ -116,7 +116,7 @@ BigVehicleTracker::BigVehicleTracker( // Set initial state { - using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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); @@ -212,7 +212,7 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; // fill covariance matrix - using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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); 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 174b7f0ad5462..67282893083c1 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 @@ -117,7 +117,7 @@ NormalVehicleTracker::NormalVehicleTracker( // Set initial state { - using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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); @@ -213,7 +213,7 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; // fill covariance matrix - using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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); diff --git a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp index 60fe5158e9096..17be415480360 100644 --- a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp @@ -87,7 +87,7 @@ bool PassThroughTracker::measure( bool PassThroughTracker::getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const { - using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; object = object_recognition_utils::toTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); 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 2abefb1c82968..d9412c3b5739c 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -99,7 +99,7 @@ PedestrianTracker::PedestrianTracker( // Set initial state { - using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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); @@ -164,7 +164,7 @@ autoware_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObje autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; // fill covariance matrix - using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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); 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 b79bfd1130f15..6905d7c3b8ced 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -73,7 +73,7 @@ UnknownTracker::UnknownTracker( // Set initial state { - using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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; @@ -148,7 +148,7 @@ autoware_perception_msgs::msg::DetectedObject UnknownTracker::getUpdatingObject( // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { // fill covariance matrix - using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using autoware::universe_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; diff --git a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp index e927ad1bfae28..e3c3231c2ed70 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp @@ -31,7 +31,7 @@ // cspell: ignore CTRV // Bicycle CTRV motion model // CTRV : Constant Turn Rate and constant Velocity -using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; +using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; BicycleMotionModel::BicycleMotionModel() : logger_(rclcpp::get_logger("BicycleMotionModel")) { @@ -45,15 +45,15 @@ void BicycleMotionModel::setDefaultParams() 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 = - autoware_universe_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate + autoware::universe_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate constexpr double q_stddev_yaw_rate_max = - autoware_universe_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate + autoware::universe_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate constexpr double q_stddev_slip_rate_min = - autoware_universe_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate + autoware::universe_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate constexpr double q_stddev_slip_rate_max = - autoware_universe_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate + autoware::universe_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate constexpr double q_max_slip_angle = - autoware_universe_utils::deg2rad(30.0); // [rad] max slip angle + autoware::universe_utils::deg2rad(30.0); // [rad] 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 diff --git a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp index a0484fcc4f4d5..807c24f2e962a 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp @@ -30,7 +30,7 @@ // cspell: ignore CTRV // Constant Turn Rate and constant Velocity (CTRV) motion model -using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; +using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; CTRVMotionModel::CTRVMotionModel() : logger_(rclcpp::get_logger("CTRVMotionModel")) { diff --git a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp index 4d5909e4c21e4..ebd59735cc8f1 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp @@ -32,7 +32,7 @@ // cspell: ignore CV // Constant Velocity (CV) motion model -using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; +using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; CVMotionModel::CVMotionModel() : logger_(rclcpp::get_logger("CVMotionModel")) { From 3f20c9882354fcb415bdbf79d6e3b87c18ed66b1 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 20 Jun 2024 11:54:45 +0900 Subject: [PATCH 30/30] fix: mis-implementation of process noise Signed-off-by: Taekjin LEE --- .../motion_model/bicycle_motion_model.cpp | 4 ++-- .../motion_model/ctrv_motion_model.cpp | 21 ++++++++++--------- .../tracker/motion_model/cv_motion_model.cpp | 8 +++---- 3 files changed, 17 insertions(+), 16 deletions(-) diff --git a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp index e3c3231c2ed70..466108a2bef0e 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp @@ -390,8 +390,8 @@ bool BicycleMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) c } const double dt2 = dt * dt; const double dt4 = dt2 * dt2; - const double q_cov_x = 0.5 * motion_params_.q_cov_acc_long * dt4; - const double q_cov_y = 0.5 * motion_params_.q_cov_acc_lat * dt4; + const double q_cov_x = 0.25 * motion_params_.q_cov_acc_long * dt4; + const double q_cov_y = 0.25 * motion_params_.q_cov_acc_lat * dt4; const double q_cov_yaw = q_stddev_yaw_rate * q_stddev_yaw_rate * dt2; const double q_cov_vel = motion_params_.q_cov_acc_long * dt2; const double q_cov_slip = q_cov_slip_rate * dt2; diff --git a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp index 807c24f2e962a..b19af1d26d162 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp @@ -64,11 +64,11 @@ void CTRVMotionModel::setMotionParams( 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_vel = std::pow(q_stddev_vel, 2.0); - motion_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0); + motion_params_.q_cov_x = q_stddev_x * q_stddev_x; + motion_params_.q_cov_y = q_stddev_y * q_stddev_y; + motion_params_.q_cov_yaw = q_stddev_yaw * q_stddev_yaw; + motion_params_.q_cov_vel = q_stddev_vel * q_stddev_vel; + motion_params_.q_cov_wz = q_stddev_wz * q_stddev_wz; } void CTRVMotionModel::setMotionLimits(const double & max_vel, const double & max_wz) @@ -296,11 +296,12 @@ bool CTRVMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) cons A(IDX::YAW, IDX::WZ) = dt; // Process noise covariance Q - const double q_cov_x = motion_params_.q_cov_x * dt * dt; - const double q_cov_y = motion_params_.q_cov_y * dt * dt; - const double q_cov_yaw = motion_params_.q_cov_yaw * dt * dt; - const double q_cov_vel = motion_params_.q_cov_vel * dt * dt; - const double q_cov_wz = motion_params_.q_cov_wz * dt * dt; + const double dt2 = dt * dt; + const double q_cov_x = motion_params_.q_cov_x * dt2; + const double q_cov_y = motion_params_.q_cov_y * dt2; + const double q_cov_yaw = motion_params_.q_cov_yaw * dt2; + const double q_cov_vel = motion_params_.q_cov_vel * dt2; + const double q_cov_wz = motion_params_.q_cov_wz * dt2; 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. diff --git a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp index ebd59735cc8f1..231ba73796ed9 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp @@ -64,10 +64,10 @@ void CVMotionModel::setMotionParams( 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); + motion_params_.q_cov_x = q_stddev_x * q_stddev_x; + motion_params_.q_cov_y = q_stddev_y * q_stddev_y; + motion_params_.q_cov_vx = q_stddev_vx * q_stddev_vx; + motion_params_.q_cov_vy = q_stddev_vy * q_stddev_vy; } void CVMotionModel::setMotionLimits(const double & max_vx, const double & max_vy)