Skip to content

Commit e517f09

Browse files
badai-nguyenbadai-nguyen
authored
feat: add low_intensity_cluster_filter (#6850)
* feat: add low_intensity_cluster_filter Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: typo Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: build test error Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> --------- Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
1 parent 6ba26e5 commit e517f09

File tree

9 files changed

+384
-1
lines changed

9 files changed

+384
-1
lines changed

‎launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml

+12-1
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@
88
<!-- Lidar + Camera detector parameters -->
99
<arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
1010
<arg name="use_roi_based_cluster" default="false"/>
11+
<arg name="use_low_intensity_cluster_filter" default="false"/>
1112

1213
<!-- Camera parameters -->
1314
<arg name="image_raw0" default="/image_raw" description="image raw topic name"/>
@@ -208,9 +209,19 @@
208209
</include>
209210
</group>
210211

212+
<!-- low_intensity_cluster_filter -->
211213
<group>
212-
<include file="$(find-pkg-share shape_estimation)/launch/shape_estimation.launch.xml">
214+
<include file="$(find-pkg-share raindrop_cluster_filter)/launch/low_intensity_cluster_filter.launch.xml" if="$(var use_low_intensity_cluster_filter)">
213215
<arg name="input/objects" value="clusters"/>
216+
<arg name="output/objects" value="filtered/clusters"/>
217+
</include>
218+
</group>
219+
220+
<group>
221+
<let name="shape_estimation/input" value="filtered/clusters" if="$(var use_low_intensity_cluster_filter)"/>
222+
<let name="shape_estimation/input" value="clusters" unless="$(var use_low_intensity_cluster_filter)"/>
223+
<include file="$(find-pkg-share shape_estimation)/launch/shape_estimation.launch.xml">
224+
<arg name="input/objects" value="$(var shape_estimation/input)"/>
214225
<arg name="output/objects" value="objects_with_feature"/>
215226
</include>
216227
</group>

‎launch/tier4_perception_launch/launch/perception.launch.xml

+1
Original file line numberDiff line numberDiff line change
@@ -77,6 +77,7 @@
7777
<arg name="use_low_height_cropbox" default="true" description="use low height crop filter in clustering"/>
7878
<arg name="use_object_filter" default="true" description="use object filter"/>
7979
<arg name="use_roi_based_cluster" default="true" description="use roi_based_cluster in clustering"/>
80+
<arg name="use_low_intensity_cluster_filter" default="false" description="use low_intensity_cluster_filter in clustering"/>
8081
<arg
8182
name="use_empty_dynamic_object_publisher"
8283
default="false"

‎perception/raindrop_cluster_filter/CMakeLists.txt

+31
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(raindrop_cluster_filter)
3+
4+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
5+
add_compile_options(-Wall -Wextra -Wpedantic)
6+
endif()
7+
# find dependencies
8+
find_package(autoware_cmake REQUIRED)
9+
autoware_package()
10+
11+
# Targets
12+
ament_auto_add_library(low_intensity_cluster_filter SHARED
13+
src/low_intensity_cluster_filter.cpp
14+
)
15+
16+
rclcpp_components_register_node(low_intensity_cluster_filter
17+
PLUGIN "low_intensity_cluster_filter::LowIntensityClusterFilter"
18+
EXECUTABLE low_intensity_cluster_filter_node)
19+
20+
21+
if(BUILD_TESTING)
22+
list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify)
23+
find_package(ament_lint_auto REQUIRED)
24+
ament_lint_auto_find_test_dependencies()
25+
endif()
26+
27+
ament_auto_package(
28+
INSTALL_TO_SHARE
29+
launch
30+
config
31+
)

‎perception/raindrop_cluster_filter/config/low_intensity_cluster_filter.param.yaml

+17
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
/**:
2+
ros__parameters:
3+
intensity_threshold: 1.0
4+
existence_probability_threshold: 0.2
5+
max_x: 60.0
6+
min_x: -20.0
7+
max_y: 20.0
8+
min_y: -20.0
9+
filter_target_label:
10+
UNKNOWN: true
11+
CAR: false
12+
TRUCK: false
13+
BUS: false
14+
TRAILER: false
15+
MOTORCYCLE: false
16+
BICYCLE: false
17+
PEDESTRIAN: false

‎perception/raindrop_cluster_filter/include/raindrop_cluster_filter/low_intensity_cluster_filter/low_intensity_cluster_filter.hpp

+78
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,78 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef RAINDROP_CLUSTER_FILTER__LOW_INTENSITY_CLUSTER_FILTER__LOW_INTENSITY_CLUSTER_FILTER_HPP_
16+
#define RAINDROP_CLUSTER_FILTER__LOW_INTENSITY_CLUSTER_FILTER__LOW_INTENSITY_CLUSTER_FILTER_HPP_
17+
18+
#include "detected_object_validation/utils/utils.hpp"
19+
20+
#include <rclcpp/rclcpp.hpp>
21+
#include <tier4_autoware_utils/ros/debug_publisher.hpp>
22+
#include <tier4_autoware_utils/system/stop_watch.hpp>
23+
24+
#include <tier4_perception_msgs/msg/detected_objects_with_feature.hpp>
25+
26+
#include <tf2_ros/buffer.h>
27+
#include <tf2_ros/transform_listener.h>
28+
// #include <tf2_eigen/tf2_eigen.hpp>
29+
#include <Eigen/Eigen>
30+
31+
#include <memory>
32+
#include <string>
33+
34+
namespace low_intensity_cluster_filter
35+
{
36+
37+
class LowIntensityClusterFilter : public rclcpp::Node
38+
{
39+
public:
40+
explicit LowIntensityClusterFilter(const rclcpp::NodeOptions & node_options);
41+
42+
private:
43+
void objectCallback(
44+
const tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_msg);
45+
bool isValidatedCluster(const sensor_msgs::msg::PointCloud2 & cluster);
46+
47+
rclcpp::Publisher<tier4_perception_msgs::msg::DetectedObjectsWithFeature>::SharedPtr object_pub_;
48+
rclcpp::Subscription<tier4_perception_msgs::msg::DetectedObjectsWithFeature>::SharedPtr
49+
object_sub_;
50+
51+
tf2_ros::Buffer tf_buffer_;
52+
tf2_ros::TransformListener tf_listener_;
53+
double intensity_threshold_;
54+
double existence_probability_threshold_;
55+
double max_x_;
56+
double min_x_;
57+
double max_y_;
58+
double min_y_;
59+
60+
double max_x_transformed_;
61+
double min_x_transformed_;
62+
double max_y_transformed_;
63+
double min_y_transformed_;
64+
// Eigen::Vector4f min_boundary_transformed_;
65+
// Eigen::Vector4f max_boundary_transformed_;
66+
bool is_validation_range_transformed_ = false;
67+
const std::string base_link_frame_id_ = "base_link";
68+
utils::FilterTargetLabel filter_target_;
69+
70+
// debugger
71+
std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_{
72+
nullptr};
73+
std::unique_ptr<tier4_autoware_utils::DebugPublisher> debug_publisher_ptr_{nullptr};
74+
};
75+
76+
} // namespace low_intensity_cluster_filter
77+
78+
#endif // RAINDROP_CLUSTER_FILTER__LOW_INTENSITY_CLUSTER_FILTER__LOW_INTENSITY_CLUSTER_FILTER_HPP_

‎perception/raindrop_cluster_filter/launch/low_intensity_cluster_filter.launch.xml

+14
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
<?xml version="1.0" encoding="UTF-8"?>
2+
<launch>
3+
<arg name="input/objects" default="/perception/object_recognition/detection/clustering/objects_with_feature"/>
4+
<arg name="output/objects" default="/perception/object_recognition/detection/clustering/validated_objects_with_feature"/>
5+
6+
<arg name="low_intensity_cluster_filter_param_path" default="$(find-pkg-share raindrop_cluster_filter)/config/low_intensity_cluster_filter.param.yaml"/>
7+
8+
<node pkg="raindrop_cluster_filter" exec="low_intensity_cluster_filter_node" name="low_intensity_cluster_filter_node" output="screen">
9+
<remap from="input/objects" to="$(var input/objects)"/>
10+
<remap from="output/objects" to="$(var output/objects)"/>
11+
<param name="input_obstacle_pointcloud" value="false"/>
12+
<param from="$(var low_intensity_cluster_filter_param_path)"/>
13+
</node>
14+
</launch>

‎perception/raindrop_cluster_filter/package.xml

+36
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>raindrop_cluster_filter</name>
5+
<version>0.1.0</version>
6+
<description>The ROS 2 filter cluster package</description>
7+
<maintainer email="yukihiro.saito@tier4.jp">Yukihiro Saito</maintainer>
8+
<maintainer email="dai.nguyen@tier4.jp">Dai Nguyen</maintainer>
9+
<maintainer email="yoshi.ri@tier4.jp">Yoshi Ri</maintainer>
10+
<license>Apache License 2.0</license>
11+
<author email="dai.nguyen@tier4.jp">Dai Nguyen</author>
12+
13+
<buildtool_depend>ament_cmake</buildtool_depend>
14+
<buildtool_depend>autoware_cmake</buildtool_depend>
15+
<depend>detected_object_validation</depend>
16+
<depend>lanelet2_extension</depend>
17+
<depend>message_filters</depend>
18+
<depend>nav_msgs</depend>
19+
<depend>pcl_conversions</depend>
20+
<depend>pcl_ros</depend>
21+
<depend>rclcpp</depend>
22+
<depend>rclcpp_components</depend>
23+
<depend>sensor_msgs</depend>
24+
<depend>tf2</depend>
25+
<depend>tf2_eigen</depend>
26+
<depend>tf2_ros</depend>
27+
<depend>tier4_autoware_utils</depend>
28+
<depend>tier4_perception_msgs</depend>
29+
30+
<test_depend>ament_lint_auto</test_depend>
31+
<test_depend>ament_lint_common</test_depend>
32+
33+
<export>
34+
<build_type>ament_cmake</build_type>
35+
</export>
36+
</package>

‎perception/raindrop_cluster_filter/raindrop_cluster_filter.md

+54
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,54 @@
1+
# low_intensity_cluster_filter
2+
3+
## Purpose
4+
5+
The `low_intensity_cluster_filter` is a node that filters clusters based on the intensity of their pointcloud.
6+
7+
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.
8+
9+
## Inner-workings / Algorithms
10+
11+
## Inputs / Outputs
12+
13+
### Input
14+
15+
| Name | Type | Description |
16+
| -------------- | -------------------------------------------------------- | ---------------------- |
17+
| `input/object` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | input detected objects |
18+
19+
### Output
20+
21+
| Name | Type | Description |
22+
| --------------- | -------------------------------------------------------- | ------------------------- |
23+
| `output/object` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | filtered detected objects |
24+
25+
## Parameters
26+
27+
### Core Parameters
28+
29+
| Name | Type | Default Value | Description |
30+
| --------------------------------- | ----- | ------------------------------------------------------- | --------------------------------------------- |
31+
| `filter_target_label.UNKNOWN` | bool | false | If true, unknown objects are filtered. |
32+
| `filter_target_label.CAR` | bool | false | If true, car objects are filtered. |
33+
| `filter_target_label.TRUCK` | bool | false | If true, truck objects are filtered. |
34+
| `filter_target_label.BUS` | bool | false | If true, bus objects are filtered. |
35+
| `filter_target_label.TRAILER` | bool | false | If true, trailer objects are filtered. |
36+
| `filter_target_label.MOTORCYCLE` | bool | false | If true, motorcycle objects are filtered. |
37+
| `filter_target_label.BICYCLE` | bool | false | If true, bicycle objects are filtered. |
38+
| `filter_target_label.PEDESTRIAN` | bool | false | If true, pedestrian objects are filtered. |
39+
| `max_x` | float | 60.00 | Maximum of x of the filter effective range |
40+
| `min_x` | float | -20.00 | Minimum of x of the filter effective range |
41+
| `max_y` | float | 20.00 | Maximum of y of the filter effective range |
42+
| `min_y` | float | -20.00 | Minium of y of the filter effective range |
43+
| `intensity_threshold` | float | 1.0 | The threshold of average intensity for filter |
44+
| `existence_probability_threshold` | float | The existence probability threshold to apply the filter |
45+
46+
## Assumptions / Known limits
47+
48+
## (Optional) Error detection and handling
49+
50+
## (Optional) Performance characterization
51+
52+
## (Optional) References/External links
53+
54+
## (Optional) Future extensions / Unimplemented parts

0 commit comments

Comments
 (0)