From 81a75f749216de15e88ec89c771d8c974bdbeebd Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Fri, 17 May 2024 11:51:26 +0900 Subject: [PATCH] feat(pointcloud_preprocessor): calculate visibility score in ring outlier filter (#7011) * calculate visibility score in ring outlier filter Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../docs/ring-outlier-filter.md | 56 +++++- .../ring_outlier_filter_nodelet.hpp | 20 +++ sensing/pointcloud_preprocessor/package.xml | 1 - .../ring_outlier_filter_nodelet.cpp | 169 ++++++++++++++---- 4 files changed, 205 insertions(+), 41 deletions(-) diff --git a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md index 61f65bc40f32d..e87023ef00149 100644 --- a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md +++ b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md @@ -10,6 +10,40 @@ 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 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 This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). @@ -22,14 +56,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 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 | ## 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 c871f1acbe5b9..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 @@ -19,9 +19,17 @@ #include "pointcloud_preprocessor/filter.hpp" #include "pointcloud_preprocessor/transform_info.hpp" +#include #include +#if __has_include() +#include +#else +#include +#endif + #include +#include #include #include @@ -42,6 +50,8 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output, const TransformInfo & transform_info); + rclcpp::Publisher::SharedPtr visibility_pub_; + private: /** \brief publisher of excluded pointcloud for debug reason. **/ rclcpp::Publisher::SharedPtr outlier_pointcloud_publisher_; @@ -53,6 +63,15 @@ 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 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 +96,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..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 @@ -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,8 @@ 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); + 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 +52,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 +82,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 +92,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 +170,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 +228,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 +260,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 +320,24 @@ 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, "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; result.successful = true; @@ -319,6 +369,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