feat(image_projection_based_fusion): fix iou geometry function (autow…

…arefoundation#5568) * enable to show debug iou value in roi_cluster_fusion Signed-off-by: yoshiri <[email protected]> * refactor iou draw settings Signed-off-by: yoshiri <[email protected]> * add backgroud color to iou Signed-off-by: yoshiri <[email protected]> * prevent object copying when debugger is not enabled Signed-off-by: yoshiri <[email protected]> * fix roi geometry functions Signed-off-by: yoshiri <[email protected]> --------- Signed-off-by: yoshiri <[email protected]>
satoshi-ota · Jan 14, 2024 · 097f525 · 097f525
1 parent cfbf825
commit 097f525
Show file tree

Hide file tree

Showing 3 changed files with 45 additions and 11 deletions.
diff --git a/...on/image_projection_based_fusion/include/image_projection_based_fusion/utils/geometry.hpp b/...on/image_projection_based_fusion/include/image_projection_based_fusion/utils/geometry.hpp
@@ -64,6 +64,8 @@ bool is_inside(
   const sensor_msgs::msg::RegionOfInterest & outer,
   const sensor_msgs::msg::RegionOfInterest & inner, const double outer_offset_scale = 1.1);
 
+void sanitizeROI(sensor_msgs::msg::RegionOfInterest & roi, const int width, const int height);
+
 }  // namespace image_projection_based_fusion
 
 #endif  // IMAGE_PROJECTION_BASED_FUSION__UTILS__GEOMETRY_HPP_
diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp
@@ -177,17 +177,18 @@ void RoiClusterFusionNode::fuseOnSingleImage(
       double iou(0.0);
       bool is_use_non_trust_object_iou_mode = is_far_enough(
         input_cluster_msg.feature_objects.at(cluster_map.first), trust_object_distance_);
+      auto image_roi = feature_obj.feature.roi;
+      auto cluster_roi = cluster_map.second;
+      sanitizeROI(image_roi, camera_info.width, camera_info.height);
+      sanitizeROI(cluster_roi, camera_info.width, camera_info.height);
       if (is_use_non_trust_object_iou_mode || is_roi_label_known) {
-        iou =
-          cal_iou_by_mode(cluster_map.second, feature_obj.feature.roi, non_trust_object_iou_mode_);
+        iou = cal_iou_by_mode(cluster_roi, image_roi, non_trust_object_iou_mode_);
       } else {
-        iou = cal_iou_by_mode(cluster_map.second, feature_obj.feature.roi, trust_object_iou_mode_);
+        iou = cal_iou_by_mode(cluster_roi, image_roi, trust_object_iou_mode_);
       }
 
       const bool passed_inside_cluster_gate =
-        only_allow_inside_cluster_
-          ? is_inside(feature_obj.feature.roi, cluster_map.second, roi_scale_factor_)
-          : true;
+        only_allow_inside_cluster_ ? is_inside(image_roi, cluster_roi, roi_scale_factor_) : true;
       if (max_iou < iou && passed_inside_cluster_gate) {
         index = cluster_map.first;
         max_iou = iou;

diff --git a/perception/image_projection_based_fusion/src/utils/geometry.cpp b/perception/image_projection_based_fusion/src/utils/geometry.cpp
@@ -166,11 +166,42 @@ bool is_inside(
   const sensor_msgs::msg::RegionOfInterest & outer,
   const sensor_msgs::msg::RegionOfInterest & inner, const double outer_offset_scale)
 {
-  const double lower_scale = 1.0 - std::abs(outer_offset_scale - 1.0);
-  return outer.x_offset * lower_scale <= inner.x_offset &&
-         outer.y_offset * lower_scale <= inner.y_offset &&
-         inner.x_offset + inner.width <= (outer.x_offset + outer.width) * outer_offset_scale &&
-         inner.y_offset + inner.height <= (outer.y_offset + outer.height) * outer_offset_scale;
+  const auto scaled_width = static_cast<double>(outer.width) * outer_offset_scale;
+  const auto scaled_height = static_cast<double>(outer.height) * outer_offset_scale;
+  const auto scaled_x_offset =
+    static_cast<double>(outer.x_offset) - (scaled_width - outer.width) / 2.0;
+  const auto scaled_y_offset =
+    static_cast<double>(outer.y_offset) - (scaled_height - outer.height) / 2.0;
+
+  // 1. check left-top corner
+  if (scaled_x_offset > inner.x_offset || scaled_y_offset > inner.y_offset) {
+    return false;
+  }
+  // 2. check right-bottom corner
+  if (
+    scaled_x_offset + scaled_width < inner.x_offset + inner.width ||
+    scaled_y_offset + scaled_height < inner.y_offset + inner.height) {
+    return false;
+  }
+  return true;
+}
+
+void sanitizeROI(sensor_msgs::msg::RegionOfInterest & roi, const int width_, const int height_)
+{
+  const unsigned int width = static_cast<unsigned int>(width_);
+  const unsigned int height = static_cast<unsigned int>(height_);
+  if (roi.x_offset >= width || roi.y_offset >= height) {
+    roi.width = 0;
+    roi.height = 0;
+    return;
+  }
+
+  if (roi.x_offset + roi.width > width) {
+    roi.width = width - roi.x_offset;
+  }
+  if (roi.y_offset + roi.height > height) {
+    roi.height = height - roi.y_offset;
+  }
 }
 
 }  // namespace image_projection_based_fusion