From 8ef4df8c96ebaf9ccf3c75e2796069cc2afd767e Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Tue, 14 May 2024 23:12:50 +0900 Subject: [PATCH] calculate visibility score in ring outlier filter Signed-off-by: kyoichi-sugahara --- control/cgmres | 1 + .../docs/ring-outlier-filter.md | 24 ++++++++++++------- .../ring_outlier_filter_nodelet.hpp | 2 -- .../ring_outlier_filter_nodelet.cpp | 18 ++++++++++---- 4 files changed, 31 insertions(+), 14 deletions(-) create mode 160000 control/cgmres diff --git a/control/cgmres b/control/cgmres new file mode 160000 index 0000000000000..5abf3be418140 --- /dev/null +++ b/control/cgmres @@ -0,0 +1 @@ +Subproject commit 5abf3be418140e1f70fb30571cfc060671593436 diff --git a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md index 61f65bc40f32d..662929bc425b4 100644 --- a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md +++ b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md @@ -10,6 +10,8 @@ A method of operating scan in chronological order and removing noise based on th ![ring_outlier_filter](./image/outlier_filter-ring.drawio.svg) +Another feature of this node is that it calculates visibility based on outlier pointcloud and publish score as a topic. With this function, for example, in heavy rain, the sensing module can notify that the processing performance has reached its limit, which can lead to ensuring the safety of the vehicle. + ## Inputs / Outputs This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). @@ -22,14 +24,20 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref ### 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. Due to performance concerns, please set to false during experiments. | +| 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 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 parameter for determining whether it is noise | ## Assumptions / Known limits diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp index 9cc6938aa44d3..3a94549dc9c49 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp @@ -30,7 +30,6 @@ #include #include -#include #include #include @@ -69,7 +68,6 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter int noise_threshold_; int vertical_bins_; int horizontal_bins_; - float max_azimuth_diff_; float min_azimuth_deg_; float max_azimuth_deg_; diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp index afebd53b87adc..ada7d2616ed38 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp @@ -35,8 +35,6 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions debug_publisher_ = std::make_unique(this, "ring_outlier_filter"); outlier_pointcloud_publisher_ = this->create_publisher("debug/ring_outlier_filter", 1); - image_pub_ = - image_transport::create_publisher(this, "ring_outlier_filter/debug/frequency_image"); visibility_pub_ = create_publisher( "ring_outlier_filter/debug/visibility", rclcpp::SensorDataQoS()); stop_watch_ptr_->tic("cyclic_time"); @@ -325,8 +323,20 @@ rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallba if (get_param(p, "vertical_bins", vertical_bins_)) { RCLCPP_DEBUG(get_logger(), "Setting new vertical_bins to: %d.", vertical_bins_); } - if (get_param(p, "max_azimuth_diff", max_azimuth_diff_)) { - RCLCPP_DEBUG(get_logger(), "Setting new max_azimuth_diff to: %f.", max_azimuth_diff_); + if (get_param(p, "horizontal_bins", horizontal_bins_)) { + RCLCPP_DEBUG(get_logger(), "Setting new horizontal_bins to: %d.", horizontal_bins_); + } + if (get_param(p, "noise_threshold", noise_threshold_)) { + RCLCPP_DEBUG(get_logger(), "Setting new noise_threshold to: %d.", noise_threshold_); + } + if (get_param(p, "max_azimuth_deg", max_azimuth_deg_)) { + RCLCPP_DEBUG(get_logger(), "Setting new max_azimuth_deg to: %f.", max_azimuth_deg_); + } + if (get_param(p, "min_azimuth_deg", min_azimuth_deg_)) { + RCLCPP_DEBUG(get_logger(), "Setting new min_azimuth_deg to: %f.", min_azimuth_deg_); + } + if (get_param(p, "max_distance", max_distance_)) { + RCLCPP_DEBUG(get_logger(), "Setting new max_distance to: %f.", max_distance_); } rcl_interfaces::msg::SetParametersResult result;