From 283ff4760c6dc97b63a8b74dc80879b9fdff87c2 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 13 Mar 2024 17:37:31 +0900 Subject: [PATCH] refactor(pointcloud_preprocessor): publish noise points in ring outlier filter (#6581) * - Renamed the `publish_excluded_points` flag to `publish_noise_points`. - Removed the `extractExcludedPoints` function - Defined `setUpPointCloudFormat` to consolidate duplicate processes - Renamed `horizontal_res` to `horizontal_resolution`. * change from publish_noise_points to publish_outlier_pointcloud * change namespace of topic Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../docs/ring-outlier-filter.md | 16 +- .../ring_outlier_filter_nodelet.hpp | 10 +- .../dual_return_outlier_filter_nodelet.cpp | 9 +- .../ring_outlier_filter_nodelet.cpp | 152 +++++++++--------- 4 files changed, 96 insertions(+), 91 deletions(-) diff --git a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md index 9dcc8b1a03715..61f65bc40f32d 100644 --- a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md +++ b/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 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 57ed7a508ca54..c871f1acbe5b9 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 @@ -44,14 +44,14 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter private: /** \brief publisher of excluded pointcloud for debug reason. **/ - rclcpp::Publisher::SharedPtr excluded_points_publisher_; + rclcpp::Publisher::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 diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp index e0cc9a5964a09..ccceae9ab5186 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp @@ -139,7 +139,8 @@ void DualReturnOutlierFilterComponent::filter( } } - uint32_t horizontal_res = static_cast((max_azimuth - min_azimuth) / horizontal_bins); + uint32_t horizontal_resolution = + static_cast((max_azimuth - min_azimuth) / horizontal_bins); pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); pcl_output->points.reserve(pcl_input->points.size()); @@ -231,7 +232,8 @@ void DualReturnOutlierFilterComponent::filter( continue; } while ((uint)deleted_azimuths[current_deleted_index] < - ((i + static_cast(min_azimuth / horizontal_res) + 1) * horizontal_res) && + ((i + static_cast(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 @@ void DualReturnOutlierFilterComponent::filter( 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(min_azimuth / horizontal_res)) * horizontal_res) && + ((i + 1 + static_cast(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]); 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 e0c8443cfe57b..762c62dc03b39 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 @@ -33,8 +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); + outlier_pointcloud_publisher_ = + this->create_publisher("debug/ring_outlier_filter", 1); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } @@ -48,8 +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)); + publish_outlier_pointcloud_ = + static_cast(declare_parameter("publish_outlier_pointcloud", false)); } using std::placeholders::_1; @@ -73,6 +73,14 @@ 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); + } + const auto ring_offset = input->fields.at(static_cast(autoware_point_types::PointIndex::Ring)).offset; const auto azimuth_offset = @@ -153,6 +161,27 @@ void RingOutlierFilterComponent::faster_filter( 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(&outlier_points.data[outlier_points_size]); + auto input_ptr = + reinterpret_cast(&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( + &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 @@ void RingOutlierFilterComponent::faster_filter( 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(&outlier_points.data[outlier_points_size]); + auto input_ptr = reinterpret_cast(&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(&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(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 @@ 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_)) { + 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 @@ 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) +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::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 = + 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(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