feat(image_projection_based_fusion): add image segmentation_pointclou…

…d_fusion for pointcloud filter (#5562) * fix: roiCallback for Image Signed-off-by: badai-nguyen <[email protected]> * fix: pointcloud filter Signed-off-by: badai-nguyen <[email protected]> * fix: pointcloud filter bugfix Signed-off-by: badai-nguyen <[email protected]> * fix: segmentation poincloud filter Signed-off-by: badai-nguyen <[email protected]> * fix: add option in launch file Signed-off-by: badai-nguyen <[email protected]> * chore: refactor Signed-off-by: badai-nguyen <[email protected]> * docs: update docs Signed-off-by: badai-nguyen <[email protected]> * fix: roi_pointcloud_fusion with new template Signed-off-by: badai-nguyen <[email protected]> * fix: add param of selectable semantic id Signed-off-by: badai-nguyen <[email protected]> * chore: typo Signed-off-by: badai-nguyen <[email protected]> * fix: launch file Signed-off-by: badai-nguyen <[email protected]> * Revert "fix: launch file" This reverts commit 5dfa160. * fix: reverse perception launcher Signed-off-by: badai-nguyen <[email protected]> * style(pre-commit): autofix * Update perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp Co-authored-by: Yoshi Ri <[email protected]> * style(pre-commit): autofix --------- 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: Yoshi Ri <[email protected]>
autowarefoundation · Jan 9, 2024 · 39e9acc · 39e9acc
1 parent 618ad01
commit 39e9acc
Show file tree

Hide file tree

Showing 16 changed files with 455 additions and 41 deletions.
diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt
@@ -22,6 +22,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/utils/utils.cpp
   src/roi_cluster_fusion/node.cpp
   src/roi_detected_object_fusion/node.cpp
+  src/segmentation_pointcloud_fusion/node.cpp
   src/roi_pointcloud_fusion/node.cpp
 )
 
@@ -40,6 +41,11 @@ rclcpp_components_register_node(${PROJECT_NAME}
   EXECUTABLE roi_cluster_fusion_node
 )
 
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "image_projection_based_fusion::SegmentPointCloudFusionNode"
+  EXECUTABLE segmentation_pointcloud_fusion_node
+)
+
 rclcpp_components_register_node(${PROJECT_NAME}
   PLUGIN "image_projection_based_fusion::RoiPointCloudFusionNode"
   EXECUTABLE roi_pointcloud_fusion_node

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
@@ -0,0 +1,30 @@
+/**:
+  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
+    filter_distance_threshold: 60.0
diff --git a/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md b/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md
@@ -0,0 +1,87 @@
+# segmentation_pointcloud_fusion
+
+## Purpose
+
+The node `segmentation_pointcloud_fusion` is a package for filtering pointcloud that are belong to less interesting region which is defined by semantic or instance segmentation by 2D image segmentation model.
+
+## Inner-workings / Algorithms
+
+- The pointclouds are projected onto the semantic/instance segmentation mask image which is the output from 2D segmentation neural network.
+- The pointclouds are belong to segment such as road, sidewalk, building, vegetation, sky ... are considered as less important points for autonomous driving and could be removed.
+
+![segmentation_pointcloud_fusion_image](./images/segmentation_pointcloud_fusion.png)
+
+## Inputs / Outputs
+
+### Input
+
+| Name                     | Type                                | Description                                               |
+| ------------------------ | ----------------------------------- | --------------------------------------------------------- |
+| `input`                  | `sensor_msgs::msg::PointCloud2`     | input pointcloud                                          |
+| `input/camera_info[0-7]` | `sensor_msgs::msg::CameraInfo`      | camera information to project 3d points onto image planes |
+| `input/rois[0-7]`        | `tier4_perception_msgs::msg::Image` | semantic segmentation mask image                          |
+| `input/image_raw[0-7]`   | `sensor_msgs::msg::Image`           | images for visualization                                  |
+
+### Output
+
+| Name     | Type                            | Description                |
+| -------- | ------------------------------- | -------------------------- |
+| `output` | `sensor_msgs::msg::PointCloud2` | output filtered pointcloud |
+
+## Parameters
+
+### Core Parameters
+
+| Name          | Type | Description              |
+| ------------- | ---- | ------------------------ |
+| `rois_number` | int  | the number of input rois |
+
+## Assumptions / Known limits
+
+<!-- Write assumptions and limitations of your implementation.
+
+Example:
+  This algorithm assumes obstacles are not moving, so if they rapidly move after the vehicle started to avoid them, it might collide with them.
+  Also, this algorithm doesn't care about blind spots. In general, since too close obstacles aren't visible due to the sensing performance limit, please take enough margin to obstacles.
+-->
+
+## (Optional) Error detection and handling
+
+<!-- Write how to detect errors and how to recover from them.
+
+Example:
+  This package can handle up to 20 obstacles. If more obstacles found, this node will give up and raise diagnostic errors.
+-->
+
+## (Optional) Performance characterization
+
+<!-- Write performance information like complexity. If it wouldn't be the bottleneck, not necessary.
+
+Example:
+
+  ### Complexity
+
+  This algorithm is O(N).
+
+  ### Processing time
+
+  ...
+-->
+
+## (Optional) References/External links
+
+<!-- Write links you referred to when you implemented.
+
+Example:
+  [1] {link_to_a_thesis}
+  [2] {link_to_an_issue}
+-->
+
+## (Optional) Future extensions / Unimplemented parts
+
+<!-- Write future extensions of this package.
+
+Example:
+  Currently, this package can't handle the chattering obstacles well. We plan to add some probabilistic filters in the perception layer to improve it.
+  Also, there are some parameters that should be global(e.g. vehicle size, max steering, etc.). These will be refactored and defined as global parameters so that we can share the same parameters between different nodes.
+-->
diff --git a/...ption/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp b/...ption/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp
@@ -22,6 +22,7 @@
 
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 #include <sensor_msgs/msg/camera_info.hpp>
+#include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/point_cloud2_iterator.hpp>
 #include <tier4_perception_msgs/msg/detected_objects_with_feature.hpp>
 
@@ -49,13 +50,14 @@ namespace image_projection_based_fusion
 {
 using autoware_auto_perception_msgs::msg::DetectedObject;
 using autoware_auto_perception_msgs::msg::DetectedObjects;
+using sensor_msgs::msg::CameraInfo;
+using sensor_msgs::msg::Image;
 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_auto_perception_msgs::msg::ObjectClassification;
-
-template <class Msg, class ObjType>
+template <class Msg, class ObjType, class Msg2D>
 class FusionNode : public rclcpp::Node
 {
 public:
@@ -78,12 +80,12 @@ class FusionNode : public rclcpp::Node
   virtual void subCallback(const typename Msg::ConstSharedPtr input_msg);
 
   // callback for roi subscription
+
   virtual void roiCallback(
-    const DetectedObjectsWithFeature::ConstSharedPtr input_roi_msg, const std::size_t roi_i);
+    const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i);
 
   virtual void fuseOnSingleImage(
-    const Msg & input_msg, const std::size_t image_id,
-    const DetectedObjectsWithFeature & input_roi_msg,
+    const Msg & input_msg, const std::size_t image_id, const Msg2D & input_roi_msg,
     const sensor_msgs::msg::CameraInfo & camera_info, Msg & output_msg) = 0;
 
   // set args if you need
@@ -111,7 +113,7 @@ class FusionNode : public rclcpp::Node
 
   /** \brief A vector of subscriber. */
   typename rclcpp::Subscription<Msg>::SharedPtr sub_;
-  std::vector<rclcpp::Subscription<DetectedObjectsWithFeature>::SharedPtr> rois_subs_;
+  std::vector<typename rclcpp::Subscription<Msg2D>::SharedPtr> rois_subs_;
 
   // offsets between cameras and the lidars
   std::vector<double> input_offset_ms_;
@@ -120,7 +122,7 @@ class FusionNode : public rclcpp::Node
   std::vector<bool> is_fused_;
   std::pair<int64_t, typename Msg::SharedPtr>
     cached_msg_;  // first element is the timestamp in nanoseconds, second element is the message
-  std::vector<std::map<int64_t, DetectedObjectsWithFeature::ConstSharedPtr>> cached_roi_msgs_;
+  std::vector<std::map<int64_t, typename Msg2D::ConstSharedPtr>> cached_roi_msgs_;
   std::mutex mutex_cached_msgs_;
 
   // output publisher

diff --git a/...ojection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp b/...ojection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp
@@ -33,7 +33,8 @@ namespace image_projection_based_fusion
 {
 using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
 
-class PointPaintingFusionNode : public FusionNode<sensor_msgs::msg::PointCloud2, DetectedObjects>
+class PointPaintingFusionNode
+: public FusionNode<sensor_msgs::msg::PointCloud2, DetectedObjects, DetectedObjectsWithFeature>
 {
 public:
   explicit PointPaintingFusionNode(const rclcpp::NodeOptions & options);

diff --git a/...projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp b/...projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp
@@ -25,7 +25,8 @@ namespace image_projection_based_fusion
 const std::map<std::string, uint8_t> IOU_MODE_MAP{{"iou", 0}, {"iou_x", 1}, {"iou_y", 2}};
 
 class RoiClusterFusionNode
-: public FusionNode<DetectedObjectsWithFeature, DetectedObjectWithFeature>
+: public FusionNode<
+    DetectedObjectsWithFeature, DetectedObjectWithFeature, DetectedObjectsWithFeature>
 {
 public:
   explicit RoiClusterFusionNode(const rclcpp::NodeOptions & options);

diff --git a/...on_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp b/...on_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp
@@ -29,7 +29,8 @@ namespace image_projection_based_fusion
 
 using sensor_msgs::msg::RegionOfInterest;
 
-class RoiDetectedObjectFusionNode : public FusionNode<DetectedObjects, DetectedObject>
+class RoiDetectedObjectFusionNode
+: public FusionNode<DetectedObjects, DetectedObject, DetectedObjectsWithFeature>
 {
 public:
   explicit RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options);

diff --git a/...jection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp b/...jection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp
@@ -22,7 +22,7 @@
 namespace image_projection_based_fusion
 {
 class RoiPointCloudFusionNode
-: public FusionNode<sensor_msgs::msg::PointCloud2, DetectedObjectWithFeature>
+: public FusionNode<PointCloud2, DetectedObjectWithFeature, DetectedObjectsWithFeature>
 {
 private:
   int min_cluster_size_{1};

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
@@ -0,0 +1,54 @@
+// Copyright 2023 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 IMAGE_PROJECTION_BASED_FUSION__SEGMENTATION_POINTCLOUD_FUSION__NODE_HPP_
+#define IMAGE_PROJECTION_BASED_FUSION__SEGMENTATION_POINTCLOUD_FUSION__NODE_HPP_
+
+#include "image_projection_based_fusion/fusion_node.hpp"
+
+#include <string>
+#include <vector>
+
+#if __has_include(<cv_bridge/cv_bridge.hpp>)
+#include <cv_bridge/cv_bridge.hpp>
+#else
+#include <cv_bridge/cv_bridge.h>
+#endif
+
+namespace image_projection_based_fusion
+{
+class SegmentPointCloudFusionNode : public FusionNode<PointCloud2, PointCloud2, Image>
+{
+private:
+  rclcpp::Publisher<PointCloud2>::SharedPtr pub_pointcloud_ptr_;
+  std::vector<bool> filter_semantic_label_target_;
+  float filter_distance_threshold_;
+  /* data */
+public:
+  explicit SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options);
+
+protected:
+  void preprocess(PointCloud2 & pointcloud_msg) override;
+
+  void postprocess(PointCloud2 & pointcloud_msg) override;
+
+  void fuseOnSingleImage(
+    const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, const Image & input_mask,
+    const CameraInfo & camera_info, PointCloud2 & output_pointcloud_msg) override;
+
+  bool out_of_scope(const PointCloud2 & filtered_cloud);
+};
+
+}  // namespace image_projection_based_fusion
+#endif  // IMAGE_PROJECTION_BASED_FUSION__SEGMENTATION_POINTCLOUD_FUSION__NODE_HPP_
diff --git a/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml b/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml
@@ -0,0 +1,87 @@
+<launch>
+  <arg name="input/camera_number" default="1"/>
+  <arg name="input/mask0" default="/perception/object_recognition/detection/mask0"/>
+  <arg name="input/camera_info0" default="/sensing/camera/camera0/camera_info"/>
+  <arg name="input/mask1" default="/perception/object_recognition/detection/mask1"/>
+  <arg name="input/camera_info1" default="/sensing/camera/camera0/camera_info"/>
+  <arg name="input/mask2" default="/perception/object_recognition/detection/mask2"/>
+  <arg name="input/camera_info2" default="/sensing/camera/camera0/camera_info"/>
+  <arg name="input/mask3" default="/perception/object_recognition/detection/mask3"/>
+  <arg name="input/camera_info3" default="/sensing/camera/camera0/camera_info"/>
+  <arg name="input/mask4" default="/perception/object_recognition/detection/mask4"/>
+  <arg name="input/camera_info4" default="/sensing/camera/camera0/camera_info"/>
+  <arg name="input/mask5" default="/perception/object_recognition/detection/mask5"/>
+  <arg name="input/camera_info5" default="/sensing/camera/camera0/camera_info"/>
+  <arg name="input/mask6" default="/perception/object_recognition/detection/mask6"/>
+  <arg name="input/camera_info6" default="/sensing/camera/camera0/camera_info"/>
+  <arg name="input/mask7" default="/perception/object_recognition/detection/mask7"/>
+  <arg name="input/camera_info7" default="/sensing/camera/camera0/camera_info"/>
+  <arg name="input/pointcloud" default="/sensing/lidar/top/outlier_filtered/pointcloud"/>
+  <arg name="output/pointcloud" default="output/pointcloud"/>
+  <arg name="sync_param_path" default="$(find-pkg-share image_projection_based_fusion)/config/roi_sync.param.yaml"/>
+  <arg name="semantic_segmentation_based_filter_param_path" default="$(find-pkg-share image_projection_based_fusion)/config/segmentation_pointcloud_fusion.param.yaml"/>
+  <!-- debug -->
+  <!-- cspell: ignore minx, maxx, miny, maxy, minz, maxz  -->
+  <arg name="debug_mode" default="false"/>
+
+  <arg name="filter_scope_minx" default="-100"/>
+  <arg name="filter_scope_maxx" default="100"/>
+  <arg name="filter_scope_miny" default="-100"/>
+  <arg name="filter_scope_maxy" default="100"/>
+  <arg name="filter_scope_minz" default="-100"/>
+  <arg name="filter_scope_maxz" default="100"/>
+  <arg name="image_buffer_size" default="15"/>
+  <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="image_projection_based_fusion" exec="segmentation_pointcloud_fusion_node" name="segmentation_pointcloud_fusion" output="screen">
+      <param name="rois_number" value="$(var input/camera_number)"/>
+      <param from="$(var semantic_segmentation_based_filter_param_path)"/>
+      <param from="$(var sync_param_path)"/>
+      <remap from="input" to="$(var input/pointcloud)"/>
+      <remap from="output" to="$(var output/pointcloud)"/>
+
+      <!-- rois, camera and info -->
+      <param name="input/rois0" value="$(var input/mask0)"/>
+      <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/mask1)"/>
+      <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/mask2)"/>
+      <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/mask3)"/>
+      <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/mask4)"/>
+      <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/mask5)"/>
+      <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/mask6)"/>
+      <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/mask7)"/>
+      <param name="input/camera_info7" value="$(var input/camera_info7)"/>
+      <param name="input/image7" value="$(var input/image7)"/>
+
+      <!-- debug -->
+      <param name="debug_mode" value="$(var debug_mode)"/>
+      <param name="filter_scope_minx" value="$(var filter_scope_minx)"/>
+      <param name="filter_scope_maxx" value="$(var filter_scope_maxx)"/>
+      <param name="filter_scope_miny" value="$(var filter_scope_miny)"/>
+      <param name="filter_scope_maxy" value="$(var filter_scope_maxy)"/>
+      <param name="filter_scope_minz" value="$(var filter_scope_minz)"/>
+      <param name="filter_scope_maxz" value="$(var filter_scope_maxz)"/>
+      <param name="image_buffer_size" value="$(var image_buffer_size)"/>
+    </node>
+  </group>
+</launch>