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 <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
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 @@
<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>
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 <sensor_msgs/point_cloud2_iterator.hpp>
+#include <pcl/search/pcl_search.h>
+
#include <algorithm>
#include <vector>
namespace pointcloud_preprocessor
@@ -29,6 +33,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 +48,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 +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<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 =
+ !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>