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,4 @@
 
     # the maximum distance of pointcloud to be applied filter
     filter_distance_threshold: 60.0
+    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

@@ -20,6 +20,7 @@
   <depend>autoware_lidar_centerpoint</depend>
   <depend>autoware_perception_msgs</depend>
   <depend>autoware_point_types</depend>
+  <depend>autoware_tensorrt_yolox</depend>
   <depend>autoware_universe_utils</depend>
   <depend>cv_bridge</depend>
   <depend>image_geometry</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

@@ -16,6 +16,7 @@
 
 #include "autoware/image_projection_based_fusion/utils/geometry.hpp"
 #include "autoware/image_projection_based_fusion/utils/utils.hpp"
+#include "autoware/tensorrt_yolox/utils.hpp"
 
 #ifdef ROS_DISTRO_GALACTIC
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
@@ -38,6 +39,8 @@ SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptio
     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)
@@ -57,19 +60,23 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage(
   if (input_pointcloud_msg.data.empty()) {
     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;
+  cv::Mat mask = autoware::tensorrt_yolox::runLengthDecoder(
+    input_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);
   }
+  Eigen::Matrix4d projection;
+  projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(3),
+    camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), camera_info.p.at(7),
+    camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11), 0.0, 0.0,
+    0.0, 1.0;
   const int orig_width = camera_info.width;
   const int orig_height = camera_info.height;
   // resize mask to the same size as the camera image