fix(autoware_multi_object_tracker): unknown object orientation (#10286)

* fix(unknown_tracker): update object pose orientation and streamline uncertainty modeling in input manager

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* fix(object_model): correct bounding box calculation by initializing limits and including min_z

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

---------

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

Author: technolojin

perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp

@@ -26,6 +26,7 @@
 #include <tf2/utils.h>
 
 #include <algorithm>
+#include <limits>
 #include <string>
 #include <vector>
 
@@ -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<float>::infinity();
+  float max_y = -std::numeric_limits<float>::infinity();
+  float min_x = std::numeric_limits<float>::infinity();
+  float min_y = std::numeric_limits<float>::infinity();
+  float max_z = -std::numeric_limits<float>::infinity();
+  float min_z = std::numeric_limits<float>::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;
 }

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);

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);