From 17db3fc8a420878b85b6290d71395b3f583e7ed2 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 13 Mar 2024 11:50:06 +0900 Subject: [PATCH 1/3] fix: update object position offset caused by size change Signed-off-by: Taekjin LEE --- .../tracker/model/big_vehicle_tracker.hpp | 11 ------ .../tracker/model/normal_vehicle_tracker.hpp | 11 ------ .../src/tracker/model/big_vehicle_tracker.cpp | 37 ++++++++----------- .../tracker/model/normal_vehicle_tracker.cpp | 37 ++++++++----------- 4 files changed, 30 insertions(+), 66 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index f284196e1c816..233d6528dca9f 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 @@ -51,7 +51,6 @@ class BigVehicleTracker : public Tracker BoundingBox bounding_box_; BoundingBox last_input_bounding_box_; Eigen::Vector2d tracking_offset_; - int last_nearest_corner_index_; private: BicycleMotionModel motion_model_; @@ -77,16 +76,6 @@ class BigVehicleTracker : public Tracker const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const override; virtual ~BigVehicleTracker() {} - -private: - void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform) - { - Eigen::MatrixXd X_t(DIM, 1); - motion_model_.getStateVector(X_t); - last_nearest_corner_index_ = utils::getNearestCornerOrSurface( - X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, - self_transform); - } }; #endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 4c9a5a113a340..0d0b5d653cecf 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 @@ -51,7 +51,6 @@ class NormalVehicleTracker : public Tracker BoundingBox bounding_box_; BoundingBox last_input_bounding_box_; Eigen::Vector2d tracking_offset_; - int last_nearest_corner_index_; private: BicycleMotionModel motion_model_; @@ -77,16 +76,6 @@ class NormalVehicleTracker : public Tracker const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const override; virtual ~NormalVehicleTracker() {} - -private: - void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform) - { - Eigen::MatrixXd X_t(DIM, 1); - motion_model_.getStateVector(X_t); - last_nearest_corner_index_ = utils::getNearestCornerOrSurface( - X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, - self_transform); - } }; #endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 5b7a52016967d..c8866a6f8ca25 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 @@ -43,8 +43,8 @@ using Label = autoware_perception_msgs::msg::ObjectClassification; BigVehicleTracker::BigVehicleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, const uint & channel_index) : Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("BigVehicleTracker")), @@ -178,9 +178,6 @@ BigVehicleTracker::BigVehicleTracker( // initialize motion model motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); } - - /* calc nearest corner index*/ - setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step } bool BigVehicleTracker::predict(const rclcpp::Time & time) @@ -352,11 +349,19 @@ bool BigVehicleTracker::measureWithShape( // update motion model motion_model_.updateExtendedState(bounding_box_.length); - // // update offset into object position - // motion_model_.adjustPosition(gain * tracking_offset_.x(), gain * tracking_offset_.y()); - // // update offset - // tracking_offset_.x() = gain_inv * tracking_offset_.x(); - // tracking_offset_.y() = gain_inv * tracking_offset_.y(); + // update offset into object position + { + // rotate back the offset vector from object coordinate to global coordinate + const double yaw = motion_model_.getStateElement(IDX::YAW); + const double offset_x_global = + tracking_offset_.x() * std::cos(yaw) - tracking_offset_.y() * std::sin(yaw); + const double offset_y_global = + tracking_offset_.x() * std::sin(yaw) + tracking_offset_.y() * std::cos(yaw); + motion_model_.adjustPosition(-gain * offset_x_global, -gain * offset_y_global); + // update offset (object coordinate) + tracking_offset_.x() = gain_inv * tracking_offset_.x(); + tracking_offset_.y() = gain_inv * tracking_offset_.y(); + } return true; } @@ -390,9 +395,6 @@ bool BigVehicleTracker::measure( measureWithPose(updating_object); measureWithShape(updating_object); - /* calc nearest corner index*/ - setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step - return true; } @@ -414,15 +416,6 @@ bool BigVehicleTracker::getTrackedObject( return false; } - // recover bounding box from tracking point - const double dl = bounding_box_.length - last_input_bounding_box_.length; - const double dw = bounding_box_.width - last_input_bounding_box_.width; - const Eigen::Vector2d recovered_pose = utils::recoverFromTrackingPoint( - pose_with_cov.pose.position.x, pose_with_cov.pose.position.y, - motion_model_.getStateElement(IDX::YAW), dw, dl, last_nearest_corner_index_, tracking_offset_); - pose_with_cov.pose.position.x = recovered_pose.x(); - pose_with_cov.pose.position.y = recovered_pose.y(); - // position pose_with_cov.pose.position.z = z_; diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index a4c080cb40eea..7047d3d34e815 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 @@ -43,8 +43,8 @@ using Label = autoware_perception_msgs::msg::ObjectClassification; NormalVehicleTracker::NormalVehicleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, const uint & channel_index) : Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("NormalVehicleTracker")), @@ -179,9 +179,6 @@ NormalVehicleTracker::NormalVehicleTracker( // initialize motion model motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); } - - /* calc nearest corner index*/ - setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step } bool NormalVehicleTracker::predict(const rclcpp::Time & time) @@ -354,11 +351,19 @@ bool NormalVehicleTracker::measureWithShape( // update motion model motion_model_.updateExtendedState(bounding_box_.length); - // // update offset into object position - // motion_model_.adjustPosition(gain * tracking_offset_.x(), gain * tracking_offset_.y()); - // // update offset - // tracking_offset_.x() = gain_inv * tracking_offset_.x(); - // tracking_offset_.y() = gain_inv * tracking_offset_.y(); + // update offset into object position + { + // rotate back the offset vector from object coordinate to global coordinate + const double yaw = motion_model_.getStateElement(IDX::YAW); + const double offset_x_global = + tracking_offset_.x() * std::cos(yaw) - tracking_offset_.y() * std::sin(yaw); + const double offset_y_global = + tracking_offset_.x() * std::sin(yaw) + tracking_offset_.y() * std::cos(yaw); + motion_model_.adjustPosition(-gain * offset_x_global, -gain * offset_y_global); + // update offset (object coordinate) + tracking_offset_.x() = gain_inv * tracking_offset_.x(); + tracking_offset_.y() = gain_inv * tracking_offset_.y(); + } return true; } @@ -392,9 +397,6 @@ bool NormalVehicleTracker::measure( measureWithPose(updating_object); measureWithShape(updating_object); - /* calc nearest corner index*/ - setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step - return true; } @@ -416,15 +418,6 @@ bool NormalVehicleTracker::getTrackedObject( return false; } - // recover bounding box from tracking point - const double dl = bounding_box_.length - last_input_bounding_box_.length; - const double dw = bounding_box_.width - last_input_bounding_box_.width; - const Eigen::Vector2d recovered_pose = utils::recoverFromTrackingPoint( - pose_with_cov.pose.position.x, pose_with_cov.pose.position.y, - motion_model_.getStateElement(IDX::YAW), dw, dl, last_nearest_corner_index_, tracking_offset_); - pose_with_cov.pose.position.x = recovered_pose.x(); - pose_with_cov.pose.position.y = recovered_pose.y(); - // position pose_with_cov.pose.position.z = z_; From 4aae3a0d083cf86d500c2b1c7a69a242d9724387 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 13 Mar 2024 13:36:45 +0900 Subject: [PATCH 2/3] fix: update object size only by updating object size, do not memorize previous object size Signed-off-by: Taekjin LEE --- .../tracker/model/big_vehicle_tracker.hpp | 1 - .../tracker/model/normal_vehicle_tracker.hpp | 1 - .../include/multi_object_tracker/utils/utils.hpp | 2 +- .../src/tracker/model/big_vehicle_tracker.cpp | 12 ++++-------- .../src/tracker/model/normal_vehicle_tracker.cpp | 12 ++++-------- 5 files changed, 9 insertions(+), 19 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index 233d6528dca9f..8eec03c5dc847 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 @@ -49,7 +49,6 @@ class BigVehicleTracker : public Tracker double height; }; BoundingBox bounding_box_; - BoundingBox last_input_bounding_box_; Eigen::Vector2d tracking_offset_; private: diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 0d0b5d653cecf..107a3ef194afb 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 @@ -49,7 +49,6 @@ class NormalVehicleTracker : public Tracker double height; }; BoundingBox bounding_box_; - BoundingBox last_input_bounding_box_; Eigen::Vector2d tracking_offset_; private: 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 2294462e05fea..259a58342c369 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 @@ -280,7 +280,7 @@ inline void calcAnchorPointOffset( // update offset const Eigen::Vector2d offset = calcOffsetVectorFromShapeChange(w_n - w, l_n - l, indx); - tracking_offset += offset; + tracking_offset = offset; // offset input object const Eigen::Matrix2d R = Eigen::Rotation2Dd(yaw).toRotationMatrix(); 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 c8866a6f8ca25..daecb0b2bd4f7 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 @@ -76,7 +76,6 @@ BigVehicleTracker::BigVehicleTracker( 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}; - last_input_bounding_box_ = bounding_box_; } else { autoware_perception_msgs::msg::DetectedObject bbox_object; if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { @@ -89,7 +88,6 @@ BigVehicleTracker::BigVehicleTracker( bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; } - last_input_bounding_box_ = bounding_box_; } // set maximum and minimum size constexpr double max_size = 30.0; @@ -211,11 +209,11 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje } // get offset measurement - int nearest_corner_index = utils::getNearestCornerOrSurface( + const int nearest_corner_index = utils::getNearestCornerOrSurface( tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform); utils::calcAnchorPointOffset( - last_input_bounding_box_.width, last_input_bounding_box_.length, nearest_corner_index, - bbox_object, tracked_yaw, updating_object, tracking_offset_); + bounding_box_.width, bounding_box_.length, nearest_corner_index, bbox_object, tracked_yaw, + updating_object, tracking_offset_); // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { @@ -329,15 +327,13 @@ bool BigVehicleTracker::measureWithShape( return false; } - constexpr double gain = 0.1; + 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; - last_input_bounding_box_ = { - object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; // set maximum and minimum size constexpr double max_size = 30.0; 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 7047d3d34e815..d197a32f84b5e 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 @@ -76,7 +76,6 @@ NormalVehicleTracker::NormalVehicleTracker( 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}; - last_input_bounding_box_ = bounding_box_; } else { autoware_perception_msgs::msg::DetectedObject bbox_object; if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { @@ -90,7 +89,6 @@ NormalVehicleTracker::NormalVehicleTracker( bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; } - last_input_bounding_box_ = bounding_box_; } // set maximum and minimum size constexpr double max_size = 20.0; @@ -213,11 +211,11 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO } // get offset measurement - int nearest_corner_index = utils::getNearestCornerOrSurface( + const int nearest_corner_index = utils::getNearestCornerOrSurface( tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform); utils::calcAnchorPointOffset( - last_input_bounding_box_.width, last_input_bounding_box_.length, nearest_corner_index, - bbox_object, tracked_yaw, updating_object, tracking_offset_); + bounding_box_.width, bounding_box_.length, nearest_corner_index, bbox_object, tracked_yaw, + updating_object, tracking_offset_); // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { @@ -331,15 +329,13 @@ bool NormalVehicleTracker::measureWithShape( return false; } - constexpr double gain = 0.1; + 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; - last_input_bounding_box_ = { - object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; // set maximum and minimum size constexpr double max_size = 20.0; From 3d739870413fe2a036ee8e4e18d9f5c05714347a Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 5 Jun 2024 13:37:16 +0900 Subject: [PATCH 3/3] fix: rename message package Signed-off-by: Taekjin LEE --- .../src/tracker/model/big_vehicle_tracker.cpp | 2 +- .../src/tracker/model/normal_vehicle_tracker.cpp | 2 +- 2 files changed, 2 insertions(+), 2 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 daecb0b2bd4f7..ebf5a977ed44a 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 @@ -43,7 +43,7 @@ using Label = autoware_perception_msgs::msg::ObjectClassification; BigVehicleTracker::BigVehicleTracker( - const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, const uint & channel_index) : Tracker(time, object.classification, channel_size), 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 d197a32f84b5e..3b6f644614d9b 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 @@ -43,7 +43,7 @@ using Label = autoware_perception_msgs::msg::ObjectClassification; NormalVehicleTracker::NormalVehicleTracker( - const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, const uint & channel_index) : Tracker(time, object.classification, channel_size),