From f978993bcc5cd045515a84bfd46a6ddc8735e2b8 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 17 Mar 2025 19:43:38 +0900 Subject: [PATCH 1/2] fix(unknown_tracker): update object pose orientation and streamline uncertainty modeling in input manager Signed-off-by: Taekjin LEE --- .../lib/tracker/model/unknown_tracker.cpp | 1 + .../src/processor/input_manager.cpp | 28 +++++++++---------- 2 files changed, 15 insertions(+), 14 deletions(-) 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 fec45f42bce3f..f97411dc1dc9c 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 @@ -144,6 +144,7 @@ bool UnknownTracker::measure(const types::DynamicObject & object, const rclcpp:: { // update object shape object_.shape = object.shape; + object_.pose.orientation = object.pose.orientation; // check time gap const double dt = motion_model_.getDeltaTime(time); diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp index 045ff427dc823..1544267bdd298 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp @@ -56,6 +56,20 @@ void InputStream::onMessage( types::DynamicObjectList dynamic_objects = types::toDynamicObjectList(objects, channel_.index); + // Model the object uncertainty only if it is not available + types::DynamicObjectList objects_with_uncertainty = + uncertainty::modelUncertainty(dynamic_objects); + + // Transform the objects to the world frame + auto transformed_objects = odometry_->transformObjects(objects_with_uncertainty); + if (!transformed_objects) { + RCLCPP_WARN( + node_.get_logger(), "InputManager::onMessage %s: Failed to transform objects.", + channel_.long_name.c_str()); + return; + } + dynamic_objects = transformed_objects.value(); + // object shape processing for (auto & object : dynamic_objects.objects) { const auto label = @@ -91,20 +105,6 @@ void InputStream::onMessage( // if object extension is not reliable, enlarge covariance of position and extend shape } - // Model the object uncertainty only if it is not available - types::DynamicObjectList objects_with_uncertainty = - uncertainty::modelUncertainty(dynamic_objects); - - // Transform the objects to the world frame - auto transformed_objects = odometry_->transformObjects(objects_with_uncertainty); - if (!transformed_objects) { - RCLCPP_WARN( - node_.get_logger(), "InputManager::onMessage %s: Failed to transform objects.", - channel_.long_name.c_str()); - return; - } - dynamic_objects = transformed_objects.value(); - // Normalize the object uncertainty uncertainty::normalizeUncertainty(dynamic_objects); From a64d4445682fb3aa04c9a2d1bd310747dcfeae1a Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 17 Mar 2025 19:48:26 +0900 Subject: [PATCH 2/2] fix(object_model): correct bounding box calculation by initializing limits and including min_z Signed-off-by: Taekjin LEE --- .../lib/object_model/shapes.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) 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 ee52e2a354d11..7019734eee59d 100644 --- a/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp +++ b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp @@ -26,6 +26,7 @@ #include #include +#include #include #include @@ -92,17 +93,20 @@ bool convertConvexHullToBoundingBox( } // look for bounding box boundary - float max_x = 0; - float max_y = 0; - float min_x = 0; - float min_y = 0; - float max_z = 0; + float max_x = -std::numeric_limits::infinity(); + float max_y = -std::numeric_limits::infinity(); + float min_x = std::numeric_limits::infinity(); + float min_y = std::numeric_limits::infinity(); + float max_z = -std::numeric_limits::infinity(); + float min_z = std::numeric_limits::infinity(); + 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); + min_z = std::min(min_z, point.z); } // calc new center @@ -120,7 +124,7 @@ bool convertConvexHullToBoundingBox( 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; + output_object.shape.dimensions.z = max_z - min_z; return true; }