calculate visibility score in ring outlier filter

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

Author: kyoichi-sugahara

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 <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 <unordered_map>
 #include <utility>
 #include <vector>
 
@@ -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<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 +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

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,10 @@ 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);
+    image_pub_ =
+      image_transport::create_publisher(this, "ring_outlier_filter/debug/frequency_image");
+    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 +54,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 +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<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 +94,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 +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<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 +230,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 +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<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>