feat: cherry-pick and add image_segmentation_based_filter for beta/v0…

….29.0 (#1526) * feat(tensorrt yolox): inference and publish mask image from yolox model with semantic segmentation header (autowarefoundation#5553) * add segmentation model Signed-off-by: badai-nguyen <[email protected]> fix: add multitask for segment Signed-off-by: badai-nguyen <[email protected]> * fix: publish mask Signed-off-by: badai-nguyen <[email protected]> * feat: publish colorized mask Signed-off-by: badai-nguyen <[email protected]> * fix: resize yolox mask Signed-off-by: badai-nguyen <[email protected]> * fix: add memory allocate operations Signed-off-by: Manato HIRABAYASHI <[email protected]> * refactor: remove underscore for a local variable Signed-off-by: Manato HIRABAYASHI <[email protected]> * chore: add condition to check the number of subscriber for newly added topics Signed-off-by: Manato HIRABAYASHI <[email protected]> * chore: pre-commit Signed-off-by: badai-nguyen <[email protected]> * fix: add roi overlapping segment Signed-off-by: badai-nguyen <[email protected]> * fix: roi overlay segment Signed-off-by: badai-nguyen <[email protected]> * chore: refactor Signed-off-by: badai-nguyen <[email protected]> * docs: update readme Signed-off-by: badai-nguyen <[email protected]> * fix: update model name Signed-off-by: badai-nguyen <[email protected]> * fix: add utils into tensorrt_yolox Signed-off-by: badai-nguyen <[email protected]> * fix: launch file Signed-off-by: badai-nguyen <[email protected]> * chore: remove unnecessary depend Signed-off-by: badai-nguyen <[email protected]> * chore: fix yaml file Signed-off-by: badai-nguyen <[email protected]> * chore: remove duplicated param in launch Signed-off-by: badai-nguyen <[email protected]> * fix: semantic class Signed-off-by: badai-nguyen <[email protected]> * docs: update readme Signed-off-by: badai-nguyen <[email protected]> * chore: update default param Signed-off-by: badai-nguyen <[email protected]> * fix: add processing time topic Signed-off-by: badai-nguyen <[email protected]> * chore: typo Signed-off-by: badai-nguyen <[email protected]> * style(pre-commit): autofix * chore: fix cspell error Signed-off-by: badai-nguyen <[email protected]> * fix: yolox default param Signed-off-by: badai-nguyen <[email protected]> * chore: rename debug topics Signed-off-by: badai-nguyen <[email protected]> * chore: rename debug topics Signed-off-by: badai-nguyen <[email protected]> * docs: update model description Signed-off-by: badai-nguyen <[email protected]> * style(pre-commit): autofix * fix: launch Signed-off-by: badai-nguyen <[email protected]> * refactor: unpublish mask for single task Signed-off-by: badai-nguyen <[email protected]> * Update perception/tensorrt_yolox/src/tensorrt_yolox.cpp Co-authored-by: Manato Hirabayashi <[email protected]> * Update perception/tensorrt_yolox/src/tensorrt_yolox.cpp Co-authored-by: Manato Hirabayashi <[email protected]> * Update perception/tensorrt_yolox/src/tensorrt_yolox.cpp Co-authored-by: Manato Hirabayashi <[email protected]> * style(pre-commit): autofix * docs: update reamde Signed-off-by: badai-nguyen <[email protected]> * style(pre-commit): autofix * fix: skip mask size as yolox output Signed-off-by: badai-nguyen <[email protected]> --------- Signed-off-by: badai-nguyen <[email protected]> Signed-off-by: Manato HIRABAYASHI <[email protected]> Co-authored-by: Manato HIRABAYASHI <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Manato Hirabayashi <[email protected]> * fix(tensorrt_yolox): add run length encoding for sematic segmentation mask (autowarefoundation#7905) * fix: add rle compress Signed-off-by: badai-nguyen <[email protected]> * fix: rle compress Signed-off-by: badai-nguyen <[email protected]> * fix: move rle into utils Signed-off-by: badai-nguyen <[email protected]> * chore: pre-commit Signed-off-by: badai-nguyen <[email protected]> * Update perception/autoware_tensorrt_yolox/src/utils.cpp Co-authored-by: Yukihiro Saito <[email protected]> * fix: remove unused variable Signed-off-by: badai-nguyen <[email protected]> * Update perception/autoware_tensorrt_yolox/src/utils.cpp Co-authored-by: Manato Hirabayashi <[email protected]> * style(pre-commit): autofix * feat: add unit test for utils Signed-off-by: badai-nguyen <[email protected]> * style(pre-commit): autofix * fix: unit test Signed-off-by: badai-nguyen <[email protected]> * chore: change to explicit index Signed-off-by: badai-nguyen <[email protected]> * style(pre-commit): autofix * fix: cuda cmake Signed-off-by: badai-nguyen <[email protected]> * fix: separate unit test into different PR Signed-off-by: badai-nguyen <[email protected]> --------- Signed-off-by: badai-nguyen <[email protected]> Co-authored-by: Yukihiro Saito <[email protected]> Co-authored-by: Manato Hirabayashi <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * refactor(tensorrt_yolox): move utils into perception_utils (autowarefoundation#8435) * chore(tensorrt_yolo): refactor utils Signed-off-by: badai-nguyen <[email protected]> * style(pre-commit): autofix * fix: tensorrt_yolox Signed-off-by: badai-nguyen <[email protected]> --------- Signed-off-by: badai-nguyen <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix(perception_utils): install library after build (autowarefoundation#8501) Signed-off-by: Manato HIRABAYASHI <[email protected]> * fix(image_projection_based_fusion): resize sematic segmentation mask as input image size (autowarefoundation#7635) Signed-off-by: badai-nguyen <[email protected]> * fix(image_projection_based_fusion): segmentation pointcloud fusion param update (autowarefoundation#7858) * fix(image_projection_based_fusion): add run length decoding for segmentation_pointcloud_fusion (autowarefoundation#7909) * fix: add rle decompress Signed-off-by: badai-nguyen <[email protected]> * style(pre-commit): autofix * fix: use rld in tensorrt utils Signed-off-by: badai-nguyen <[email protected]> * fix: rebase error Signed-off-by: badai-nguyen <[email protected]> * fix: dependency Signed-off-by: badai-nguyen <[email protected]> * fix: skip publish debug mask Signed-off-by: badai-nguyen <[email protected]> * Update perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp Co-authored-by: kminoda <[email protected]> * style(pre-commit): autofix * Revert "fix: skip publish debug mask" This reverts commit 30fa9ae. --------- Signed-off-by: badai-nguyen <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: kminoda <[email protected]> * feat(tier4_perception_launch): add image segmentation based pointcloud filter (autowarefoundation#7225) * feat(tier4_perception_launch): add image segmentation based pointcloud filter Signed-off-by: badai-nguyen <[email protected]> * chore: typo Signed-off-by: badai-nguyen <[email protected]> * fix: detection launch Signed-off-by: badai-nguyen <[email protected]> * chore: add maintainer Signed-off-by: badai-nguyen <[email protected]> * Revert "chore: add maintainer" This reverts commit 5adfef6. --------- Signed-off-by: badai-nguyen <[email protected]> * style(pre-commit): autofix * fix(autoware_image_projection_based_fusion): resolve issue with segmentation pointcloud fusion node failing with multiple mask inputs (autowarefoundation#8769) --------- Signed-off-by: badai-nguyen <[email protected]> Signed-off-by: Manato HIRABAYASHI <[email protected]> Co-authored-by: Manato HIRABAYASHI <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Manato Hirabayashi <[email protected]> Co-authored-by: Yukihiro Saito <[email protected]> Co-authored-by: kminoda <[email protected]> Co-authored-by: Yi-Hsiang Fang (Vivid) <[email protected]>
tier4 · Sep 10, 2024 · 2077fae · 2077fae
1 parent 79dc610
commit 2077fae
Show file tree

Hide file tree

Showing 25 changed files with 976 additions and 117 deletions.
diff --git a/common/perception_utils/CMakeLists.txt b/common/perception_utils/CMakeLists.txt
@@ -4,4 +4,13 @@ project(perception_utils)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/run_length_encoder.cpp
+)
+
+find_package(OpenCV REQUIRED)
+target_link_libraries(${PROJECT_NAME}
+  ${OpenCV_LIBS}
+)
+
 ament_auto_package()
diff --git a/common/perception_utils/include/perception_utils/run_length_encoder.hpp b/common/perception_utils/include/perception_utils/run_length_encoder.hpp
@@ -0,0 +1,29 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// 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 PERCEPTION_UTILS__RUN_LENGTH_ENCODER_HPP_
+
+#define PERCEPTION_UTILS__RUN_LENGTH_ENCODER_HPP_
+#include <opencv2/opencv.hpp>
+
+#include <utility>
+#include <vector>
+
+namespace perception_utils
+{
+std::vector<std::pair<uint8_t, int>> runLengthEncoder(const cv::Mat & mask);
+cv::Mat runLengthDecoder(const std::vector<uint8_t> & rle_data, const int rows, const int cols);
+}  // namespace perception_utils
+
+#endif  // PERCEPTION_UTILS__RUN_LENGTH_ENCODER_HPP_
diff --git a/common/perception_utils/package.xml b/common/perception_utils/package.xml
@@ -12,6 +12,7 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <depend>libopencv-dev</depend>
   <depend>rclcpp</depend>
 
   <export>

diff --git a/common/perception_utils/src/run_length_encoder.cpp b/common/perception_utils/src/run_length_encoder.cpp
@@ -0,0 +1,65 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// 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.
+
+#include "perception_utils/run_length_encoder.hpp"
+
+namespace perception_utils
+{
+
+std::vector<std::pair<uint8_t, int>> runLengthEncoder(const cv::Mat & image)
+{
+  std::vector<std::pair<uint8_t, int>> compressed_data;
+  const int rows = image.rows;
+  const int cols = image.cols;
+  compressed_data.emplace_back(image.at<uint8_t>(0, 0), 0);
+  for (int i = 0; i < rows; ++i) {
+    for (int j = 0; j < cols; ++j) {
+      uint8_t current_value = image.at<uint8_t>(i, j);
+      if (compressed_data.back().first == current_value) {
+        ++compressed_data.back().second;
+      } else {
+        compressed_data.emplace_back(current_value, 1);
+      }
+    }
+  }
+  return compressed_data;
+}
+
+cv::Mat runLengthDecoder(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);
+  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 + 1],
+      sizeof(
+        int));  // under the condition that we know rle_data[i] only consume 1 element of the vector
+    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) {
+        break;
+      }
+    }
+  }
+  return mask;
+}
+
+}  // namespace perception_utils
diff --git a/common/tensorrt_common/include/tensorrt_common/tensorrt_common.hpp b/common/tensorrt_common/include/tensorrt_common/tensorrt_common.hpp
@@ -15,7 +15,9 @@
 #ifndef TENSORRT_COMMON__TENSORRT_COMMON_HPP_
 #define TENSORRT_COMMON__TENSORRT_COMMON_HPP_
 
+#ifndef YOLOX_STANDALONE
 #include <rclcpp/rclcpp.hpp>
+#endif
 
 #include <NvInfer.h>
 #include <NvOnnxParser.h>
@@ -86,6 +88,7 @@ struct BuildConfig
     profile_per_layer(profile_per_layer),
     clip_value(clip_value)
   {
+#ifndef YOLOX_STANDALONE
     if (
       std::find(valid_calib_type.begin(), valid_calib_type.end(), calib_type_str) ==
       valid_calib_type.end()) {
@@ -95,6 +98,7 @@ struct BuildConfig
               << "Default calibration type will be used: MinMax" << std::endl;
       std::cerr << message.str();
     }
+#endif
   }
 };
 

diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml
@@ -3,6 +3,7 @@
   <!-- Interface parameters -->
   <arg name="mode" default="lidar" description="options: `camera_lidar_radar_fusion`, `camera_lidar_fusion`, `lidar_radar_fusion`, `lidar` or `radar`"/>
   <arg name="use_detection_by_tracker"/>
+  <arg name="use_image_segmentation_based_filter"/>
 
   <!-- Lidar parameters -->
   <arg name="input/pointcloud"/>
@@ -60,6 +61,7 @@
       <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
       <arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>
       <arg name="detection_by_tracker_param_path" value="$(var detection_by_tracker_param_path)"/>
+      <arg name="use_image_segmentation_based_filter" value="$(var use_image_segmentation_based_filter)"/>
     </include>
     <!-- Lidar dnn-based detectors-->
     <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml">

diff --git a/...tion_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml b/...tion_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml
@@ -9,6 +9,7 @@
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
   <arg name="use_roi_based_cluster" default="false"/>
   <arg name="use_low_intensity_cluster_filter" default="false"/>
+  <arg name="use_image_segmentation_based_filter" default="false"/>
 
   <!-- Camera parameters -->
   <arg name="image_raw0" default="/image_raw" description="image raw topic name"/>
@@ -104,14 +105,24 @@
     </include>
   </group>
 
+  <!-- Image_segmentation based filter, apply for camera0 only-->
+  <group>
+    <include file="$(find-pkg-share image_projection_based_fusion)/launch/segmentation_pointcloud_fusion.launch.xml" if="$(var use_image_segmentation_based_filter)">
+      <arg name="input/pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
+      <arg name="output/pointcloud" value="/perception/object_recognition/detection/segmentation_based_filtered/pointcloud"/>
+    </include>
+  </group>
+
   <!-- Clustering -->
   <group>
     <push-ros-namespace namespace="clustering"/>
     <group>
+      <let name="euclidean_cluster_input" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud" unless="$(var use_image_segmentation_based_filter)"/>
+      <let name="euclidean_cluster_input" value="/perception/object_recognition/detection/segmentation_based_filtered/pointcloud" if="$(var use_image_segmentation_based_filter)"/>
       <let name="euclidean_cluster_output" value="euclidean_cluster/clusters" if="$(var use_roi_based_cluster)"/>
       <let name="euclidean_cluster_output" value="clusters" unless="$(var use_roi_based_cluster)"/>
       <include file="$(find-pkg-share euclidean_cluster)/launch/voxel_grid_based_euclidean_cluster.launch.xml">
-        <arg name="input_pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
+        <arg name="input_pointcloud" value="$(var euclidean_cluster_input)"/>
         <arg name="output_clusters" value="$(var euclidean_cluster_output)"/>
         <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
       </include>

diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -79,6 +79,7 @@
   <arg name="use_object_filter" default="true" description="use object filter"/>
   <arg name="use_roi_based_cluster" default="true" description="use roi_based_cluster in clustering"/>
   <arg name="use_low_intensity_cluster_filter" default="true" description="use low_intensity_cluster_filter in clustering"/>
+  <arg name="use_image_segmentation_based_filter" default="true" description="use image_segmentation_based_filter in clustering"/>
   <arg
     name="use_empty_dynamic_object_publisher"
     default="false"
@@ -221,6 +222,7 @@
           <arg name="use_radar_tracking_fusion" value="$(var use_radar_tracking_fusion)"/>
           <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
           <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
+          <arg name="use_image_segmentation_based_filter" value="$(var use_image_segmentation_based_filter)"/>
           <arg name="use_object_filter" value="$(var use_object_filter)"/>
           <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
           <arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>

diff --git a/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml
@@ -1,39 +1,27 @@
 /**:
   ros__parameters:
     # if the semantic label is applied for pointcloud filtering
+
     filter_semantic_label_target:
-      [
-      true, # road
-      true, # sidewalk
-      true, # building
-      true, # wall
-      true, # fence
-      true, # pole
-      true, # traffic_light
-      true, # traffic_sign
-      true, # vegetation
-      true, # terrain
-      true, # sky
-      false, # person
-      false, # ride
-      false, # car
-      false, # truck
-      false, # bus
-      false, # train
-      false, # motorcycle
-      false, # bicycle
-      false, # others
-      ]
-    # the maximum distance of pointcloud to be applied filter,
-    # this is selected based on semantic segmentation model accuracy,
-    # calibration accuracy and unknown reaction distance
+      UNKNOWN: false
+      BUILDING: true
+      WALL: true
+      OBSTACLE: false
+      TRAFFIC_LIGHT: false
+      TRAFFIC_SIGN: false
+      PERSON: false
+      VEHICLE: false
+      BIKE: false
+      ROAD: true
+      SIDEWALK: false
+      ROAD_PAINT: false
+      CURBSTONE: false
+      CROSSWALK: false
+      VEGETATION: true
+      SKY: false
+
+    # the maximum distance of pointcloud to be applied filter
     filter_distance_threshold: 60.0
 
-    # debug
-    debug_mode: false
-    filter_scope_min_x: -100.0
-    filter_scope_max_x: 100.0
-    filter_scope_min_y: -100.0
-    filter_scope_max_y: 100.0
-    filter_scope_min_z: -100.0
-    filter_scope_max_z: 100.0
+    # Avoid using debug mask in case of multiple camera semantic segmentation fusion
+    is_publish_debug_mask: false
diff --git a/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md b/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md
@@ -32,9 +32,7 @@ The node `segmentation_pointcloud_fusion` is a package for filtering pointcloud
 
 ### Core Parameters
 
-| Name          | Type | Description              |
-| ------------- | ---- | ------------------------ |
-| `rois_number` | int  | the number of input rois |
+{{ json_to_markdown("perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json") }}
 
 ## Assumptions / Known limits
 

diff --git a/...ased_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/...ased_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp
@@ -17,7 +17,11 @@
 
 #include "image_projection_based_fusion/fusion_node.hpp"
 
+#include <image_transport/image_transport.hpp>
+
 #include <string>
+#include <unordered_set>
+#include <utility>
 #include <vector>
 
 #if __has_include(<cv_bridge/cv_bridge.hpp>)
@@ -34,7 +38,17 @@ class SegmentPointCloudFusionNode : public FusionNode<PointCloud2, PointCloud2,
   rclcpp::Publisher<PointCloud2>::SharedPtr pub_pointcloud_ptr_;
   std::vector<bool> filter_semantic_label_target_;
   float filter_distance_threshold_;
-  /* data */
+  // declare list of semantic label target, depend on trained data of yolox segmentation model
+  std::vector<std::pair<std::string, bool>> filter_semantic_label_target_list_ = {
+    {"UNKNOWN", false},       {"BUILDING", false},     {"WALL", false},       {"OBSTACLE", false},
+    {"TRAFFIC_LIGHT", false}, {"TRAFFIC_SIGN", false}, {"PERSON", false},     {"VEHICLE", false},
+    {"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_;
+  std::unordered_set<size_t> filter_global_offset_set_;
+
 public:
   explicit SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options);
 

diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml
@@ -27,6 +27,7 @@
   <depend>object_recognition_utils</depend>
   <depend>pcl_conversions</depend>
   <depend>pcl_ros</depend>
+  <depend>perception_utils</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>sensor_msgs</depend>