publish excluded points for debug purpose

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

Author: kyoichi-sugahara

sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md

@@ -29,6 +29,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref
 | `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_excluded_points` | bool    | false         | Flag to publish excluded pointcloud                                                 |
 
 ## Assumptions / Known limits
 

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp

@@ -21,6 +21,7 @@
 
 #include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>
 
+#include <memory>
 #include <utility>
 #include <vector>
 
@@ -42,11 +43,15 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter
     const TransformInfo & transform_info);
 
 private:
+  /** \brief publisher of excluded pointcloud for debug reason. **/
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr excluded_points_publisher_;
+
   double distance_ratio_;
   double object_length_threshold_;
   int num_points_threshold_;
   uint16_t max_rings_num_;
   size_t max_points_num_per_ring_;
+  bool publish_excluded_points_;
 
   /** \brief Parameter service callback result : needed to be hold */
   OnSetParametersCallbackHandle::SharedPtr set_param_res_;
@@ -68,6 +73,8 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter
 
     return x * x + y * y + z * z >= object_length_threshold_ * object_length_threshold_;
   }
+  PointCloud2 extractExcludedPoints(
+    const PointCloud2 & input, const PointCloud2 & output, float epsilon);
 
 public:
   PCL_MAKE_ALIGNED_OPERATOR_NEW

sensing/pointcloud_preprocessor/package.xml

@@ -18,6 +18,7 @@
   <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,6 +14,8 @@
 
 #include "pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp"
 
+#include "autoware_auto_geometry/common_3d.hpp"
+
 #include <sensor_msgs/point_cloud2_iterator.hpp>
 
 #include <algorithm>
@@ -29,6 +31,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions
     using tier4_autoware_utils::StopWatch;
     stop_watch_ptr_ = std::make_unique<StopWatch<std::chrono::milliseconds>>();
     debug_publisher_ = std::make_unique<DebugPublisher>(this, "ring_outlier_filter");
+    excluded_points_publisher_ =
+      this->create_publisher<sensor_msgs::msg::PointCloud2>("debug/ring_outlier_filter", 1);
     stop_watch_ptr_->tic("cyclic_time");
     stop_watch_ptr_->tic("processing_time");
   }
@@ -42,6 +46,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions
     max_rings_num_ = static_cast<uint16_t>(declare_parameter("max_rings_num", 128));
     max_points_num_per_ring_ =
       static_cast<size_t>(declare_parameter("max_points_num_per_ring", 4000));
+    publish_excluded_points_ =
+      static_cast<bool>(declare_parameter("publish_excluded_points", false));
   }
 
   using std::placeholders::_1;
@@ -196,6 +202,17 @@ void RingOutlierFilterComponent::faster_filter(
     sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32,
     "intensity", 1, sensor_msgs::msg::PointField::FLOAT32);
 
+  if (publish_excluded_points_) {
+    auto excluded_points = extractExcludedPoints(*input, output, 0.01);
+    // set fields
+    sensor_msgs::PointCloud2Modifier excluded_pcd_modifier(excluded_points);
+    excluded_pcd_modifier.setPointCloud2Fields(
+      num_fields, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1,
+      sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32,
+      "intensity", 1, sensor_msgs::msg::PointField::FLOAT32);
+    excluded_points_publisher_->publish(excluded_points);
+  }
+
   // add processing time for debug
   if (debug_publisher_) {
     const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);
@@ -241,13 +258,67 @@ rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallba
   if (get_param(p, "num_points_threshold", num_points_threshold_)) {
     RCLCPP_DEBUG(get_logger(), "Setting new num_points_threshold to: %d.", num_points_threshold_);
   }
+  if (get_param(p, "publish_excluded_points", publish_excluded_points_)) {
+    RCLCPP_DEBUG(
+      get_logger(), "Setting new publish_excluded_points to: %d.", publish_excluded_points_);
+  }
 
   rcl_interfaces::msg::SetParametersResult result;
   result.successful = true;
   result.reason = "success";
 
   return result;
 }
+
+sensor_msgs::msg::PointCloud2 RingOutlierFilterComponent::extractExcludedPoints(
+  const sensor_msgs::msg::PointCloud2 & input, const sensor_msgs::msg::PointCloud2 & output,
+  float epsilon)
+{
+  // Convert ROS PointCloud2 message to PCL point cloud for easier manipulation
+  pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::fromROSMsg(input, *input_cloud);
+  pcl::fromROSMsg(output, *output_cloud);
+
+  pcl::PointCloud<pcl::PointXYZ>::Ptr excluded_points(new pcl::PointCloud<pcl::PointXYZ>);
+
+  for (const auto & input_point : *input_cloud) {
+    bool is_excluded = true;
+
+    for (const auto & output_point : *output_cloud) {
+      float distance = autoware::common::geometry::distance_3d(input_point, output_point);
+
+      // If the distance is less than the threshold (epsilon), the point is not excluded
+      if (distance < epsilon) {
+        is_excluded = false;
+        break;
+      }
+    }
+
+    // If the point is still marked as excluded after all comparisons, add it to the excluded
+    // pointcloud
+    if (is_excluded) {
+      excluded_points->push_back(input_point);
+    }
+  }
+
+  sensor_msgs::msg::PointCloud2 excluded_points_msg;
+  pcl::toROSMsg(*excluded_points, excluded_points_msg);
+
+  // Set the metadata for the excluded points message based on the input cloud
+  excluded_points_msg.height = 1;
+  excluded_points_msg.width =
+    static_cast<uint32_t>(output.data.size() / output.height / output.point_step);
+  excluded_points_msg.row_step = static_cast<uint32_t>(output.data.size() / output.height);
+  excluded_points_msg.is_bigendian = input.is_bigendian;
+  excluded_points_msg.is_dense = input.is_dense;
+  excluded_points_msg.header = input.header;
+  excluded_points_msg.header.frame_id =
+    !tf_input_frame_.empty() ? tf_input_frame_ : tf_input_orig_frame_;
+
+  return excluded_points_msg;
+}
+
 }  // namespace pointcloud_preprocessor
 
 #include <rclcpp_components/register_node_macro.hpp>