Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

refactor(autoware_pointcloud_preprocessor): rework dual return outlier filter parameters #8475

Open
wants to merge 13 commits into
base: main
Choose a base branch
from
Open
2 changes: 1 addition & 1 deletion sensing/autoware_pointcloud_preprocessor/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED
src/outlier_filter/ring_outlier_filter_nodelet.cpp
src/outlier_filter/radius_search_2d_outlier_filter_node.cpp
src/outlier_filter/voxel_grid_outlier_filter_node.cpp
src/outlier_filter/dual_return_outlier_filter_nodelet.cpp
src/outlier_filter/dual_return_outlier_filter_node.cpp
src/passthrough_filter/passthrough_filter_node.cpp
src/passthrough_filter/passthrough_filter_uint16_node.cpp
src/passthrough_filter/passthrough_uint16.cpp
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,19 @@
/**:
ros__parameters:
x_max: 18.0
x_min: -12.0
y_max: 2.0
y_min: -2.0
z_max: 10.0
z_min: 0.0
min_azimuth_deg: 135.0
max_azimuth_deg: 225.0
max_distance: 12.0
vertical_bins: 128
max_azimuth_diff: 50.0
weak_first_distance_ratio: 1.004
general_distance_ratio: 1.03
weak_first_local_noise_threshold: 2
roi_mode: "Fixed_xyz_ROI"
visibility_error_threshold: 0.5
visibility_warn_threshold: 0.7
Original file line number Diff line number Diff line change
Expand Up @@ -50,25 +50,7 @@ This implementation inherits `autoware::pointcloud_preprocessor::Filter` class,

### Core Parameters

| Name | Type | Description |
| ---------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------- |
| `vertical_bins` | int | The number of vertical bin for visibility histogram |
| `max_azimuth_diff` | float | Threshold for ring_outlier_filter |
| `weak_first_distance_ratio` | double | Threshold for ring_outlier_filter |
| `general_distance_ratio` | double | Threshold for ring_outlier_filter |
| `weak_first_local_noise_threshold` | int | The parameter for determining whether it is noise |
| `visibility_error_threshold` | float | When the percentage of white pixels in the binary histogram falls below this parameter the diagnostic status becomes ERR |
| `visibility_warn_threshold` | float | When the percentage of white pixels in the binary histogram falls below this parameter the diagnostic status becomes WARN |
| `roi_mode` | string | The name of ROI mode for switching |
| `min_azimuth_deg` | float | The left limit of azimuth for `Fixed_azimuth_ROI` mode |
| `max_azimuth_deg` | float | The right limit of azimuth for `Fixed_azimuth_ROI` mode |
| `max_distance` | float | The limit distance for for `Fixed_azimuth_ROI` mode |
| `x_max` | float | Maximum of x for `Fixed_xyz_ROI` mode |
| `x_min` | float | Minimum of x for `Fixed_xyz_ROI` mode |
| `y_max` | float | Maximum of y for `Fixed_xyz_ROI` mode |
| `y_min` | float | Minimum of y for `Fixed_xyz_ROI` mode |
| `z_max` | float | Maximum of z for `Fixed_xyz_ROI` mode |
| `z_min` | float | Minimum of z for `Fixed_xyz_ROI` mode |
{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/dual_return_outlier_filter_node.schema.json") }}

## Assumptions / Known limits

Expand Down
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2021 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.
Expand All @@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODELET_HPP_
#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODELET_HPP_
#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODE_HPP_
#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODE_HPP_

#include "autoware/pointcloud_preprocessor/filter.hpp"

Expand Down Expand Up @@ -94,5 +94,5 @@ class DualReturnOutlierFilterComponent : public autoware::pointcloud_preprocesso
} // namespace autoware::pointcloud_preprocessor

// clang-format off
#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODELET_HPP_ // NOLINT
#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODE_HPP_ // NOLINT
// clang-format on

This file was deleted.

Original file line number Diff line number Diff line change
@@ -0,0 +1,17 @@
<launch>
<arg name="input_topic_name" default="/pointcloud"/>
<arg name="output_topic_name" default="/pointcloud_filtered"/>
<arg name="input_frame" default=""/>
<arg name="output_frame" default=""/>
<arg name="dual_return_outlier_filter_param_file" default="$(find-pkg-share autoware_pointcloud_preprocessor)/config/dual_return_outlier_filter_node.param.yaml"/>
<arg name="filter_param_file" default="$(find-pkg-share autoware_pointcloud_preprocessor)/config/filter.param.yaml"/>

<node pkg="autoware_pointcloud_preprocessor" exec="dual_return_outlier_filter_node" name="dual_return_outlier_filter_node">
<param from="$(var dual_return_outlier_filter_param_file)"/>
<param from="$(var filter_param_file)"/>
<remap from="input" to="$(var input_topic_name)"/>
<remap from="output" to="$(var output_topic_name)"/>
<param name="input_frame" value="$(var input_frame)"/>
<param name="output_frame" value="$(var output_frame)"/>
</node>
</launch>
Original file line number Diff line number Diff line change
@@ -0,0 +1,146 @@
{
"$schema": "http://json-schema.org/draft-07/schema#",
"title": "Parameters for Dual Return Outlier Filter Node",
"type": "object",
"definitions": {
"dual_return_outlier_filter": {
"type": "object",
"properties": {
"x_max": {
"type": "number",
"description": "Maximum of x in meters for `Fixed_xyz_ROI` mode",
"default": "18.0"
},
"x_min": {
"type": "number",
"description": "Minimum of x in meters for `Fixed_xyz_ROI` mode",
"default": "-12.0"
},
"y_max": {
"type": "number",
"description": "Maximum of y in meters for `Fixed_xyz_ROI` mode",
"default": "2.0"
},
"y_min": {
"type": "number",
"description": "Minimum of y in meters for `Fixed_xyz_ROI` mode",
"default": "-2.0"
},
"z_max": {
"type": "number",
"description": "Maximum of z in meters for `Fixed_xyz_ROI` mode",
"default": "10.0"
},
"z_min": {
"type": "number",
"description": "Minimum of z in meters for `Fixed_xyz_ROI` mode",
"default": "0.0"
},
"min_azimuth_deg": {
"type": "number",
"description": "The left limit of azimuth in degrees for `Fixed_azimuth_ROI` mode",
"default": "135.0",
"minimum": 0,
"maximum": 360
},
"max_azimuth_deg": {
"type": "number",
"description": "The right limit of azimuth in degrees for `Fixed_azimuth_ROI` mode",
"default": "225.0",
"minimum": 0,
"maximum": 360
},
"max_distance": {
"type": "number",
"description": "The limit distance in meters for `Fixed_azimuth_ROI` mode",
"default": "12.0",
"minimum": 0
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Like I explained before, maybe in another PR, ROS is weak with the difference between floating points and integers.
-> just in case, please keep consistency between floating point representation and integers.
This applies in several places here

},
"vertical_bins": {
"type": "integer",
"description": "The number of vertical bins for the visibility histogram",
"default": "128",
"minimum": 1
},
"max_azimuth_diff": {
"type": "number",
"description": "The azimuth difference threshold in degrees for ring_outlier_filter",
"default": "50",
"minimum": 0
},
"weak_first_distance_ratio": {
"type": "number",
"description": "The maximum allowed ratio between consecutive weak point distances",
"default": "1.004",
"minimum": 0
},
"general_distance_ratio": {
"type": "number",
"description": "The maximum allowed ratio between consecutive normal point distances",
"default": "1.03",
"minimum": 0
},
"weak_first_local_noise_threshold": {
"type": "integer",
"description": "If the number of outliers among weak points is less than the weak_first_local_noise_threshold in the (max_azimuth - min_azimuth) / horizontal_bins interval, all points within the interval will not be filtered out.",
"default": "2",
"minimum": 0
},
"roi_mode": {
"type": "string",
"description": "roi mode",
"default": "Fixed_xyz_ROI",
"enum": ["Fixed_xyz_ROI", "No_ROI", "Fixed_azimuth_ROI"]
},
"visibility_error_threshold": {
"type": "number",
"description": "When the proportion of white pixels in the binary histogram falls below this parameter the diagnostic status becomes ERR",
"default": "0.5",
"minimum": 0,
"maximum": 1
},
"visibility_warn_threshold": {
"type": "number",
"description": "When the proportion of white pixels in the binary histogram falls below this parameter the diagnostic status becomes WARN",
"default": "0.7",
"minimum": 0,
"maximum": 1
}
},
"required": [
"x_max",
"x_min",
"y_max",
"y_min",
"z_max",
"z_min",
"min_azimuth_deg",
"max_azimuth_deg",
"max_distance",
"vertical_bins",
"max_azimuth_diff",
"weak_first_distance_ratio",
"general_distance_ratio",
"weak_first_local_noise_threshold",
"roi_mode",
"visibility_error_threshold",
"visibility_warn_threshold"
],
"additionalProperties": false
}
},
"properties": {
"/**": {
"type": "object",
"properties": {
"ros__parameters": {
"$ref": "#/definitions/dual_return_outlier_filter"
}
},
"required": ["ros__parameters"],
"additionalProperties": false
}
},
"required": ["/**"],
"additionalProperties": false
}
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2021 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.
Expand All @@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include "autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp"
#include "autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp"

#include "autoware_point_types/types.hpp"

Expand All @@ -38,29 +38,24 @@ DualReturnOutlierFilterComponent::DualReturnOutlierFilterComponent(
{
// set initial parameters
{
x_max_ = static_cast<float>(declare_parameter("x_max", 18.0));
x_min_ = static_cast<float>(declare_parameter("x_min", -12.0));
y_max_ = static_cast<float>(declare_parameter("y_max", 2.0));
y_min_ = static_cast<float>(declare_parameter("y_min", -2.0));
z_max_ = static_cast<float>(declare_parameter("z_max", 10.0));
z_min_ = static_cast<float>(declare_parameter("z_min", 0.0));
min_azimuth_deg_ = static_cast<float>(declare_parameter("min_azimuth_deg", 135.0));
max_azimuth_deg_ = static_cast<float>(declare_parameter("max_azimuth_deg", 225.0));
max_distance_ = static_cast<float>(declare_parameter("max_distance", 12.0));
vertical_bins_ = static_cast<int>(declare_parameter("vertical_bins", 128));
max_azimuth_diff_ = static_cast<float>(declare_parameter("max_azimuth_diff", 50.0));
weak_first_distance_ratio_ =
static_cast<double>(declare_parameter("weak_first_distance_ratio", 1.004));
general_distance_ratio_ =
static_cast<double>(declare_parameter("general_distance_ratio", 1.03));

weak_first_local_noise_threshold_ =
static_cast<int>(declare_parameter("weak_first_local_noise_threshold", 2));
roi_mode_ = static_cast<std::string>(declare_parameter("roi_mode", "Fixed_xyz_ROI"));
visibility_error_threshold_ =
static_cast<float>(declare_parameter("visibility_error_threshold", 0.5));
visibility_warn_threshold_ =
static_cast<float>(declare_parameter("visibility_warn_threshold", 0.7));
x_max_ = declare_parameter<float>("x_max");
x_min_ = declare_parameter<float>("x_min");
y_max_ = declare_parameter<float>("y_max");
y_min_ = declare_parameter<float>("y_min");
z_max_ = declare_parameter<float>("z_max");
z_min_ = declare_parameter<float>("z_min");
min_azimuth_deg_ = declare_parameter<float>("min_azimuth_deg");
max_azimuth_deg_ = declare_parameter<float>("max_azimuth_deg");
max_distance_ = declare_parameter<float>("max_distance");
vertical_bins_ = declare_parameter<int>("vertical_bins");
max_azimuth_diff_ = declare_parameter<float>("max_azimuth_diff");
weak_first_distance_ratio_ = declare_parameter<double>("weak_first_distance_ratio");
general_distance_ratio_ = declare_parameter<double>("general_distance_ratio");

weak_first_local_noise_threshold_ = declare_parameter<int>("weak_first_local_noise_threshold");
roi_mode_ = declare_parameter<std::string>("roi_mode");
visibility_error_threshold_ = declare_parameter<double>("visibility_error_threshold");
visibility_warn_threshold_ = declare_parameter<double>("visibility_warn_threshold");
}
updater_.setHardwareID("dual_return_outlier_filter");
updater_.add(
Expand Down
Loading