feat: add low_intensity_cluster_filter (#6850)

* feat: add low_intensity_cluster_filter Signed-off-by: badai-nguyen <[email protected]> * chore: typo Signed-off-by: badai-nguyen <[email protected]> * fix: build test error Signed-off-by: badai-nguyen <[email protected]> --------- Signed-off-by: badai-nguyen <[email protected]>
autowarefoundation · May 8, 2024 · e517f09 · e517f09
1 parent 6ba26e5
commit e517f09
Show file tree

Hide file tree

Showing 9 changed files with 384 additions and 1 deletion.
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
@@ -8,6 +8,7 @@
   <!-- Lidar + Camera detector parameters -->
   <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"/>
 
   <!-- Camera parameters -->
   <arg name="image_raw0" default="/image_raw" description="image raw topic name"/>
@@ -208,9 +209,19 @@
         </include>
       </group>
 
+      <!-- low_intensity_cluster_filter -->
       <group>
-        <include file="$(find-pkg-share shape_estimation)/launch/shape_estimation.launch.xml">
+        <include file="$(find-pkg-share raindrop_cluster_filter)/launch/low_intensity_cluster_filter.launch.xml" if="$(var use_low_intensity_cluster_filter)">
           <arg name="input/objects" value="clusters"/>
+          <arg name="output/objects" value="filtered/clusters"/>
+        </include>
+      </group>
+
+      <group>
+        <let name="shape_estimation/input" value="filtered/clusters" if="$(var use_low_intensity_cluster_filter)"/>
+        <let name="shape_estimation/input" value="clusters" unless="$(var use_low_intensity_cluster_filter)"/>
+        <include file="$(find-pkg-share shape_estimation)/launch/shape_estimation.launch.xml">
+          <arg name="input/objects" value="$(var shape_estimation/input)"/>
           <arg name="output/objects" value="objects_with_feature"/>
         </include>
       </group>

diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -77,6 +77,7 @@
   <arg name="use_low_height_cropbox" default="true" description="use low height crop filter in clustering"/>
   <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="false" description="use low_intensity_cluster_filter in clustering"/>
   <arg
     name="use_empty_dynamic_object_publisher"
     default="false"

diff --git a/perception/raindrop_cluster_filter/CMakeLists.txt b/perception/raindrop_cluster_filter/CMakeLists.txt
@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 3.8)
+project(raindrop_cluster_filter)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+# find dependencies
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+# Targets
+ament_auto_add_library(low_intensity_cluster_filter SHARED
+  src/low_intensity_cluster_filter.cpp
+)
+
+rclcpp_components_register_node(low_intensity_cluster_filter
+  PLUGIN "low_intensity_cluster_filter::LowIntensityClusterFilter"
+  EXECUTABLE low_intensity_cluster_filter_node)
+
+
+if(BUILD_TESTING)
+  list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+  config
+)
diff --git a/perception/raindrop_cluster_filter/config/low_intensity_cluster_filter.param.yaml b/perception/raindrop_cluster_filter/config/low_intensity_cluster_filter.param.yaml
@@ -0,0 +1,17 @@
+/**:
+  ros__parameters:
+    intensity_threshold: 1.0
+    existence_probability_threshold: 0.2
+    max_x: 60.0
+    min_x: -20.0
+    max_y: 20.0
+    min_y: -20.0
+    filter_target_label:
+      UNKNOWN: true
+      CAR: false
+      TRUCK: false
+      BUS: false
+      TRAILER: false
+      MOTORCYCLE: false
+      BICYCLE: false
+      PEDESTRIAN: false
diff --git a/...ude/raindrop_cluster_filter/low_intensity_cluster_filter/low_intensity_cluster_filter.hpp b/...ude/raindrop_cluster_filter/low_intensity_cluster_filter/low_intensity_cluster_filter.hpp
@@ -0,0 +1,78 @@
+// 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 RAINDROP_CLUSTER_FILTER__LOW_INTENSITY_CLUSTER_FILTER__LOW_INTENSITY_CLUSTER_FILTER_HPP_
+#define RAINDROP_CLUSTER_FILTER__LOW_INTENSITY_CLUSTER_FILTER__LOW_INTENSITY_CLUSTER_FILTER_HPP_
+
+#include "detected_object_validation/utils/utils.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
+
+#include <tier4_perception_msgs/msg/detected_objects_with_feature.hpp>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+// #include <tf2_eigen/tf2_eigen.hpp>
+#include <Eigen/Eigen>
+
+#include <memory>
+#include <string>
+
+namespace low_intensity_cluster_filter
+{
+
+class LowIntensityClusterFilter : public rclcpp::Node
+{
+public:
+  explicit LowIntensityClusterFilter(const rclcpp::NodeOptions & node_options);
+
+private:
+  void objectCallback(
+    const tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_msg);
+  bool isValidatedCluster(const sensor_msgs::msg::PointCloud2 & cluster);
+
+  rclcpp::Publisher<tier4_perception_msgs::msg::DetectedObjectsWithFeature>::SharedPtr object_pub_;
+  rclcpp::Subscription<tier4_perception_msgs::msg::DetectedObjectsWithFeature>::SharedPtr
+    object_sub_;
+
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+  double intensity_threshold_;
+  double existence_probability_threshold_;
+  double max_x_;
+  double min_x_;
+  double max_y_;
+  double min_y_;
+
+  double max_x_transformed_;
+  double min_x_transformed_;
+  double max_y_transformed_;
+  double min_y_transformed_;
+  // Eigen::Vector4f min_boundary_transformed_;
+  // Eigen::Vector4f max_boundary_transformed_;
+  bool is_validation_range_transformed_ = false;
+  const std::string base_link_frame_id_ = "base_link";
+  utils::FilterTargetLabel filter_target_;
+
+  // debugger
+  std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_{
+    nullptr};
+  std::unique_ptr<tier4_autoware_utils::DebugPublisher> debug_publisher_ptr_{nullptr};
+};
+
+}  // namespace low_intensity_cluster_filter
+
+#endif  // RAINDROP_CLUSTER_FILTER__LOW_INTENSITY_CLUSTER_FILTER__LOW_INTENSITY_CLUSTER_FILTER_HPP_
diff --git a/perception/raindrop_cluster_filter/launch/low_intensity_cluster_filter.launch.xml b/perception/raindrop_cluster_filter/launch/low_intensity_cluster_filter.launch.xml
@@ -0,0 +1,14 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="input/objects" default="/perception/object_recognition/detection/clustering/objects_with_feature"/>
+  <arg name="output/objects" default="/perception/object_recognition/detection/clustering/validated_objects_with_feature"/>
+
+  <arg name="low_intensity_cluster_filter_param_path" default="$(find-pkg-share raindrop_cluster_filter)/config/low_intensity_cluster_filter.param.yaml"/>
+
+  <node pkg="raindrop_cluster_filter" exec="low_intensity_cluster_filter_node" name="low_intensity_cluster_filter_node" output="screen">
+    <remap from="input/objects" to="$(var input/objects)"/>
+    <remap from="output/objects" to="$(var output/objects)"/>
+    <param name="input_obstacle_pointcloud" value="false"/>
+    <param from="$(var low_intensity_cluster_filter_param_path)"/>
+  </node>
+</launch>
diff --git a/perception/raindrop_cluster_filter/package.xml b/perception/raindrop_cluster_filter/package.xml
@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>raindrop_cluster_filter</name>
+  <version>0.1.0</version>
+  <description>The ROS 2 filter cluster package</description>
+  <maintainer email="[email protected]">Yukihiro Saito</maintainer>
+  <maintainer email="[email protected]">Dai Nguyen</maintainer>
+  <maintainer email="[email protected]">Yoshi Ri</maintainer>
+  <license>Apache License 2.0</license>
+  <author email="[email protected]">Dai Nguyen</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+  <depend>detected_object_validation</depend>
+  <depend>lanelet2_extension</depend>
+  <depend>message_filters</depend>
+  <depend>nav_msgs</depend>
+  <depend>pcl_conversions</depend>
+  <depend>pcl_ros</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_eigen</depend>
+  <depend>tf2_ros</depend>
+  <depend>tier4_autoware_utils</depend>
+  <depend>tier4_perception_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/perception/raindrop_cluster_filter/raindrop_cluster_filter.md b/perception/raindrop_cluster_filter/raindrop_cluster_filter.md
@@ -0,0 +1,54 @@
+# low_intensity_cluster_filter
+
+## Purpose
+
+The `low_intensity_cluster_filter` is a node that filters clusters based on the intensity of their pointcloud.
+
+Mainly this focuses on filtering out unknown objects with very low intensity pointcloud, such as fail detection of unknown objects caused by raindrop or water splash from ego or other fast moving vehicles.
+
+## Inner-workings / Algorithms
+
+## Inputs / Outputs
+
+### Input
+
+| Name           | Type                                                     | Description            |
+| -------------- | -------------------------------------------------------- | ---------------------- |
+| `input/object` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | input detected objects |
+
+### Output
+
+| Name            | Type                                                     | Description               |
+| --------------- | -------------------------------------------------------- | ------------------------- |
+| `output/object` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | filtered detected objects |
+
+## Parameters
+
+### Core Parameters
+
+| Name                              | Type  | Default Value                                           | Description                                   |
+| --------------------------------- | ----- | ------------------------------------------------------- | --------------------------------------------- |
+| `filter_target_label.UNKNOWN`     | bool  | false                                                   | If true, unknown objects are filtered.        |
+| `filter_target_label.CAR`         | bool  | false                                                   | If true, car objects are filtered.            |
+| `filter_target_label.TRUCK`       | bool  | false                                                   | If true, truck objects are filtered.          |
+| `filter_target_label.BUS`         | bool  | false                                                   | If true, bus objects are filtered.            |
+| `filter_target_label.TRAILER`     | bool  | false                                                   | If true, trailer objects are filtered.        |
+| `filter_target_label.MOTORCYCLE`  | bool  | false                                                   | If true, motorcycle objects are filtered.     |
+| `filter_target_label.BICYCLE`     | bool  | false                                                   | If true, bicycle objects are filtered.        |
+| `filter_target_label.PEDESTRIAN`  | bool  | false                                                   | If true, pedestrian objects are filtered.     |
+| `max_x`                           | float | 60.00                                                   | Maximum of x of the filter effective range    |
+| `min_x`                           | float | -20.00                                                  | Minimum of x of the filter effective range    |
+| `max_y`                           | float | 20.00                                                   | Maximum of y of the filter effective range    |
+| `min_y`                           | float | -20.00                                                  | Minium of y of the filter effective range     |
+| `intensity_threshold`             | float | 1.0                                                     | The threshold of average intensity for filter |
+| `existence_probability_threshold` | float | The existence probability threshold to apply the filter |
+
+## Assumptions / Known limits
+
+## (Optional) Error detection and handling
+
+## (Optional) Performance characterization
+
+## (Optional) References/External links
+
+## (Optional) Future extensions / Unimplemented parts