From 8993b7fef65e6ecf45670ff663f9b543ee2d0187 Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 14 Aug 2024 17:57:54 +0900 Subject: [PATCH 1/7] feat: rework ring outlier parameters Signed-off-by: vividf --- .../CMakeLists.txt | 2 +- .../config/filter.param.yaml | 8 ++ .../ring_outlier_filter_node.param.yaml | 14 +++ .../docs/ring-outlier-filter.md | 15 +-- ...delet.hpp => ring_outlier_filter_node.hpp} | 8 +- .../ring_outlier_filter_node.launch.xml | 16 +++ .../ring_outlier_filter_node.schema.json | 103 ++++++++++++++++++ ...delet.cpp => ring_outlier_filter_node.cpp} | 33 +++--- 8 files changed, 162 insertions(+), 37 deletions(-) create mode 100644 sensing/autoware_pointcloud_preprocessor/config/filter.param.yaml create mode 100644 sensing/autoware_pointcloud_preprocessor/config/ring_outlier_filter_node.param.yaml rename sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/{ring_outlier_filter_nodelet.hpp => ring_outlier_filter_node.hpp} (97%) create mode 100644 sensing/autoware_pointcloud_preprocessor/launch/ring_outlier_filter_node.launch.xml create mode 100644 sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json rename sensing/autoware_pointcloud_preprocessor/src/outlier_filter/{ring_outlier_filter_nodelet.cpp => ring_outlier_filter_node.cpp} (93%) diff --git a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt index 744bac480e058..526f7320a9d9c 100644 --- a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt @@ -70,7 +70,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/downsample_filter/random_downsample_filter_nodelet.cpp src/downsample_filter/approximate_downsample_filter_nodelet.cpp src/downsample_filter/pickup_based_voxel_grid_downsample_filter.cpp - src/outlier_filter/ring_outlier_filter_nodelet.cpp + src/outlier_filter/ring_outlier_filter_node.cpp src/outlier_filter/voxel_grid_outlier_filter_nodelet.cpp src/outlier_filter/radius_search_2d_outlier_filter_nodelet.cpp src/outlier_filter/dual_return_outlier_filter_nodelet.cpp diff --git a/sensing/autoware_pointcloud_preprocessor/config/filter.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/filter.param.yaml new file mode 100644 index 0000000000000..abd396892239e --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/config/filter.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + input_frame: "" + output_frame: "" + max_queue_size: 5 + use_indices: false + latched_indices: false + approximate_sync: false diff --git a/sensing/autoware_pointcloud_preprocessor/config/ring_outlier_filter_node.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/ring_outlier_filter_node.param.yaml new file mode 100644 index 0000000000000..76bf68958f504 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/config/ring_outlier_filter_node.param.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + distance_ratio: 1.03 + object_length_threshold: 0.1 + num_points_threshold: 4 + max_rings_num: 128 + max_points_num_per_ring: 4000 + publish_outlier_pointcloud: false + min_azimuth_deg: 0.0 + max_azimuth_deg: 360.0 + max_distance: 12.0 + vertical_bins: 128 + horizontal_bins: 36 + noise_threshold: 2 diff --git a/sensing/autoware_pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/ring-outlier-filter.md index 7f2efe90a674f..95ea2ed8ba969 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/ring-outlier-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/ring-outlier-filter.md @@ -56,20 +56,7 @@ This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, ### Core Parameters -| Name | Type | Default Value | Description | -| ---------------------------- | ------- | ------------- | ----------------------------------------------------------------------------------------------------------------------------- | -| `distance_ratio` | double | 1.03 | | -| `object_length_threshold` | double | 0.1 | | -| `num_points_threshold` | int | 4 | | -| `max_rings_num` | uint_16 | 128 | | -| `max_points_num_per_ring` | size_t | 4000 | Set this value large enough such that `HFoV / resolution < max_points_num_per_ring` | -| `publish_outlier_pointcloud` | bool | false | Flag to publish outlier pointcloud and visibility score. Due to performance concerns, please set to false during experiments. | -| `min_azimuth_deg` | float | 0.0 | The left limit of azimuth for visibility score calculation | -| `max_azimuth_deg` | float | 360.0 | The right limit of azimuth for visibility score calculation | -| `max_distance` | float | 12.0 | The limit distance for visibility score calculation | -| `vertical_bins` | int | 128 | The number of vertical bin for visibility histogram | -| `horizontal_bins` | int | 36 | The number of horizontal bin for visibility histogram | -| `noise_threshold` | int | 2 | The threshold value for distinguishing noise from valid points in the frequency image | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json") }} | ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp similarity index 97% rename from sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp index caedeac62b88a..cf396e3e3a4be 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// 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. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODELET_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODE_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODE_HPP_ #include "autoware/pointcloud_preprocessor/filter.hpp" #include "autoware/pointcloud_preprocessor/transform_info.hpp" @@ -108,4 +108,4 @@ class RingOutlierFilterComponent : public autoware::pointcloud_preprocessor::Fil }; } // namespace autoware::pointcloud_preprocessor -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODELET_HPP_ +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODE_HPP_ diff --git a/sensing/autoware_pointcloud_preprocessor/launch/ring_outlier_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/ring_outlier_filter_node.launch.xml new file mode 100644 index 0000000000000..db1c78c51bf7b --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/launch/ring_outlier_filter_node.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json new file mode 100644 index 0000000000000..5984c40df82e0 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json @@ -0,0 +1,103 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ring Outlier Filter Node", + "type": "object", + "definitions": { + "ring_outlier_filter": { + "type": "object", + "properties": { + "distance_ratio": { + "type": "number", + "description": "distance_ratio", + "default": "1.03" + }, + "object_length_threshold": { + "type": "number", + "description": "object_length_threshold", + "default": "0.1" + }, + "num_points_threshold": { + "type": "integer", + "description": "num_points_threshold", + "default": "4" + }, + "max_rings_num": { + "type": "integer", + "minimum": 1, + "description": "max_rings_num", + "default": "128" + }, + "max_points_num_per_ring": { + "type": "integer", + "minimum": 0, + "description": "Set this value large enough such that HFoV / resolution < max_points_num_per_ring", + "default": "4000" + }, + "publish_outlier_pointcloud": { + "type": "boolean", + "description": "Flag to publish outlier pointcloud and visibility score. Due to performance concerns, please set to false during experiments.", + "default": "false" + }, + "min_azimuth_deg": { + "type": "number", + "description": "The left limit of azimuth for visibility score calculation", + "default": "0.0" + }, + "max_azimuth_deg": { + "type": "number", + "description": "The right limit of azimuth for visibility score calculation", + "default": "360.0" + }, + "max_distance": { + "type": "number", + "description": "The limit distance for visibility score calculation", + "default": "12.0" + }, + "vertical_bins": { + "type": "integer", + "description": "The number of vertical bin for visibility histogram", + "default": "128" + }, + "horizontal_bins": { + "type": "integer", + "description": "The number of horizontal bin for visibility histogram", + "default": "36" + }, + "noise_threshold": { + "type": "integer", + "description": "The threshold value for distinguishing noise from valid points in the frequency image", + "default": "2" + } + }, + "required": [ + "distance_ratio", + "object_length_threshold", + "num_points_threshold", + "max_rings_num", + "max_points_num_per_ring", + "publish_outlier_pointcloud", + "min_azimuth_deg", + "max_azimuth_deg", + "max_distance", + "vertical_bins", + "horizontal_bins", + "noise_threshold" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/ring_outlier_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp similarity index 93% rename from sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp index e71158ccf32d6..d591c7bd28200 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// 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. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp" #include "autoware_point_types/types.hpp" @@ -47,22 +47,19 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions // set initial parameters { - distance_ratio_ = static_cast(declare_parameter("distance_ratio", 1.03)); - object_length_threshold_ = - static_cast(declare_parameter("object_length_threshold", 0.1)); - num_points_threshold_ = static_cast(declare_parameter("num_points_threshold", 4)); - max_rings_num_ = static_cast(declare_parameter("max_rings_num", 128)); - max_points_num_per_ring_ = - static_cast(declare_parameter("max_points_num_per_ring", 4000)); - publish_outlier_pointcloud_ = - static_cast(declare_parameter("publish_outlier_pointcloud", false)); - - min_azimuth_deg_ = static_cast(declare_parameter("min_azimuth_deg", 0.0)); - max_azimuth_deg_ = static_cast(declare_parameter("max_azimuth_deg", 360.0)); - max_distance_ = static_cast(declare_parameter("max_distance", 12.0)); - vertical_bins_ = static_cast(declare_parameter("vertical_bins", 128)); - horizontal_bins_ = static_cast(declare_parameter("horizontal_bins", 36)); - noise_threshold_ = static_cast(declare_parameter("noise_threshold", 2)); + distance_ratio_ = declare_parameter("distance_ratio"); + object_length_threshold_ = declare_parameter("object_length_threshold"); + num_points_threshold_ = declare_parameter("num_points_threshold"); + max_rings_num_ = declare_parameter("max_rings_num"); + max_points_num_per_ring_ = declare_parameter("max_points_num_per_ring"); + publish_outlier_pointcloud_ = declare_parameter("publish_outlier_pointcloud"); + + min_azimuth_deg_ = declare_parameter("min_azimuth_deg"); + max_azimuth_deg_ = declare_parameter("max_azimuth_deg"); + max_distance_ = declare_parameter("max_distance"); + vertical_bins_ = declare_parameter("vertical_bins"); + horizontal_bins_ = declare_parameter("horizontal_bins"); + noise_threshold_ = declare_parameter("noise_threshold"); } using std::placeholders::_1; From 21dc9d4f8e3b665bf42ece734d3a63d00f6d0d64 Mon Sep 17 00:00:00 2001 From: vividf Date: Tue, 20 Aug 2024 18:47:44 +0900 Subject: [PATCH 2/7] chore: add explicit cast Signed-off-by: vividf --- .../src/outlier_filter/ring_outlier_filter_node.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp index d591c7bd28200..bf140e662440b 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp @@ -50,8 +50,10 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions distance_ratio_ = declare_parameter("distance_ratio"); object_length_threshold_ = declare_parameter("object_length_threshold"); num_points_threshold_ = declare_parameter("num_points_threshold"); - max_rings_num_ = declare_parameter("max_rings_num"); - max_points_num_per_ring_ = declare_parameter("max_points_num_per_ring"); + max_rings_num_ = static_cast(declare_parameter("max_rings_num")); + max_points_num_per_ring_ = + static_cast(declare_parameter("max_points_num_per_ring")); + publish_outlier_pointcloud_ = declare_parameter("publish_outlier_pointcloud"); min_azimuth_deg_ = declare_parameter("min_azimuth_deg"); From efb43daad85ee36ef70280c75d5517b07e306735 Mon Sep 17 00:00:00 2001 From: vividf Date: Tue, 20 Aug 2024 18:55:49 +0900 Subject: [PATCH 3/7] chore: add boundary Signed-off-by: vividf --- .../ring_outlier_filter_node.schema.json | 32 ++++++++++++------- 1 file changed, 20 insertions(+), 12 deletions(-) diff --git a/sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json index 5984c40df82e0..d89d011d2c4df 100644 --- a/sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json +++ b/sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json @@ -9,29 +9,32 @@ "distance_ratio": { "type": "number", "description": "distance_ratio", - "default": "1.03" + "default": "1.03", + "minimum": 0 }, "object_length_threshold": { "type": "number", "description": "object_length_threshold", - "default": "0.1" + "default": "0.1", + "minimum": 0 }, "num_points_threshold": { "type": "integer", "description": "num_points_threshold", - "default": "4" + "default": "4", + "minimum": 0 }, "max_rings_num": { "type": "integer", - "minimum": 1, "description": "max_rings_num", - "default": "128" + "default": "128", + "minimum": 1 }, "max_points_num_per_ring": { "type": "integer", - "minimum": 0, "description": "Set this value large enough such that HFoV / resolution < max_points_num_per_ring", - "default": "4000" + "default": "4000", + "minimum": 0 }, "publish_outlier_pointcloud": { "type": "boolean", @@ -41,27 +44,32 @@ "min_azimuth_deg": { "type": "number", "description": "The left limit of azimuth for visibility score calculation", - "default": "0.0" + "default": "0.0", + "minimum": 0 }, "max_azimuth_deg": { "type": "number", "description": "The right limit of azimuth for visibility score calculation", - "default": "360.0" + "default": "360.0", + "minimum": 0 }, "max_distance": { "type": "number", "description": "The limit distance for visibility score calculation", - "default": "12.0" + "default": "12.0", + "minimum": 0 }, "vertical_bins": { "type": "integer", "description": "The number of vertical bin for visibility histogram", - "default": "128" + "default": "128", + "minimum": 1 }, "horizontal_bins": { "type": "integer", "description": "The number of horizontal bin for visibility histogram", - "default": "36" + "default": "36", + "minimum": 1 }, "noise_threshold": { "type": "integer", From f511fc2b24df4e8a77384b06ec8c387a5fcbc6c0 Mon Sep 17 00:00:00 2001 From: vividf Date: Tue, 20 Aug 2024 18:56:43 +0900 Subject: [PATCH 4/7] chore: remove filter.param Signed-off-by: vividf --- .../config/filter.param.yaml | 8 -------- 1 file changed, 8 deletions(-) delete mode 100644 sensing/autoware_pointcloud_preprocessor/config/filter.param.yaml diff --git a/sensing/autoware_pointcloud_preprocessor/config/filter.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/filter.param.yaml deleted file mode 100644 index abd396892239e..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/config/filter.param.yaml +++ /dev/null @@ -1,8 +0,0 @@ -/**: - ros__parameters: - input_frame: "" - output_frame: "" - max_queue_size: 5 - use_indices: false - latched_indices: false - approximate_sync: false From 137b54689675871ce011871e6e50549ec791f9a3 Mon Sep 17 00:00:00 2001 From: vividf Date: Thu, 29 Aug 2024 14:11:47 +0900 Subject: [PATCH 5/7] chore: set default frame Signed-off-by: vividf --- .../launch/ring_outlier_filter_node.launch.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sensing/autoware_pointcloud_preprocessor/launch/ring_outlier_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/ring_outlier_filter_node.launch.xml index db1c78c51bf7b..68076f5c9c321 100644 --- a/sensing/autoware_pointcloud_preprocessor/launch/ring_outlier_filter_node.launch.xml +++ b/sensing/autoware_pointcloud_preprocessor/launch/ring_outlier_filter_node.launch.xml @@ -1,8 +1,8 @@ - - + + From cd0be8e437d3e99c92e615a2569f06fb44408584 Mon Sep 17 00:00:00 2001 From: vividf Date: Tue, 10 Sep 2024 10:37:40 +0900 Subject: [PATCH 6/7] chore: add maximum boundary Signed-off-by: vividf --- .../schema/ring_outlier_filter_node.schema.json | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json index d89d011d2c4df..8c5196b38aa4e 100644 --- a/sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json +++ b/sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json @@ -51,7 +51,8 @@ "type": "number", "description": "The right limit of azimuth for visibility score calculation", "default": "360.0", - "minimum": 0 + "minimum": 0, + "maximum": 360 }, "max_distance": { "type": "number", From 9b6614c2b00234daab3fba1af29883400854ab7b Mon Sep 17 00:00:00 2001 From: vividf Date: Mon, 30 Sep 2024 16:08:56 +0900 Subject: [PATCH 7/7] chore: boundary to float type Signed-off-by: vividf --- .../schema/ring_outlier_filter_node.schema.json | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json index 8c5196b38aa4e..1c0cb406584ac 100644 --- a/sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json +++ b/sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json @@ -10,13 +10,13 @@ "type": "number", "description": "distance_ratio", "default": "1.03", - "minimum": 0 + "minimum": 0.0 }, "object_length_threshold": { "type": "number", "description": "object_length_threshold", "default": "0.1", - "minimum": 0 + "minimum": 0.0 }, "num_points_threshold": { "type": "integer", @@ -45,20 +45,20 @@ "type": "number", "description": "The left limit of azimuth for visibility score calculation", "default": "0.0", - "minimum": 0 + "minimum": 0.0 }, "max_azimuth_deg": { "type": "number", "description": "The right limit of azimuth for visibility score calculation", "default": "360.0", - "minimum": 0, - "maximum": 360 + "minimum": 0.0, + "maximum": 360.0 }, "max_distance": { "type": "number", "description": "The limit distance for visibility score calculation", "default": "12.0", - "minimum": 0 + "minimum": 0.0 }, "vertical_bins": { "type": "integer", @@ -75,7 +75,8 @@ "noise_threshold": { "type": "integer", "description": "The threshold value for distinguishing noise from valid points in the frequency image", - "default": "2" + "default": "2", + "minimum": 0 } }, "required": [