diff --git a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md index 42ca694235a00..9dcc8b1a03715 100644 --- a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md +++ b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md @@ -22,13 +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` | +| 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. | ## 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 ab7d4e0012304..57ed7a508ca54 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 @@ -21,6 +21,7 @@ #include +#include #include #include @@ -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::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 diff --git a/sensing/pointcloud_preprocessor/package.xml b/sensing/pointcloud_preprocessor/package.xml index e1a396c69b3e9..936c52ef81b37 100644 --- a/sensing/pointcloud_preprocessor/package.xml +++ b/sensing/pointcloud_preprocessor/package.xml @@ -18,6 +18,7 @@ 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 d968b06a0dc61..e0c8443cfe57b 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,8 +14,12 @@ #include "pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp" +#include "autoware_auto_geometry/common_3d.hpp" + #include +#include + #include #include namespace pointcloud_preprocessor @@ -29,6 +33,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions using tier4_autoware_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "ring_outlier_filter"); + excluded_points_publisher_ = + this->create_publisher("debug/ring_outlier_filter", 1); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } @@ -42,6 +48,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions max_rings_num_ = static_cast(declare_parameter("max_rings_num", 128)); max_points_num_per_ring_ = static_cast(declare_parameter("max_points_num_per_ring", 4000)); + publish_excluded_points_ = + static_cast(declare_parameter("publish_excluded_points", false)); } using std::placeholders::_1; @@ -196,6 +204,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,6 +260,10 @@ 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; @@ -248,6 +271,54 @@ rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallba 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::Ptr input_cloud(new pcl::PointCloud); + pcl::PointCloud::Ptr output_cloud(new pcl::PointCloud); + pcl::fromROSMsg(input, *input_cloud); + pcl::fromROSMsg(output, *output_cloud); + + pcl::PointCloud::Ptr excluded_points(new pcl::PointCloud); + + pcl::search::Search::Ptr tree; + if (output_cloud->isOrganized()) { + tree.reset(new pcl::search::OrganizedNeighbor()); + } else { + tree.reset(new pcl::search::KdTree(false)); + } + tree->setInputCloud(output_cloud); + std::vector nn_indices(1); + std::vector 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(output.data.size() / output.height / output.point_step); + excluded_points_msg.row_step = static_cast(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