fix(image_projection_based_fusion): add run length decoding for segmentation_pointcloud_fusion

perception/autoware_image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml

@@ -22,3 +22,6 @@
 
     # the maximum distance of pointcloud to be applied filter
     filter_distance_threshold: 60.0
+
+    # Avoid using debug mask in case of multiple camera semantic segmentation fusion
+    is_publish_debug_mask: false

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

@@ -18,6 +18,7 @@
 #include "autoware/image_projection_based_fusion/fusion_node.hpp"
 
 #include <autoware/image_projection_based_fusion/utils/utils.hpp>
+#include <image_transport/image_transport.hpp>
 
 #include <string>
 #include <utility>
@@ -44,6 +45,9 @@ class SegmentPointCloudFusionNode : public FusionNode<PointCloud2, PointCloud2,
     {"BIKE", false},          {"ROAD", false},         {"SIDEWALK", false},   {"ROAD_PAINT", false},
     {"CURBSTONE", false},     {"CROSSWALK", false},    {"VEGETATION", false}, {"SKY", false}};
 
+  image_transport::Publisher pub_debug_mask_ptr_;
+  bool is_publish_debug_mask_;
+
 public:
   explicit SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options);
 

perception/autoware_image_projection_based_fusion/package.xml

@@ -26,6 +26,7 @@
   <depend>image_transport</depend>
   <depend>message_filters</depend>
   <depend>object_recognition_utils</depend>
+  <depend>perception_utils</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>sensor_msgs</depend>

perception/autoware_image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json

@@ -114,9 +114,18 @@
           "description": "A maximum distance of pointcloud to apply filter [m].",
           "default": 60.0,
           "minimum": 0.0
+        },
+        "is_publish_debug_mask": {
+          "type": "boolean",
+          "description": "If true, debug mask image will be published.",
+          "default": false
         }
       },
-      "required": ["filter_semantic_label_target", "filter_distance_threshold"]
+      "required": [
+        "filter_semantic_label_target",
+        "filter_distance_threshold",
+        "is_publish_debug_mask"
+      ]
     }
   },
   "properties": {

perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp

@@ -17,6 +17,8 @@
 #include "autoware/image_projection_based_fusion/utils/geometry.hpp"
 #include "autoware/image_projection_based_fusion/utils/utils.hpp"
 
+#include <perception_utils/run_length_encoder.hpp>
+
 #ifdef ROS_DISTRO_GALACTIC
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <tf2_sensor_msgs/tf2_sensor_msgs.h>
@@ -38,6 +40,8 @@
     RCLCPP_INFO(
       this->get_logger(), "filter_semantic_label_target: %s %d", item.first.c_str(), item.second);
   }
+  is_publish_debug_mask_ = declare_parameter<bool>("is_publish_debug_mask");
+  pub_debug_mask_ptr_ = image_transport::create_publisher(this, "~/debug/mask");
 }
 
 void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg)
@@ -58,19 +62,18 @@
     return;
   }
   if (!checkCameraInfo(camera_info)) return;
-
-  cv_bridge::CvImagePtr in_image_ptr;
-  try {
-    in_image_ptr = cv_bridge::toCvCopy(
-      std::make_shared<sensor_msgs::msg::Image>(input_mask), input_mask.encoding);
-  } catch (const std::exception & e) {
-    RCLCPP_ERROR(this->get_logger(), "cv_bridge exception:%s", e.what());
+  if (input_mask.height == 0 || input_mask.width == 0) {
     return;
   }
-
-  cv::Mat mask = in_image_ptr->image;
-  if (mask.cols == 0 || mask.rows == 0) {
-    return;
+  std::vector<uint8_t> mask_data(input_mask.data.begin(), input_mask.data.end());
+  cv::Mat mask = perception_utils::runLengthDecoder(mask_data, input_mask.height, input_mask.width);
+
+  // publish debug mask
+  if (is_publish_debug_mask_) {
+    sensor_msgs::msg::Image::SharedPtr debug_mask_msg =
+      cv_bridge::CvImage(std_msgs::msg::Header(), "mono8", mask).toImageMsg();
+    debug_mask_msg->header = input_mask.header;
+    pub_debug_mask_ptr_.publish(debug_mask_msg);
   }
   const int orig_width = camera_info.width;
   const int orig_height = camera_info.height;