From cfcdbcadf9a85a920fae00ef9099c2bb1f7bf974 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 10 Mar 2025 13:55:45 +0900 Subject: [PATCH] refactor(multi_object_tracker): update calcAnchorPointOffset function signature and streamline object handling --- .../multi_object_tracker/object_model/shapes.hpp | 4 ++-- .../lib/object_model/shapes.cpp | 13 ++++++------- .../lib/tracker/model/bicycle_tracker.cpp | 4 ++-- .../lib/tracker/model/pedestrian_tracker.cpp | 7 ++----- .../lib/tracker/model/unknown_tracker.cpp | 4 ++-- .../lib/tracker/model/vehicle_tracker.cpp | 13 +++++-------- 6 files changed, 19 insertions(+), 26 deletions(-) diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp index 9f38dae04baee..5e1de408862e1 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp @@ -41,8 +41,8 @@ void getNearestCornerOrSurface( const geometry_msgs::msg::Transform & self_transform, types::DynamicObject & object); void calcAnchorPointOffset( - const types::DynamicObject & this_object, const types::DynamicObject & input_object, - Eigen::Vector2d & tracking_offset, types::DynamicObject & offset_object); + const types::DynamicObject & this_object, Eigen::Vector2d & tracking_offset, + types::DynamicObject & offset_object); } // namespace shapes } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp index c10a3b658580d..ee52e2a354d11 100644 --- a/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp +++ b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp @@ -218,17 +218,16 @@ void getNearestCornerOrSurface( } void calcAnchorPointOffset( - const types::DynamicObject & this_object, const types::DynamicObject & input_object, - Eigen::Vector2d & tracking_offset, types::DynamicObject & offset_object) + const types::DynamicObject & this_object, Eigen::Vector2d & tracking_offset, + types::DynamicObject & updating_object) { // copy value - offset_object = input_object; - const geometry_msgs::msg::Point anchor_vector = input_object.anchor_point; + const geometry_msgs::msg::Point anchor_vector = updating_object.anchor_point; // invalid anchor if (anchor_vector.x <= 1e-6 && anchor_vector.y <= 1e-6) { return; } - double input_yaw = tf2::getYaw(input_object.pose.orientation); + double input_yaw = tf2::getYaw(updating_object.pose.orientation); // current object width and height const double length = this_object.shape.dimensions.x; @@ -250,8 +249,8 @@ void calcAnchorPointOffset( // offset input object const Eigen::Matrix2d R = Eigen::Rotation2Dd(input_yaw).toRotationMatrix(); const Eigen::Vector2d rotated_offset = R * tracking_offset; - offset_object.pose.position.x += rotated_offset.x(); - offset_object.pose.position.y += rotated_offset.y(); + updating_object.pose.position.x += rotated_offset.x(); + updating_object.pose.position.y += rotated_offset.y(); } } // namespace shapes diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp index 1343d3089dd21..1a43af98427ce 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp @@ -215,12 +215,12 @@ bool BicycleTracker::getTrackedObject( const rclcpp::Time & time, types::DynamicObject & object) const { object = object_; + + // predict from motion model auto & pose = object.pose; auto & pose_cov = object.pose_covariance; auto & twist = object.twist; auto & twist_cov = object.twist_covariance; - - // predict from motion model if (!motion_model_.getPredictedState(time, pose, pose_cov, twist, twist_cov)) { RCLCPP_WARN(logger_, "BicycleTracker::getTrackedObject: Failed to get predicted state."); return false; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp index b0e85a276c202..af80de7f12060 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp @@ -168,9 +168,6 @@ bool PedestrianTracker::measureWithShape(const types::DynamicObject & object) bool PedestrianTracker::measure(const types::DynamicObject & object, const rclcpp::Time & time) { - // keep the latest input object - object_ = object; - // check time gap const double dt = motion_model_.getDeltaTime(time); if (0.01 /*10msec*/ < dt) { @@ -192,12 +189,12 @@ bool PedestrianTracker::getTrackedObject( const rclcpp::Time & time, types::DynamicObject & object) const { object = object_; + + // predict from motion model auto & pose = object.pose; auto & pose_cov = object.pose_covariance; auto & twist = object.twist; auto & twist_cov = object.twist_covariance; - - // predict from motion model if (!motion_model_.getPredictedState(time, pose, pose_cov, twist, twist_cov)) { RCLCPP_WARN(logger_, "PedestrianTracker::getTrackedObject: Failed to get predicted state."); return false; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp index a99b4e7bb0480..93eb532b70fc3 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp @@ -142,8 +142,8 @@ bool UnknownTracker::measureWithPose(const types::DynamicObject & object) bool UnknownTracker::measure(const types::DynamicObject & object, const rclcpp::Time & time) { - // keep the latest input object - object_ = object; + // update object shape + object_.shape = object.shape; // check time gap const double dt = motion_model_.getDeltaTime(time); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp index dba4d93a98ae2..4c8a9fa661bfd 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp @@ -227,11 +227,8 @@ bool VehicleTracker::measureWithShape(const types::DynamicObject & object) return true; } -bool VehicleTracker::measure(const types::DynamicObject & object, const rclcpp::Time & time) +bool VehicleTracker::measure(const types::DynamicObject & in_object, const rclcpp::Time & time) { - // keep the latest input object - object_ = object; - // check time gap const double dt = motion_model_.getDeltaTime(time); if (0.01 /*10msec*/ < dt) { @@ -243,8 +240,8 @@ bool VehicleTracker::measure(const types::DynamicObject & object, const rclcpp:: } // update object - types::DynamicObject updating_object = object; - shapes::calcAnchorPointOffset(object_, object, tracking_offset_, updating_object); + types::DynamicObject updating_object = in_object; + shapes::calcAnchorPointOffset(object_, tracking_offset_, updating_object); measureWithPose(updating_object); measureWithShape(updating_object); @@ -255,12 +252,12 @@ bool VehicleTracker::getTrackedObject( const rclcpp::Time & time, types::DynamicObject & object) const { object = object_; + + // predict from motion model auto & pose = object.pose; auto & pose_cov = object.pose_covariance; auto & twist = object.twist; auto & twist_cov = object.twist_covariance; - - // predict from motion model if (!motion_model_.getPredictedState(time, pose, pose_cov, twist, twist_cov)) { RCLCPP_WARN(logger_, "VehicleTracker::getTrackedObject: Failed to get predicted state."); return false;