fix(multi_object_tracker): prevent too large object tracking (autowar…

…efoundation#7159)

* fix: set minimum and maximum object for size update

Signed-off-by: Taekjin LEE <[email protected]>

* fix: bug of convertConvexHullToBoundingBox

Signed-off-by: Taekjin LEE <[email protected]>

* fix: return false when footprint is less than 3 points

Signed-off-by: Taekjin LEE <[email protected]>

* fix: size filter bug

Signed-off-by: Taekjin LEE <[email protected]>

* style(pre-commit): autofix

Signed-off-by: Taekjin LEE <[email protected]>

* fix: try to convert polygon to bbox

Signed-off-by: Taekjin LEE <[email protected]>

* chore: clean-up

Signed-off-by: Taekjin LEE <[email protected]>

* fix: bicycle tracker to try bbox convert

Signed-off-by: Taekjin LEE <[email protected]>

* style(pre-commit): autofix

Signed-off-by: Taekjin LEE <[email protected]>

* chore: clean-up

Signed-off-by: Taekjin LEE <[email protected]>

---------

Signed-off-by: Taekjin LEE <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp

@@ -295,40 +295,43 @@ inline void calcAnchorPointOffset(
  * @param input_object: input convex hull objects
  * @param output_object: output bounding box objects
  */
-inline void convertConvexHullToBoundingBox(
+inline bool convertConvexHullToBoundingBox(
   const autoware_auto_perception_msgs::msg::DetectedObject & input_object,
   autoware_auto_perception_msgs::msg::DetectedObject & output_object)
 {
-  const Eigen::Vector2d center{
-    input_object.kinematics.pose_with_covariance.pose.position.x,
-    input_object.kinematics.pose_with_covariance.pose.position.y};
-  const auto yaw = tf2::getYaw(input_object.kinematics.pose_with_covariance.pose.orientation);
-  const Eigen::Matrix2d R_inv = Eigen::Rotation2Dd(-yaw).toRotationMatrix();
+  // check footprint size
+  if (input_object.shape.footprint.points.size() < 3) {
+    return false;
+  }
 
+  // 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;
-
-  // look for bounding box boundary
   for (size_t i = 0; i < input_object.shape.footprint.points.size(); ++i) {
-    Eigen::Vector2d vertex{
-      input_object.shape.footprint.points.at(i).x, input_object.shape.footprint.points.at(i).y};
-
-    const Eigen::Vector2d local_vertex = R_inv * (vertex - center);
-    max_x = std::max(max_x, local_vertex.x());
-    max_y = std::max(max_y, local_vertex.y());
-    min_x = std::min(min_x, local_vertex.x());
-    min_y = std::min(min_y, local_vertex.y());
-
-    max_z = std::max(max_z, static_cast<double>(input_object.shape.footprint.points.at(i).z));
+    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);
   }
 
   // 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,
+    input_object.kinematics.pose_with_covariance.pose.position.y};
+  const auto yaw = tf2::getYaw(input_object.kinematics.pose_with_covariance.pose.orientation);
+  const Eigen::Matrix2d R_inv = Eigen::Rotation2Dd(-yaw).toRotationMatrix();
   const Eigen::Vector2d new_local_center{(max_x + min_x) / 2.0, (max_y + min_y) / 2.0};
   const Eigen::Vector2d new_center = center + R_inv.transpose() * new_local_center;
 
@@ -341,6 +344,8 @@ inline void convertConvexHullToBoundingBox(
   output_object.shape.dimensions.x = length;
   output_object.shape.dimensions.y = width;
   output_object.shape.dimensions.z = height;
+
+  return true;
 }
 
 inline bool getMeasurementYaw(

perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp

@@ -71,10 +71,12 @@ BicycleTracker::BicycleTracker(
   } else {
     bounding_box_ = {1.0, 0.5, 1.7};
   }
-  // set minimum size
-  bounding_box_.length = std::max(bounding_box_.length, 0.3);
-  bounding_box_.width = std::max(bounding_box_.width, 0.3);
-  bounding_box_.height = std::max(bounding_box_.height, 0.3);
+  // 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);
 
   // Set motion model parameters
   {
@@ -172,7 +174,9 @@ autoware_auto_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingOb
   // OBJECT SHAPE MODEL
   // convert to bounding box if input is convex shape
   if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
-    utils::convertConvexHullToBoundingBox(object, updating_object);
+    if (!utils::convertConvexHullToBoundingBox(object, updating_object)) {
+      updating_object = object;
+    }
   } else {
     updating_object = object;
   }
@@ -226,22 +230,38 @@ bool BicycleTracker::measureWithPose(
 bool BicycleTracker::measureWithShape(
   const autoware_auto_perception_msgs::msg::DetectedObject & object)
 {
+  autoware_auto_perception_msgs::msg::DetectedObject bbox_object;
   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 true;
+    return false;
   }
 
-  constexpr double gain = 0.1;
-  constexpr double gain_inv = 1.0 - gain;
+  // check bound box size abnormality
+  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) {
+    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) {
+    return false;
+  }
 
   // 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;
-  // set minimum size
-  bounding_box_.length = std::max(bounding_box_.length, 0.3);
-  bounding_box_.width = std::max(bounding_box_.width, 0.3);
-  bounding_box_.height = std::max(bounding_box_.height, 0.3);
+  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;
+
+  // 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);
 
   // update motion model
   motion_model_.updateExtendedState(bounding_box_.length);

perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp

@@ -79,16 +79,24 @@ BigVehicleTracker::BigVehicleTracker(
     last_input_bounding_box_ = bounding_box_;
   } else {
     autoware_auto_perception_msgs::msg::DetectedObject bbox_object;
-    utils::convertConvexHullToBoundingBox(object, bbox_object);
-    bounding_box_ = {
-      bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y,
-      bbox_object.shape.dimensions.z};
+    if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) {
+      RCLCPP_WARN(
+        logger_,
+        "BigVehicleTracker::BigVehicleTracker: Failed to convert convex hull to bounding box.");
+      bounding_box_ = {6.0, 2.0, 2.0};  // default value
+    } else {
+      bounding_box_ = {
+        bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y,
+        bbox_object.shape.dimensions.z};
+    }
     last_input_bounding_box_ = bounding_box_;
   }
-  // set minimum size
-  bounding_box_.length = std::max(bounding_box_.length, 0.3);
-  bounding_box_.width = std::max(bounding_box_.width, 0.3);
-  bounding_box_.height = std::max(bounding_box_.height, 0.3);
+  // 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);
 
   // Set motion model parameters
   {
@@ -195,7 +203,12 @@ autoware_auto_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatin
   // convert to bounding box if input is convex shape
   autoware_auto_perception_msgs::msg::DetectedObject bbox_object;
   if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
-    utils::convertConvexHullToBoundingBox(object, bbox_object);
+    if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) {
+      RCLCPP_WARN(
+        logger_,
+        "BigVehicleTracker::getUpdatingObject: Failed to convert convex hull to bounding box.");
+      bbox_object = object;
+    }
   } else {
     bbox_object = object;
   }
@@ -306,6 +319,20 @@ bool BigVehicleTracker::measureWithPose(
 bool BigVehicleTracker::measureWithShape(
   const autoware_auto_perception_msgs::msg::DetectedObject & object)
 {
+  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;
+  }
+
+  // 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) {
+    return false;
+  }
+
   constexpr double gain = 0.1;
   constexpr double gain_inv = 1.0 - gain;
 
@@ -315,10 +342,13 @@ bool BigVehicleTracker::measureWithShape(
   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 minimum size
-  bounding_box_.length = std::max(bounding_box_.length, 0.3);
-  bounding_box_.width = std::max(bounding_box_.width, 0.3);
-  bounding_box_.height = std::max(bounding_box_.height, 0.3);
+
+  // 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);
 
   // update motion model
   motion_model_.updateExtendedState(bounding_box_.length);

perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp

@@ -79,16 +79,25 @@ NormalVehicleTracker::NormalVehicleTracker(
     last_input_bounding_box_ = bounding_box_;
   } else {
     autoware_auto_perception_msgs::msg::DetectedObject bbox_object;
-    utils::convertConvexHullToBoundingBox(object, bbox_object);
-    bounding_box_ = {
-      bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y,
-      bbox_object.shape.dimensions.z};
+    if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) {
+      RCLCPP_WARN(
+        logger_,
+        "NormalVehicleTracker::NormalVehicleTracker: Failed to convert convex hull to bounding "
+        "box.");
+      bounding_box_ = {3.0, 2.0, 1.8};  // default value
+    } else {
+      bounding_box_ = {
+        bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y,
+        bbox_object.shape.dimensions.z};
+    }
     last_input_bounding_box_ = bounding_box_;
   }
-  // set minimum size
-  bounding_box_.length = std::max(bounding_box_.length, 0.3);
-  bounding_box_.width = std::max(bounding_box_.width, 0.3);
-  bounding_box_.height = std::max(bounding_box_.height, 0.3);
+  // 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);
 
   // Set motion model parameters
   {
@@ -195,7 +204,13 @@ autoware_auto_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpda
   // convert to bounding box if input is convex shape
   autoware_auto_perception_msgs::msg::DetectedObject bbox_object;
   if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
-    utils::convertConvexHullToBoundingBox(object, bbox_object);
+    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;
   }
@@ -306,6 +321,20 @@ bool NormalVehicleTracker::measureWithPose(
 bool NormalVehicleTracker::measureWithShape(
   const autoware_auto_perception_msgs::msg::DetectedObject & object)
 {
+  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;
+  }
+
+  // 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) {
+    return false;
+  }
+
   constexpr double gain = 0.1;
   constexpr double gain_inv = 1.0 - gain;
 
@@ -315,10 +344,13 @@ bool NormalVehicleTracker::measureWithShape(
   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 minimum size
-  bounding_box_.length = std::max(bounding_box_.length, 0.3);
-  bounding_box_.width = std::max(bounding_box_.width, 0.3);
-  bounding_box_.height = std::max(bounding_box_.height, 0.3);
+
+  // 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);
 
   // update motion model
   motion_model_.updateExtendedState(bounding_box_.length);