feat(pointcloud_preprocessor): calculate visibility score in ring outlier filter (#7011)

* calculate visibility score in ring outlier filter

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

---------

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

Author: kyoichi-sugahara

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
 

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 <image_transport/image_transport.hpp>
 #include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>
 
+#if __has_include(<cv_bridge/cv_bridge.hpp>)
+#include <cv_bridge/cv_bridge.hpp>
+#else
+#include <cv_bridge/cv_bridge.h>
+#endif
+
 #include <memory>
+#include <string>
 #include <utility>
 #include <vector>
 
@@ -42,6 +50,8 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter
     const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output,
     const TransformInfo & transform_info);
 
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr visibility_pub_;
+
 private:
   /** \brief publisher of excluded pointcloud for debug reason. **/
   rclcpp::Publisher<PointCloud2>::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

sensing/pointcloud_preprocessor/package.xml

@@ -22,7 +22,6 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
-  <depend>autoware_auto_geometry</depend>
   <depend>autoware_point_types</depend>
   <depend>cgal</depend>
   <depend>cv_bridge</depend>

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 <sensor_msgs/point_cloud2_iterator.hpp>
 
-#include <pcl/search/pcl_search.h>
-
 #include <algorithm>
 #include <vector>
 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<DebugPublisher>(this, "ring_outlier_filter");
     outlier_pointcloud_publisher_ =
       this->create_publisher<PointCloud2>("debug/ring_outlier_filter", 1);
+    visibility_pub_ = create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
+      "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<size_t>(declare_parameter("max_points_num_per_ring", 4000));
     publish_outlier_pointcloud_ =
       static_cast<bool>(declare_parameter("publish_outlier_pointcloud", false));
+
+    min_azimuth_deg_ = static_cast<float>(declare_parameter("min_azimuth_deg", 0.0));
+    max_azimuth_deg_ = static_cast<float>(declare_parameter("max_azimuth_deg", 360.0));
+    max_distance_ = static_cast<float>(declare_parameter("max_distance", 12.0));
+    vertical_bins_ = static_cast<int>(declare_parameter("vertical_bins", 128));
+    horizontal_bins_ = static_cast<int>(declare_parameter("horizontal_bins", 36));
+    noise_threshold_ = static_cast<int>(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<PointXYZIRADRT>::Ptr outlier_pcl(new pcl::PointCloud<PointXYZIRADRT>);
 
   const auto ring_offset =
     input->fields.at(static_cast<size_t>(autoware_point_types::PointIndex::Ring)).offset;
@@ -89,6 +92,10 @@ void RingOutlierFilterComponent::faster_filter(
     input->fields.at(static_cast<size_t>(autoware_point_types::PointIndex::Distance)).offset;
   const auto intensity_offset =
     input->fields.at(static_cast<size_t>(autoware_point_types::PointIndex::Intensity)).offset;
+  const auto return_type_offset =
+    input->fields.at(static_cast<size_t>(autoware_point_types::PointIndex::ReturnType)).offset;
+  const auto time_stamp_offset =
+    input->fields.at(static_cast<size_t>(autoware_point_types::PointIndex::TimeStamp)).offset;
 
   std::vector<std::vector<size_t>> 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<PointXYZI *>(&outlier_points.data[outlier_points_size]);
+          PointXYZIRADRT outlier_point;
           auto input_ptr =
-            reinterpret_cast<const PointXYZI *>(&input->data[indices[walk_first_idx]]);
+            reinterpret_cast<const PointXYZIRADRT *>(&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<const float *>(
-            &input->data[indices[walk_first_idx] + intensity_offset]);
-          outlier_ptr->intensity = intensity;
 
-          outlier_points_size += outlier_points.point_step;
+          outlier_point.intensity = *reinterpret_cast<const float *>(
+            &input->data[indices[walk_first_idx] + intensity_offset]);
+          outlier_point.ring = *reinterpret_cast<const uint16_t *>(
+            &input->data[indices[walk_first_idx] + ring_offset]);
+          outlier_point.azimuth = *reinterpret_cast<const float *>(
+            &input->data[indices[walk_first_idx] + azimuth_offset]);
+          outlier_point.distance = *reinterpret_cast<const float *>(
+            &input->data[indices[walk_first_idx] + distance_offset]);
+          outlier_point.return_type = *reinterpret_cast<const uint8_t *>(
+            &input->data[indices[walk_first_idx] + return_type_offset]);
+          outlier_point.time_stamp = *reinterpret_cast<const float *>(
+            &input->data[indices[walk_first_idx] + time_stamp_offset]);
+          outlier_pcl->push_back(outlier_point);
         }
       }
 
@@ -213,30 +228,47 @@ 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<PointXYZI *>(&outlier_points.data[outlier_points_size]);
-        auto input_ptr = reinterpret_cast<const PointXYZI *>(&input->data[indices[i]]);
+        PointXYZIRADRT outlier_point;
+
+        auto input_ptr = reinterpret_cast<const PointXYZIRADRT *>(&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<const float *>(&input->data[indices[i] + intensity_offset]);
-        outlier_ptr->intensity = intensity;
-        outlier_points_size += outlier_points.point_step;
+        outlier_point.ring =
+          *reinterpret_cast<const uint16_t *>(&input->data[indices[i] + ring_offset]);
+        outlier_point.azimuth =
+          *reinterpret_cast<const float *>(&input->data[indices[i] + azimuth_offset]);
+        outlier_point.distance =
+          *reinterpret_cast<const float *>(&input->data[indices[i] + distance_offset]);
+        outlier_point.return_type =
+          *reinterpret_cast<const uint8_t *>(&input->data[indices[i] + return_type_offset]);
+        outlier_point.time_stamp =
+          *reinterpret_cast<const float *>(&input->data[indices[i] + time_stamp_offset]);
+        outlier_pcl->push_back(outlier_point);
       }
     }
   }
 
   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<PointXYZIRADRT>::Ptr input_cloud(new pcl::PointCloud<PointXYZIRADRT>);
+  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<uint32_t>((max_azimuth - min_azimuth) / horizontal_bins);
+
+  std::vector<pcl::PointCloud<PointXYZIRADRT>> 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<int> 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<uint>((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<uchar>(ring_id, bin_index) =
+        static_cast<uchar>(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<float>(num_pixels) / static_cast<float>(vertical_bins * horizontal_bins);
+
+  return 1.0f - num_filled_pixels;
+}
+
 }  // namespace pointcloud_preprocessor
 
 #include <rclcpp_components/register_node_macro.hpp>