diff --git a/perception/image_projection_based_fusion/config/roi_pointcloud_fusion.param.yaml b/perception/image_projection_based_fusion/config/roi_pointcloud_fusion.param.yaml index 5b86b8e81d1aa..acf5f75163ffe 100644 --- a/perception/image_projection_based_fusion/config/roi_pointcloud_fusion.param.yaml +++ b/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 diff --git a/perception/image_projection_based_fusion/docs/roi-pointcloud-fusion.md b/perception/image_projection_based_fusion/docs/roi-pointcloud-fusion.md index 5d0b241a578d6..f2ce7662da976 100644 --- a/perception/image_projection_based_fusion/docs/roi-pointcloud-fusion.md +++ b/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 | diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp index fe685baa0b6e2..4cbc0990352e6 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp +++ b/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_; diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp index d7fd3c3882046..943f028621189 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp @@ -70,8 +70,8 @@ PointCloud closest_cluster( void updateOutputFusedObjects( std::vector & output_objs, const std::vector & 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 & 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 & output_fused_objects); geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud); diff --git a/perception/image_projection_based_fusion/schema/roi_pointcloud_fusion.schema.json b/perception/image_projection_based_fusion/schema/roi_pointcloud_fusion.schema.json index f39ef257ea789..9dba97a2029ee 100644 --- a/perception/image_projection_based_fusion/schema/roi_pointcloud_fusion.schema.json +++ b/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]", diff --git a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp index 421aa3a453451..af04d174ba661 100644 --- a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp +++ b/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("fuse_unknown_only"); min_cluster_size_ = declare_parameter("min_cluster_size"); + max_cluster_size_ = declare_parameter("max_cluster_size"); cluster_2d_tolerance_ = declare_parameter("cluster_2d_tolerance"); pub_objects_ptr_ = this->create_publisher("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(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)) diff --git a/perception/image_projection_based_fusion/src/utils/utils.cpp b/perception/image_projection_based_fusion/src/utils/utils.cpp index 76cd1e3c61e82..e978b5ab55868 100644 --- a/perception/image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/image_projection_based_fusion/src/utils/utils.cpp @@ -83,8 +83,8 @@ PointCloud closest_cluster( void updateOutputFusedObjects( std::vector & output_objs, const std::vector & 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 & 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 & 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; }