refactor(pointcloud_preprocessor): publish noise points in ring outlier filter

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

@@ -22,14 +22,14 @@ 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_excluded_points` | bool    | false         | Flag to publish excluded pointcloud for debugging purpose. 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. Due to performance concerns, please set to false during experiments. |
 
 ## Assumptions / Known limits
 

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

@@ -44,14 +44,14 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter
 
 private:
   /** \brief publisher of excluded pointcloud for debug reason. **/
-  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr excluded_points_publisher_;
+  rclcpp::Publisher<PointCloud2>::SharedPtr outlier_pointcloud_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_;
+  bool publish_outlier_pointcloud_;
 
   /** \brief Parameter service callback result : needed to be hold */
   OnSetParametersCallbackHandle::SharedPtr set_param_res_;
@@ -73,8 +73,10 @@ 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);
+
+  void setUpPointCloudFormat(
+    const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size,
+    size_t num_fields);
 
 public:
   PCL_MAKE_ALIGNED_OPERATOR_NEW

sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp

@@ -139,7 +139,8 @@
     }
   }
 
-  uint32_t horizontal_res = static_cast<uint32_t>((max_azimuth - min_azimuth) / horizontal_bins);
+  uint32_t horizontal_resolution =
+    static_cast<uint32_t>((max_azimuth - min_azimuth) / horizontal_bins);
 
   pcl::PointCloud<PointXYZIRADRT>::Ptr pcl_output(new pcl::PointCloud<PointXYZIRADRT>);
   pcl_output->points.reserve(pcl_input->points.size());
@@ -231,7 +232,8 @@
         continue;
       }
       while ((uint)deleted_azimuths[current_deleted_index] <
-               ((i + static_cast<uint>(min_azimuth / horizontal_res) + 1) * horizontal_res) &&
+               ((i + static_cast<uint>(min_azimuth / horizontal_resolution) + 1) *
+                horizontal_resolution) &&
              current_deleted_index < (deleted_azimuths.size() - 1)) {
         noise_frequency[i] = noise_frequency[i] + 1;
         current_deleted_index++;
@@ -240,7 +242,8 @@
         while ((temp_segment.points[current_temp_segment_index].azimuth < 0.f
                   ? 0.f
                   : temp_segment.points[current_temp_segment_index].azimuth) <
-                 ((i + 1 + static_cast<uint>(min_azimuth / horizontal_res)) * horizontal_res) &&
+                 ((i + 1 + static_cast<uint>(min_azimuth / horizontal_resolution)) *
+                  horizontal_resolution) &&
                current_temp_segment_index < (temp_segment.points.size() - 1)) {
           if (noise_frequency[i] < weak_first_local_noise_threshold_) {
             pcl_output->points.push_back(temp_segment.points[current_temp_segment_index]);

sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp

@@ -33,8 +33,8 @@
     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);
+    outlier_pointcloud_publisher_ =
+      this->create_publisher<PointCloud2>("debug/ring_outlier_filter", 1);
     stop_watch_ptr_->tic("cyclic_time");
     stop_watch_ptr_->tic("processing_time");
   }
@@ -48,8 +48,8 @@
     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));
+    publish_outlier_pointcloud_ =
+      static_cast<bool>(declare_parameter("publish_outlier_pointcloud", false));
   }
 
   using std::placeholders::_1;
@@ -73,6 +73,14 @@
   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);
+  }
+
   const auto ring_offset =
     input->fields.at(static_cast<size_t>(autoware_point_types::PointIndex::Ring)).offset;
   const auto azimuth_offset =
@@ -153,6 +161,27 @@
 
           output_size += output.point_step;
         }
+      } 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[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];
+          } else {
+            *outlier_ptr = *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;
+        }
       }
 
       walk_first_idx = idx + 1;
@@ -182,37 +211,32 @@
 
         output_size += output.point_step;
       }
+    } 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]]);
+        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];
+        } else {
+          *outlier_ptr = *input_ptr;
+        }
+        const float & intensity =
+          *reinterpret_cast<const float *>(&input->data[indices[i] + intensity_offset]);
+        outlier_ptr->intensity = intensity;
+        outlier_points_size += outlier_points.point_step;
+      }
     }
   }
 
-  output.data.resize(output_size);
-
-  // Note that `input->header.frame_id` is data before converted when `transform_info.need_transform
-  // == true`
-  output.header.frame_id = !tf_input_frame_.empty() ? tf_input_frame_ : tf_input_orig_frame_;
-
-  output.height = 1;
-  output.width = static_cast<uint32_t>(output.data.size() / output.height / output.point_step);
-  output.is_bigendian = input->is_bigendian;
-  output.is_dense = input->is_dense;
-
-  // set fields
-  sensor_msgs::PointCloud2Modifier pcd_modifier(output);
-  constexpr int num_fields = 4;
-  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);
+  setUpPointCloudFormat(input, output, output_size, /*num_fields=*/4);
 
-  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);
+  if (publish_outlier_pointcloud_) {
+    setUpPointCloudFormat(input, outlier_points, outlier_points_size, /*num_fields=*/4);
+    outlier_pointcloud_publisher_->publish(outlier_points);
   }
 
   // add processing time for debug
@@ -260,9 +284,9 @@
   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_)) {
+  if (get_param(p, "publish_outlier_pointcloud", publish_outlier_pointcloud_)) {
     RCLCPP_DEBUG(
-      get_logger(), "Setting new publish_excluded_points to: %d.", publish_excluded_points_);
+      get_logger(), "Setting new publish_outlier_pointcloud to: %d.", publish_outlier_pointcloud_);
   }
 
   rcl_interfaces::msg::SetParametersResult result;
@@ -272,51 +296,27 @@
   return result;
 }
 
-sensor_msgs::msg::PointCloud2 RingOutlierFilterComponent::extractExcludedPoints(
-  const sensor_msgs::msg::PointCloud2 & input, const sensor_msgs::msg::PointCloud2 & output,
-  float epsilon)
+void RingOutlierFilterComponent::setUpPointCloudFormat(
+  const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size,
+  size_t num_fields)
 {
-  // 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>);
-
-  pcl::search::Search<pcl::PointXYZ>::Ptr tree;
-  if (output_cloud->isOrganized()) {
-    tree.reset(new pcl::search::OrganizedNeighbor<pcl::PointXYZ>());
-  } else {
-    tree.reset(new pcl::search::KdTree<pcl::PointXYZ>(false));
-  }
-  tree->setInputCloud(output_cloud);
-  std::vector<int> nn_indices(1);
-  std::vector<float> nn_distances(1);
-  for (const auto & point : input_cloud->points) {
-    if (!tree->nearestKSearch(point, 1, nn_indices, nn_distances)) {
-      continue;
-    }
-    if (nn_distances[0] > epsilon) {
-      excluded_points->points.push_back(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 =
+  formatted_points.data.resize(points_size);
+  // Note that `input->header.frame_id` is data before converted when `transform_info.need_transform
+  // == true`
+  formatted_points.header.frame_id =
     !tf_input_frame_.empty() ? tf_input_frame_ : tf_input_orig_frame_;
-
-  return excluded_points_msg;
+  formatted_points.data.resize(formatted_points.point_step * input->width);
+  formatted_points.height = 1;
+  formatted_points.width =
+    static_cast<uint32_t>(formatted_points.data.size() / formatted_points.point_step);
+  formatted_points.is_bigendian = input->is_bigendian;
+  formatted_points.is_dense = input->is_dense;
+
+  sensor_msgs::PointCloud2Modifier pcd_modifier(formatted_points);
+  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);
 }
 
 }  // namespace pointcloud_preprocessor