fix: use rld in tensorrt utils

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

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,11 +45,13 @@ 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);
 
 protected:
-  cv::Mat rle_decompress(const std::vector<uint8_t> & rle_data, const int rows, const int cols);
   void preprocess(PointCloud2 & pointcloud_msg) override;
 
   void postprocess(PointCloud2 & pointcloud_msg) override;

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)
@@ -49,32 +52,6 @@ void SegmentPointCloudFusionNode::postprocess(__attribute__((unused)) PointCloud
 {
   return;
 }
-cv::Mat SegmentPointCloudFusionNode::rle_decompress(
-  const std::vector<uint8_t> & rle_data, const int rows, const int cols)
-{
-  cv::Mat mask(rows, cols, CV_8UC1, cv::Scalar(0));
-  int idx = 0;
-  int step = sizeof(uint8_t) + sizeof(int);
-  int nb_pixels = 0;
-  for (size_t i = 0; i < rle_data.size(); i += step) {
-    uint8_t value;
-    int length;
-    std::memcpy(&value, &rle_data[i], sizeof(uint8_t));
-    std::memcpy(&length, &rle_data[i + sizeof(uint8_t)], sizeof(int));
-    nb_pixels += length;
-    for (int j = 0; j < length; ++j) {
-      int row_idx = static_cast<int>(idx / cols);
-      int col_idx = static_cast<int>(idx % cols);
-      mask.at<uint8_t>(row_idx, col_idx) = value;
-      idx++;
-      if (idx > rows * cols) {
-        RCLCPP_ERROR(this->get_logger(), "RLE decompression error: idx out of bound");
-        break;
-      }
-    }
-  }
-  return mask;
-}
 void SegmentPointCloudFusionNode::fuseOnSingleImage(
   const PointCloud2 & input_pointcloud_msg, __attribute__((unused)) const std::size_t image_id,
   [[maybe_unused]] const Image & input_mask, __attribute__((unused)) const CameraInfo & camera_info,
@@ -86,9 +63,20 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage(
   if (input_mask.height == 0 || input_mask.width == 0) {
     return;
   }
-  cv::Mat mask = SegmentPointCloudFusionNode::rle_decompress(
+  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