fix(multi_object_tracker): object size becomes zero, risk of numeric …

…error and overflow on IoU calculation (autowarefoundation#6597) * fix: set object minimum size limits Signed-off-by: Taekjin LEE <[email protected]> * fix: object shape conversion, from cylinder to bbox Signed-off-by: Taekjin LEE <[email protected]> * fix: adjust minimum sizes to 0.3 m The minimum size is unified to a small number (0.3 m) to avoid side-effect of tracking. Signed-off-by: Taekjin LEE <[email protected]> --------- Signed-off-by: Taekjin LEE <[email protected]> Signed-off-by: kaigohirao <[email protected]>
kaigohirao · Mar 22, 2024 · 143f843 · 143f843
1 parent 54bcbb4
commit 143f843
Show file tree

Hide file tree

Showing 4 changed files with 52 additions and 3 deletions.
diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp
@@ -140,6 +140,11 @@ 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);
+
   ekf_.init(X, P);
 
   // Set lf, lr
@@ -400,10 +405,15 @@ bool BicycleTracker::measureWithShape(
 {
   // if the input shape is convex type, convert it to bbox type
   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);
-  } else {
+  if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
     bbox_object = object;
+  } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) {
+    // convert cylinder to bbox
+    bbox_object.shape.dimensions.x = object.shape.dimensions.x;
+    bbox_object.shape.dimensions.y = object.shape.dimensions.x;
+    bbox_object.shape.dimensions.z = object.shape.dimensions.z;
+  } else {
+    utils::convertConvexHullToBoundingBox(object, bbox_object);
   }
 
   constexpr float gain = 0.9;
@@ -415,6 +425,11 @@ bool BicycleTracker::measureWithShape(
   last_input_bounding_box_ = {
     bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_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);
+
   // update lf, lr
   lf_ = std::max(bounding_box_.length * 0.3, 0.3);  // 30% front from the center, minimum of 0.3m
   lr_ = std::max(bounding_box_.length * 0.3, 0.3);  // 30% rear from the center, minimum of 0.3m

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
@@ -157,6 +157,11 @@ BigVehicleTracker::BigVehicleTracker(
   /* calc nearest corner index*/
   setNearestCornerOrSurfaceIndex(self_transform);  // this index is used in next measure step
 
+  // 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 lf, lr
   lf_ = std::max(bounding_box_.length * 0.3, 1.5);   // 30% front from the center, minimum of 1.5m
   lr_ = std::max(bounding_box_.length * 0.25, 1.5);  // 25% rear from the center, minimum of 1.5m
@@ -455,6 +460,11 @@ bool BigVehicleTracker::measureWithShape(
   last_input_bounding_box_ = {
     bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_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);
+
   // update lf, lr
   lf_ = std::max(bounding_box_.length * 0.3, 1.5);   // 30% front from the center, minimum of 1.5m
   lr_ = std::max(bounding_box_.length * 0.25, 1.5);  // 25% rear from the center, minimum of 1.5m

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
@@ -157,6 +157,11 @@ NormalVehicleTracker::NormalVehicleTracker(
   /* calc nearest corner index*/
   setNearestCornerOrSurfaceIndex(self_transform);  // this index is used in next measure step
 
+  // 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 lf, lr
   lf_ = std::max(bounding_box_.length * 0.3, 1.0);   // 30% front from the center, minimum of 1.0m
   lr_ = std::max(bounding_box_.length * 0.25, 1.0);  // 25% rear from the center, minimum of 1.0m
@@ -455,6 +460,11 @@ bool NormalVehicleTracker::measureWithShape(
   last_input_bounding_box_ = {
     bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_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);
+
   // update lf, lr
   lf_ = std::max(bounding_box_.length * 0.3, 1.0);   // 30% front from the center, minimum of 1.0m
   lr_ = std::max(bounding_box_.length * 0.25, 1.0);  // 25% rear from the center, minimum of 1.0m

diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp
@@ -143,6 +143,13 @@ PedestrianTracker::PedestrianTracker(
     cylinder_ = {object.shape.dimensions.x, 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);
+  cylinder_.width = std::max(cylinder_.width, 0.3);
+  cylinder_.height = std::max(cylinder_.height, 0.3);
+
   ekf_.init(X, P);
 }
 
@@ -317,6 +324,13 @@ bool PedestrianTracker::measureWithShape(
     return false;
   }
 
+  // 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);
+  cylinder_.width = std::max(cylinder_.width, 0.3);
+  cylinder_.height = std::max(cylinder_.height, 0.3);
+
   return true;
 }