feat(autoware_image_projection_based_fusion): add mask cluster fusion node

perception/autoware_image_projection_based_fusion/CMakeLists.txt

@@ -24,6 +24,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/roi_detected_object_fusion/node.cpp
   src/segmentation_pointcloud_fusion/node.cpp
   src/roi_pointcloud_fusion/node.cpp
+  src/mask_cluster_fusion/node.cpp
 )
 
 target_link_libraries(${PROJECT_NAME}
@@ -51,6 +52,11 @@ rclcpp_components_register_node(${PROJECT_NAME}
   EXECUTABLE roi_pointcloud_fusion_node
 )
 
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::image_projection_based_fusion::MaskClusterFusionNode"
+  EXECUTABLE mask_cluster_fusion_node
+)
+
 set(CUDA_VERBOSE OFF)
 
 # set flags for CUDA availability

perception/autoware_image_projection_based_fusion/config/mask_cluster_fusion.param.yaml

@@ -0,0 +1,5 @@
+/**:
+  ros__parameters:
+    fusion_distance: 100.0 # the distance threshold for the fusion
+    fusion_ratio: 0.4 # the ratio between the selected cluster points and the total points in the cluster
+    remove_unknown: true # remove unknown clusters

perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp

@@ -20,6 +20,7 @@
 #include <autoware/universe_utils/system/stop_watch.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <autoware_internal_msgs/msg/segmentation_mask.hpp>
 #include <autoware_perception_msgs/msg/detected_objects.hpp>
 #include <sensor_msgs/msg/camera_info.hpp>
 #include <sensor_msgs/msg/image.hpp>
@@ -54,6 +55,7 @@ using sensor_msgs::msg::PointCloud2;
 using tier4_perception_msgs::msg::DetectedObjectsWithFeature;
 using tier4_perception_msgs::msg::DetectedObjectWithFeature;
 using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
+using autoware_internal_msgs::msg::SegmentationMask;
 using autoware_perception_msgs::msg::ObjectClassification;
 
 template <class TargetMsg3D, class ObjType, class Msg2D>

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

@@ -0,0 +1,62 @@
+// Copyright 2024 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__MASK_CLUSTER_FUSION__NODE_HPP_
+#define AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__MASK_CLUSTER_FUSION__NODE_HPP_
+
+#include "autoware/image_projection_based_fusion/fusion_node.hpp"
+
+#include <autoware/image_projection_based_fusion/utils/utils.hpp>
+
+#include <cv_bridge/cv_bridge.h>
+
+#include <string>
+#include <vector>
+
+namespace autoware::image_projection_based_fusion
+{
+struct MatchedCluster
+{
+  uint32_t mask_index{};
+  uint32_t cluster_index{};
+  uint32_t valid_point_number{};
+  sensor_msgs::msg::RegionOfInterest roi;
+};
+using MatchedClusters = std::vector<MatchedCluster>;
+
+class MaskClusterFusionNode
+: public FusionNode<DetectedObjectsWithFeature, DetectedObjectWithFeature, SegmentationMask>
+{
+public:
+  explicit MaskClusterFusionNode(const rclcpp::NodeOptions & options);
+
+protected:
+  void preprocess(DetectedObjectsWithFeature & output_msg) override;
+
+  void postprocess(DetectedObjectsWithFeature & output_msg) override;
+
+  void fuseOnSingleImage(
+    const DetectedObjectsWithFeature & input_cluster_msg, const std::size_t image_id,
+    const SegmentationMask & input_mask_msg, const sensor_msgs::msg::CameraInfo & camera_info,
+    DetectedObjectsWithFeature & output_object_msg) override;
+
+  bool out_of_scope(const DetectedObjectWithFeature & obj) override;
+
+  double fusion_distance_;
+  double fusion_ratio_;
+  bool remove_unknown_;
+};
+}  // namespace autoware::image_projection_based_fusion
+
+#endif  // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__MASK_CLUSTER_FUSION__NODE_HPP_

perception/autoware_image_projection_based_fusion/launch/mask_cluster_fusion.launch.xml

@@ -0,0 +1,72 @@
+<launch>
+  <arg name="input/rois_number" default="6"/>
+  <arg name="input/rois0" default="/perception/object_recognition/detection/mask0"/>
+  <arg name="input/camera_info0" default="/sensing/camera/traffic_light0/camera_info"/>
+  <arg name="input/rois1" default="/perception/object_recognition/detection/mask1"/>
+  <arg name="input/camera_info1" default="/sensing/camera/traffic_light1/camera_info"/>
+  <arg name="input/rois2" default="/perception/object_recognition/detection/mask2"/>
+  <arg name="input/camera_info2" default="/sensing/camera/traffic_light2/camera_info"/>
+  <arg name="input/rois3" default="/perception/object_recognition/detection/mask3"/>
+  <arg name="input/camera_info3" default="/sensing/camera/traffic_light3/camera_info"/>
+  <arg name="input/rois4" default="/perception/object_recognition/detection/mask4"/>
+  <arg name="input/camera_info4" default="/sensing/camera/traffic_light4/camera_info"/>
+  <arg name="input/rois5" default="/perception/object_recognition/detection/mask5"/>
+  <arg name="input/camera_info5" default="/sensing/camera/traffic_light5/camera_info"/>
+  <arg name="input/rois6" default="/perception/object_recognition/detection/mask6"/>
+  <arg name="input/camera_info6" default="/sensing/camera/traffic_light6/camera_info"/>
+  <arg name="input/rois7" default="/perception/object_recognition/detection/mask7"/>
+  <arg name="input/camera_info7" default="/sensing/camera/traffic_light7/camera_info"/>
+  <arg name="input/clusters" default="/perception/object_recognition/detection/clustering/clusters"/>
+  <arg name="output/clusters" default="/perception/object_recognition/detection/clustering/camera_lidar_fusion/clusters"/>
+  <arg name="param_path" default="$(find-pkg-share autoware_image_projection_based_fusion)/config/mask_cluster_fusion.param.yaml"/>
+  <arg name="sync_param_path" default="$(find-pkg-share autoware_image_projection_based_fusion)/config/fusion_common.param.yaml"/>
+
+  <!-- for eval variable-->
+  <arg name="input_rois_number" default="$(var input/rois_number)"/>
+
+  <!-- debug -->
+  <arg name="input/image0" default="/image_raw0"/>
+  <arg name="input/image1" default="/image_raw1"/>
+  <arg name="input/image2" default="/image_raw2"/>
+  <arg name="input/image3" default="/image_raw3"/>
+  <arg name="input/image4" default="/image_raw4"/>
+  <arg name="input/image5" default="/image_raw5"/>
+  <arg name="input/image6" default="/image_raw6"/>
+  <arg name="input/image7" default="/image_raw7"/>
+
+  <group>
+    <node pkg="autoware_image_projection_based_fusion" exec="mask_cluster_fusion_node" name="mask_cluster_fusion" output="screen">
+      <param name="rois_number" value="$(var input/rois_number)"/>
+      <param from="$(var sync_param_path)"/>
+      <param from="$(var param_path)"/>
+      <remap from="input" to="$(var input/clusters)"/>
+      <remap from="output" to="$(var output/clusters)"/>
+
+      <!-- mask, camera and info -->
+      <param name="input/rois0" value="$(var input/rois0)"/>
+      <param name="input/camera_info0" value="$(var input/camera_info0)"/>
+      <param name="input/image0" value="$(var input/image0)"/>
+      <param name="input/rois1" value="$(var input/rois1)"/>
+      <param name="input/camera_info1" value="$(var input/camera_info1)"/>
+      <param name="input/image1" value="$(var input/image1)"/>
+      <param name="input/rois2" value="$(var input/rois2)"/>
+      <param name="input/camera_info2" value="$(var input/camera_info2)"/>
+      <param name="input/image2" value="$(var input/image2)"/>
+      <param name="input/rois3" value="$(var input/rois3)"/>
+      <param name="input/camera_info3" value="$(var input/camera_info3)"/>
+      <param name="input/image3" value="$(var input/image3)"/>
+      <param name="input/rois4" value="$(var input/rois4)"/>
+      <param name="input/camera_info4" value="$(var input/camera_info4)"/>
+      <param name="input/image4" value="$(var input/image4)"/>
+      <param name="input/rois5" value="$(var input/rois5)"/>
+      <param name="input/camera_info5" value="$(var input/camera_info5)"/>
+      <param name="input/image5" value="$(var input/image5)"/>
+      <param name="input/rois6" value="$(var input/rois6)"/>
+      <param name="input/camera_info6" value="$(var input/camera_info6)"/>
+      <param name="input/image6" value="$(var input/image6)"/>
+      <param name="input/rois7" value="$(var input/rois7)"/>
+      <param name="input/camera_info7" value="$(var input/camera_info7)"/>
+      <param name="input/image7" value="$(var input/image7)"/>
+    </node>
+  </group>
+</launch>

perception/autoware_image_projection_based_fusion/package.xml

@@ -17,6 +17,7 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
   <depend>autoware_euclidean_cluster</depend>
+  <depend>autoware_internal_msgs</depend>
   <depend>autoware_lidar_centerpoint</depend>
   <depend>autoware_object_recognition_utils</depend>
   <depend>autoware_perception_msgs</depend>

perception/autoware_image_projection_based_fusion/src/fusion_node.cpp

@@ -439,4 +439,5 @@ template class FusionNode<
 template class FusionNode<PointCloud2, DetectedObjects, DetectedObjectsWithFeature>;
 template class FusionNode<PointCloud2, DetectedObjectWithFeature, DetectedObjectsWithFeature>;
 template class FusionNode<PointCloud2, PointCloud2, Image>;
+template class FusionNode<DetectedObjectsWithFeature, DetectedObjectWithFeature, SegmentationMask>;
 }  // namespace autoware::image_projection_based_fusion