chore(roi_pointcloud_fusion): add maximum cluster size param (#6860)

Signed-off-by: badai-nguyen <[email protected]>

perception/image_projection_based_fusion/config/roi_pointcloud_fusion.param.yaml

@@ -2,4 +2,5 @@
   ros__parameters:
     fuse_unknown_only: true
     min_cluster_size: 2
+    max_cluster_size: 20
     cluster_2d_tolerance: 0.5

perception/image_projection_based_fusion/docs/roi-pointcloud-fusion.md

@@ -37,6 +37,7 @@ The node `roi_pointcloud_fusion` is to cluster the pointcloud based on Region Of
 | Name                   | Type   | Description                                                                                  |
 | ---------------------- | ------ | -------------------------------------------------------------------------------------------- |
 | `min_cluster_size`     | int    | the minimum number of points that a cluster needs to contain in order to be considered valid |
+| `max_cluster_size`     | int    | the maximum number of points that a cluster needs to contain in order to be considered valid |
 | `cluster_2d_tolerance` | double | cluster tolerance measured in radial direction                                               |
 | `rois_number`          | int    | the number of input rois                                                                     |
 

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

@@ -26,6 +26,7 @@ class RoiPointCloudFusionNode
 {
 private:
   int min_cluster_size_{1};
+  int max_cluster_size_{20};
   bool fuse_unknown_only_{true};
   double cluster_2d_tolerance_;
 

perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp

@@ -70,8 +70,8 @@ PointCloud closest_cluster(
 void updateOutputFusedObjects(
   std::vector<DetectedObjectWithFeature> & output_objs, const std::vector<PointCloud> & clusters,
   const std_msgs::msg::Header & in_cloud_header, const std_msgs::msg::Header & in_roi_header,
-  const tf2_ros::Buffer & tf_buffer, const int min_cluster_size, const float cluster_2d_tolerance,
-  std::vector<DetectedObjectWithFeature> & output_fused_objects);
+  const tf2_ros::Buffer & tf_buffer, const int min_cluster_size, const int max_cluster_size,
+  const float cluster_2d_tolerance, std::vector<DetectedObjectWithFeature> & output_fused_objects);
 
 geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud);
 

perception/image_projection_based_fusion/schema/roi_pointcloud_fusion.schema.json

@@ -16,6 +16,11 @@
           "description": "The minimum number of points that a cluster must contain to be considered as valid.",
           "default": 2
         },
+        "max_cluster_size": {
+          "type": "integer",
+          "description": "The maximum number of points that a cluster must contain to be considered as valid.",
+          "default": 20
+        },
         "cluster_2d_tolerance": {
           "type": "number",
           "description": "A cluster tolerance measured in radial direction [m]",

perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp

@@ -35,6 +35,7 @@ RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & opt
 {
   fuse_unknown_only_ = declare_parameter<bool>("fuse_unknown_only");
   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");
   pub_objects_ptr_ =
     this->create_publisher<DetectedObjectsWithFeature>("output_clusters", rclcpp::QoS{1});
@@ -138,7 +139,9 @@ void RoiPointCloudFusionNode::fuseOnSingleImage(
       auto & feature_obj = output_objs.at(i);
       const auto & check_roi = feature_obj.feature.roi;
       auto & cluster = clusters.at(i);
-
+      if (cluster.points.size() >= static_cast<size_t>(max_cluster_size_)) {
+        continue;
+      }
       if (
         check_roi.x_offset <= normalized_projected_point.x() &&
         check_roi.y_offset <= normalized_projected_point.y() &&
@@ -152,7 +155,7 @@ void RoiPointCloudFusionNode::fuseOnSingleImage(
   // refine and update output_fused_objects_
   updateOutputFusedObjects(
     output_objs, clusters, input_pointcloud_msg.header, input_roi_msg.header, tf_buffer_,
-    min_cluster_size_, cluster_2d_tolerance_, output_fused_objects_);
+    min_cluster_size_, max_cluster_size_, cluster_2d_tolerance_, output_fused_objects_);
 }
 
 bool RoiPointCloudFusionNode::out_of_scope(__attribute__((unused))

perception/image_projection_based_fusion/src/utils/utils.cpp

@@ -83,8 +83,8 @@ PointCloud closest_cluster(
 void updateOutputFusedObjects(
   std::vector<DetectedObjectWithFeature> & output_objs, const std::vector<PointCloud> & clusters,
   const std_msgs::msg::Header & in_cloud_header, const std_msgs::msg::Header & in_roi_header,
-  const tf2_ros::Buffer & tf_buffer, const int min_cluster_size, const float cluster_2d_tolerance,
-  std::vector<DetectedObjectWithFeature> & output_fused_objects)
+  const tf2_ros::Buffer & tf_buffer, const int min_cluster_size, const int max_cluster_size,
+  const float cluster_2d_tolerance, std::vector<DetectedObjectWithFeature> & output_fused_objects)
 {
   if (output_objs.size() != clusters.size()) {
     return;
@@ -107,7 +107,9 @@ void updateOutputFusedObjects(
   for (std::size_t i = 0; i < output_objs.size(); ++i) {
     PointCloud cluster = clusters.at(i);
     auto & feature_obj = output_objs.at(i);
-    if (cluster.points.size() < std::size_t(min_cluster_size)) {
+    if (
+      cluster.points.size() < std::size_t(min_cluster_size) ||
+      cluster.points.size() >= std::size_t(max_cluster_size)) {
       continue;
     }