fix(roi_pointcloud_fusion): add roi scale factor param (#10333)

* fix(roi_pointcloud_fusion): add roi scale factor param

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* fix: missing declare

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

---------

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

Author: badai-nguyen

perception/autoware_image_projection_based_fusion/config/roi_pointcloud_fusion.param.yaml

@@ -4,3 +4,4 @@
     min_cluster_size: 2
     max_cluster_size: 20
     cluster_2d_tolerance: 0.5
+    roi_scale_factor: 1.0

perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp

@@ -46,6 +46,7 @@ class RoiPointCloudFusionNode : public FusionNode<PointCloudMsgType, RoiMsgType,
   int max_cluster_size_{20};
   bool fuse_unknown_only_{true};
   double cluster_2d_tolerance_;
+  double roi_scale_factor_{1.0};
 
   std::vector<ClusterObjType> output_fused_objects_;
 };

perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/geometry.hpp

@@ -64,6 +64,10 @@ bool is_inside(
   const sensor_msgs::msg::RegionOfInterest & outer,
   const sensor_msgs::msg::RegionOfInterest & inner, const double outer_offset_scale = 1.1);
 
+bool isPointInsideRoi(
+  const sensor_msgs::msg::RegionOfInterest & roi, const double camera_point_x,
+  const double camera_point_y, const double roi_scale_factor);
+
 void sanitizeROI(sensor_msgs::msg::RegionOfInterest & roi, const int width, const int height);
 
 }  // namespace autoware::image_projection_based_fusion

perception/autoware_image_projection_based_fusion/schema/roi_pointcloud_fusion.schema.json

@@ -26,9 +26,20 @@
           "description": "A cluster tolerance measured in radial direction [m]",
           "default": 0.5,
           "exclusiveMinimum": 0.0
+        },
+        "roi_scale_factor": {
+          "type": "number",
+          "description": "A scale factor for resizing RoI while checking if points are inside the RoI.",
+          "default": 1.0,
+          "exclusiveMinimum": 0.0
         }
       },
-      "required": ["fuse_unknown_only", "min_cluster_size", "cluster_2d_tolerance"]
+      "required": [
+        "fuse_unknown_only",
+        "min_cluster_size",
+        "cluster_2d_tolerance",
+        "roi_scale_factor"
+      ]
     }
   },
   "properties": {

perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp

@@ -43,6 +43,7 @@ RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & opt
   min_cluster_size_ = declare_parameter<int>("min_cluster_size");
   max_cluster_size_ = declare_parameter<int>("max_cluster_size");
   cluster_2d_tolerance_ = declare_parameter<double>("cluster_2d_tolerance");
+  roi_scale_factor_ = declare_parameter<double>("roi_scale_factor");
 
   // publisher
   pub_ptr_ = this->create_publisher<ClusterMsgType>("output", rclcpp::QoS{1});
@@ -149,10 +150,7 @@ void RoiPointCloudFusionNode::fuse_on_single_image(
           static_cast<size_t>(max_cluster_size_) * static_cast<size_t>(point_step)) {
           continue;
         }
-        if (
-          check_roi.x_offset <= px && check_roi.y_offset <= py &&
-          check_roi.x_offset + check_roi.width >= px &&
-          check_roi.y_offset + check_roi.height >= py) {
+        if (isPointInsideRoi(check_roi, px, py, roi_scale_factor_)) {
           std::memcpy(
             &cluster.data[clusters_data_size.at(i)], &input_pointcloud_msg.data[offset],
             point_step);

perception/autoware_image_projection_based_fusion/src/utils/geometry.cpp

@@ -187,6 +187,23 @@ bool is_inside(
   }
   return true;
 }
+bool isPointInsideRoi(
+  const sensor_msgs::msg::RegionOfInterest & roi, const double camera_point_x,
+  const double camera_point_y, const double roi_scale_factor)
+{
+  const auto scaled_width = static_cast<double>(roi.width) * roi_scale_factor;
+  const auto scaled_heigh = static_cast<double>(roi.height) * roi_scale_factor;
+  const auto scaled_x_offset = static_cast<double>(roi.x_offset) - (scaled_width - roi.width) / 2.0;
+  const auto scaled_y_offset =
+    static_cast<double>(roi.y_offset) - (scaled_heigh - roi.height) / 2.0;
+  if (
+    scaled_x_offset <= camera_point_x && scaled_y_offset <= camera_point_y &&
+    scaled_x_offset + scaled_width >= camera_point_x &&
+    scaled_y_offset + scaled_heigh >= camera_point_y) {
+    return true;
+  }
+  return false;
+}
 
 void sanitizeROI(sensor_msgs::msg::RegionOfInterest & roi, const int width_, const int height_)
 {