From 113ad510e7ee125201cc68234302334dcb654808 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Wed, 15 May 2024 00:09:17 +0900 Subject: [PATCH 1/4] calculate visibility score in ring outlier filter Signed-off-by: kyoichi-sugahara --- .../ring_outlier_filter_nodelet.hpp | 23 +++ sensing/pointcloud_preprocessor/package.xml | 1 - .../ring_outlier_filter_nodelet.cpp | 159 ++++++++++++++---- 3 files changed, 150 insertions(+), 33 deletions(-) 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 c871f1acbe5b9..9cc6938aa44d3 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 @@ -19,9 +19,18 @@ #include "pointcloud_preprocessor/filter.hpp" #include "pointcloud_preprocessor/transform_info.hpp" +#include #include +#if __has_include() +#include +#else +#include +#endif + #include +#include +#include #include #include @@ -42,6 +51,9 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output, const TransformInfo & transform_info); + image_transport::Publisher image_pub_; + rclcpp::Publisher::SharedPtr visibility_pub_; + private: /** \brief publisher of excluded pointcloud for debug reason. **/ rclcpp::Publisher::SharedPtr outlier_pointcloud_publisher_; @@ -53,6 +65,16 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter size_t max_points_num_per_ring_; bool publish_outlier_pointcloud_; + // for visibility score + int noise_threshold_; + int vertical_bins_; + int horizontal_bins_; + float max_azimuth_diff_; + + float min_azimuth_deg_; + float max_azimuth_deg_; + float max_distance_; + /** \brief Parameter service callback result : needed to be hold */ OnSetParametersCallbackHandle::SharedPtr set_param_res_; @@ -77,6 +99,7 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter void setUpPointCloudFormat( const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size, size_t num_fields); + float calculateVisibilityScore(const PointCloud2 & input); public: PCL_MAKE_ALIGNED_OPERATOR_NEW diff --git a/sensing/pointcloud_preprocessor/package.xml b/sensing/pointcloud_preprocessor/package.xml index 9a1eae786c379..47fa39e7fcda2 100644 --- a/sensing/pointcloud_preprocessor/package.xml +++ b/sensing/pointcloud_preprocessor/package.xml @@ -22,7 +22,6 @@ ament_cmake_auto autoware_cmake - autoware_auto_geometry autoware_point_types cgal cv_bridge 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 762c62dc03b39..afebd53b87adc 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 @@ -14,16 +14,16 @@ #include "pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp" -#include "autoware_auto_geometry/common_3d.hpp" +#include "autoware_point_types/types.hpp" #include -#include - #include #include namespace pointcloud_preprocessor { +using autoware_point_types::PointXYZIRADRT; + RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions & options) : Filter("RingOutlierFilter", options) { @@ -35,6 +35,10 @@ 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"); stop_watch_ptr_->tic("processing_time"); } @@ -50,6 +54,13 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions 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)); } using std::placeholders::_1; @@ -73,13 +84,7 @@ void RingOutlierFilterComponent::faster_filter( output.data.resize(output.point_step * input->width); size_t output_size = 0; - // Set up the noise points cloud, if noise points are to be published. - PointCloud2 outlier_points; - size_t outlier_points_size = 0; - if (publish_outlier_pointcloud_) { - outlier_points.point_step = sizeof(PointXYZI); - outlier_points.data.resize(outlier_points.point_step * input->width); - } + pcl::PointCloud::Ptr outlier_pcl(new pcl::PointCloud); const auto ring_offset = input->fields.at(static_cast(autoware_point_types::PointIndex::Ring)).offset; @@ -89,6 +94,10 @@ void RingOutlierFilterComponent::faster_filter( input->fields.at(static_cast(autoware_point_types::PointIndex::Distance)).offset; const auto intensity_offset = input->fields.at(static_cast(autoware_point_types::PointIndex::Intensity)).offset; + const auto return_type_offset = + input->fields.at(static_cast(autoware_point_types::PointIndex::ReturnType)).offset; + const auto time_stamp_offset = + input->fields.at(static_cast(autoware_point_types::PointIndex::TimeStamp)).offset; std::vector> ring2indices; ring2indices.reserve(max_rings_num_); @@ -163,24 +172,32 @@ void RingOutlierFilterComponent::faster_filter( } } else if (publish_outlier_pointcloud_) { for (int i = walk_first_idx; i <= walk_last_idx; i++) { - auto outlier_ptr = - reinterpret_cast(&outlier_points.data[outlier_points_size]); + PointXYZIRADRT outlier_point; auto input_ptr = - reinterpret_cast(&input->data[indices[walk_first_idx]]); + reinterpret_cast(&input->data[indices[walk_first_idx]]); if (transform_info.need_transform) { Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); p = transform_info.eigen_transform * p; - outlier_ptr->x = p[0]; - outlier_ptr->y = p[1]; - outlier_ptr->z = p[2]; + outlier_point.x = p[0]; + outlier_point.y = p[1]; + outlier_point.z = p[2]; } else { - *outlier_ptr = *input_ptr; + outlier_point = *input_ptr; } - const float & intensity = *reinterpret_cast( - &input->data[indices[walk_first_idx] + intensity_offset]); - outlier_ptr->intensity = intensity; - outlier_points_size += outlier_points.point_step; + outlier_point.intensity = *reinterpret_cast( + &input->data[indices[walk_first_idx] + intensity_offset]); + outlier_point.ring = *reinterpret_cast( + &input->data[indices[walk_first_idx] + ring_offset]); + outlier_point.azimuth = *reinterpret_cast( + &input->data[indices[walk_first_idx] + azimuth_offset]); + outlier_point.distance = *reinterpret_cast( + &input->data[indices[walk_first_idx] + distance_offset]); + outlier_point.return_type = *reinterpret_cast( + &input->data[indices[walk_first_idx] + return_type_offset]); + outlier_point.time_stamp = *reinterpret_cast( + &input->data[indices[walk_first_idx] + time_stamp_offset]); + outlier_pcl->push_back(outlier_point); } } @@ -213,21 +230,31 @@ void RingOutlierFilterComponent::faster_filter( } } else if (publish_outlier_pointcloud_) { for (int i = walk_first_idx; i < walk_last_idx; i++) { - auto outlier_ptr = reinterpret_cast(&outlier_points.data[outlier_points_size]); - auto input_ptr = reinterpret_cast(&input->data[indices[i]]); + PointXYZIRADRT outlier_point; + + auto input_ptr = reinterpret_cast(&input->data[indices[i]]); if (transform_info.need_transform) { Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); p = transform_info.eigen_transform * p; - outlier_ptr->x = p[0]; - outlier_ptr->y = p[1]; - outlier_ptr->z = p[2]; + outlier_point.x = p[0]; + outlier_point.y = p[1]; + outlier_point.z = p[2]; } else { - *outlier_ptr = *input_ptr; + outlier_point = *input_ptr; } - const float & intensity = + outlier_point.intensity = *reinterpret_cast(&input->data[indices[i] + intensity_offset]); - outlier_ptr->intensity = intensity; - outlier_points_size += outlier_points.point_step; + outlier_point.ring = + *reinterpret_cast(&input->data[indices[i] + ring_offset]); + outlier_point.azimuth = + *reinterpret_cast(&input->data[indices[i] + azimuth_offset]); + outlier_point.distance = + *reinterpret_cast(&input->data[indices[i] + distance_offset]); + outlier_point.return_type = + *reinterpret_cast(&input->data[indices[i] + return_type_offset]); + outlier_point.time_stamp = + *reinterpret_cast(&input->data[indices[i] + time_stamp_offset]); + outlier_pcl->push_back(outlier_point); } } } @@ -235,8 +262,15 @@ void RingOutlierFilterComponent::faster_filter( setUpPointCloudFormat(input, output, output_size, /*num_fields=*/4); if (publish_outlier_pointcloud_) { - setUpPointCloudFormat(input, outlier_points, outlier_points_size, /*num_fields=*/4); - outlier_pointcloud_publisher_->publish(outlier_points); + PointCloud2 outlier; + pcl::toROSMsg(*outlier_pcl, outlier); + outlier.header = input->header; + outlier_pointcloud_publisher_->publish(outlier); + + tier4_debug_msgs::msg::Float32Stamped visibility_msg; + visibility_msg.data = calculateVisibilityScore(outlier); + visibility_msg.stamp = input->header.stamp; + visibility_pub_->publish(visibility_msg); } // add processing time for debug @@ -288,6 +322,12 @@ rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallba RCLCPP_DEBUG( get_logger(), "Setting new publish_outlier_pointcloud to: %d.", publish_outlier_pointcloud_); } + 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_); + } rcl_interfaces::msg::SetParametersResult result; result.successful = true; @@ -319,6 +359,61 @@ void RingOutlierFilterComponent::setUpPointCloudFormat( "intensity", 1, sensor_msgs::msg::PointField::FLOAT32); } +float RingOutlierFilterComponent::calculateVisibilityScore( + const sensor_msgs::msg::PointCloud2 & input) +{ + pcl::PointCloud::Ptr input_cloud(new pcl::PointCloud); + pcl::fromROSMsg(input, *input_cloud); + + const uint32_t vertical_bins = vertical_bins_; + const uint32_t horizontal_bins = horizontal_bins_; + const float max_azimuth = max_azimuth_deg_ * 100.0; + const float min_azimuth = min_azimuth_deg_ * 100.0; + + const uint32_t horizontal_resolution = + static_cast((max_azimuth - min_azimuth) / horizontal_bins); + + std::vector> ring_point_clouds(vertical_bins); + cv::Mat frequency_image(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + + // Split points into rings + for (const auto & point : input_cloud->points) { + ring_point_clouds.at(point.ring).push_back(point); + } + + // Calculate frequency for each bin in each ring + for (const auto & ring_points : ring_point_clouds) { + if (ring_points.empty()) continue; + + const uint ring_id = ring_points.front().ring; + std::vector frequency_in_ring(horizontal_bins, 0); + + for (const auto & point : ring_points.points) { + if (point.azimuth < min_azimuth || point.azimuth >= max_azimuth) continue; + if (point.distance >= max_distance_) continue; + + const uint bin_index = + static_cast((point.azimuth - min_azimuth) / horizontal_resolution); + + frequency_in_ring[bin_index]++; + frequency_in_ring[bin_index] = + std::min(frequency_in_ring[bin_index], 255); // Ensure value is within uchar range + + frequency_image.at(ring_id, bin_index) = + static_cast(frequency_in_ring[bin_index]); + } + } + + cv::Mat binary_image; + cv::inRange(frequency_image, noise_threshold_, 255, binary_image); + + const int num_pixels = cv::countNonZero(frequency_image); + const float num_filled_pixels = + static_cast(num_pixels) / static_cast(vertical_bins * horizontal_bins); + + return 1.0f - num_filled_pixels; +} + } // namespace pointcloud_preprocessor #include From 1d09b2076b44beb2b05b63509dfa9d1b5ef8b017 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Tue, 14 May 2024 23:12:50 +0900 Subject: [PATCH 2/4] calculate visibility score in ring outlier filter Signed-off-by: kyoichi-sugahara --- .../docs/ring-outlier-filter.md | 24 ++++++++++++------- .../ring_outlier_filter_nodelet.hpp | 3 --- .../ring_outlier_filter_nodelet.cpp | 18 ++++++++++---- 3 files changed, 30 insertions(+), 15 deletions(-) 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..55ba79aac4593 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 @@ -51,7 +50,6 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output, const TransformInfo & transform_info); - image_transport::Publisher image_pub_; rclcpp::Publisher::SharedPtr visibility_pub_; private: @@ -69,7 +67,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; From 294986d100b4876b7a3541910ff8e12ce4202c60 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Wed, 15 May 2024 15:02:42 +0900 Subject: [PATCH 3/4] fix readme description Signed-off-by: kyoichi-sugahara --- sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md index 662929bc425b4..3d333ed1e3ab7 100644 --- a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md +++ b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md @@ -34,7 +34,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref | `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 | +| `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 parameter for determining whether it is noise | From c61dab92e5f602cc1afe6823fb6f55a786a98e5e Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Thu, 16 May 2024 12:12:41 +0900 Subject: [PATCH 4/4] update document Signed-off-by: kyoichi-sugahara --- .../docs/ring-outlier-filter.md | 36 +++++++++++++++++-- 1 file changed, 34 insertions(+), 2 deletions(-) diff --git a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md index 3d333ed1e3ab7..e87023ef00149 100644 --- a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md +++ b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md @@ -10,7 +10,39 @@ 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. +Another feature of this node is that it calculates visibility score based on outlier pointcloud and publish score as a topic. + +### visibility score calculation algorithm + +The pointcloud is divided into vertical bins (rings) and horizontal bins (azimuth divisions). +The algorithm starts by splitting the input point cloud into separate rings based on the ring value of each point. Then, for each ring, it iterates through the points and calculates the frequency of points within each horizontal bin. The frequency is determined by incrementing a counter for the corresponding bin based on the point's azimuth value. +The frequency values are stored in a frequency image matrix, where each cell represents a specific ring and azimuth bin. After calculating the frequency image, the algorithm applies a noise threshold to create a binary image. Points with frequency values above the noise threshold are considered valid, while points below the threshold are considered noise. +Finally, the algorithm calculates the visibility score by counting the number of non-zero pixels in the frequency image and dividing it by the total number of pixels (vertical bins multiplied by horizontal bins). + +```plantuml +@startuml +start + +:Convert input point cloud to PCL format; + +:Initialize vertical and horizontal bins; + +:Split point cloud into rings; + +while (For each ring) is (not empty) + :Calculate frequency of points in each azimuth bin; + :Update frequency image matrix; +endwhile + +:Apply noise threshold to create binary image; + +:Count non-zero pixels in frequency image; + +:Calculate visibility score as complement of filled pixel ratio; + +stop +@enduml +``` ## Inputs / Outputs @@ -37,7 +69,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref | `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 parameter for determining whether it is noise | +| `noise_threshold` | int | 2 | The threshold value for distinguishing noise from valid points in the frequency image | ## Assumptions / Known limits