Skip to content

Commit 0410e92

Browse files
feat(pointcloud_preprocessor): publish excluded points in ring_outlier_filter for debug purpose (autowarefoundation#6483)
* publish excluded points for debug purpose Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * improve performance of extraction method Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * change variable name to avoid spell miss checker error --------- Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
1 parent a25d0ec commit 0410e92

File tree

4 files changed

+87
-7
lines changed

4 files changed

+87
-7
lines changed

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

+8-7
Original file line numberDiff line numberDiff line change
@@ -22,13 +22,14 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref
2222

2323
### Core Parameters
2424

25-
| Name | Type | Default Value | Description |
26-
| ------------------------- | ------- | ------------- | ----------------------------------------------------------------------------------- |
27-
| `distance_ratio` | double | 1.03 | |
28-
| `object_length_threshold` | double | 0.1 | |
29-
| `num_points_threshold` | int | 4 | |
30-
| `max_rings_num` | uint_16 | 128 | |
31-
| `max_points_num_per_ring` | size_t | 4000 | Set this value large enough such that `HFoV / resolution < max_points_num_per_ring` |
25+
| Name | Type | Default Value | Description |
26+
| ------------------------- | ------- | ------------- | ------------------------------------------------------------------------------------------------------------------------------- |
27+
| `distance_ratio` | double | 1.03 | |
28+
| `object_length_threshold` | double | 0.1 | |
29+
| `num_points_threshold` | int | 4 | |
30+
| `max_rings_num` | uint_16 | 128 | |
31+
| `max_points_num_per_ring` | size_t | 4000 | Set this value large enough such that `HFoV / resolution < max_points_num_per_ring` |
32+
| `publish_excluded_points` | bool | false | Flag to publish excluded pointcloud for debugging purpose. Due to performance concerns, please set to false during experiments. |
3233

3334
## Assumptions / Known limits
3435

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

+7
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121

2222
#include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>
2323

24+
#include <memory>
2425
#include <utility>
2526
#include <vector>
2627

@@ -42,11 +43,15 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter
4243
const TransformInfo & transform_info);
4344

4445
private:
46+
/** \brief publisher of excluded pointcloud for debug reason. **/
47+
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr excluded_points_publisher_;
48+
4549
double distance_ratio_;
4650
double object_length_threshold_;
4751
int num_points_threshold_;
4852
uint16_t max_rings_num_;
4953
size_t max_points_num_per_ring_;
54+
bool publish_excluded_points_;
5055

5156
/** \brief Parameter service callback result : needed to be hold */
5257
OnSetParametersCallbackHandle::SharedPtr set_param_res_;
@@ -68,6 +73,8 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter
6873

6974
return x * x + y * y + z * z >= object_length_threshold_ * object_length_threshold_;
7075
}
76+
PointCloud2 extractExcludedPoints(
77+
const PointCloud2 & input, const PointCloud2 & output, float epsilon);
7178

7279
public:
7380
PCL_MAKE_ALIGNED_OPERATOR_NEW

sensing/pointcloud_preprocessor/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818
<buildtool_depend>ament_cmake_auto</buildtool_depend>
1919
<buildtool_depend>autoware_cmake</buildtool_depend>
2020

21+
<depend>autoware_auto_geometry</depend>
2122
<depend>autoware_point_types</depend>
2223
<depend>cgal</depend>
2324
<depend>cv_bridge</depend>

sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp

+71
Original file line numberDiff line numberDiff line change
@@ -14,8 +14,12 @@
1414

1515
#include "pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp"
1616

17+
#include "autoware_auto_geometry/common_3d.hpp"
18+
1719
#include <sensor_msgs/point_cloud2_iterator.hpp>
1820

21+
#include <pcl/search/pcl_search.h>
22+
1923
#include <algorithm>
2024
#include <vector>
2125
namespace pointcloud_preprocessor
@@ -29,6 +33,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions
2933
using tier4_autoware_utils::StopWatch;
3034
stop_watch_ptr_ = std::make_unique<StopWatch<std::chrono::milliseconds>>();
3135
debug_publisher_ = std::make_unique<DebugPublisher>(this, "ring_outlier_filter");
36+
excluded_points_publisher_ =
37+
this->create_publisher<sensor_msgs::msg::PointCloud2>("debug/ring_outlier_filter", 1);
3238
stop_watch_ptr_->tic("cyclic_time");
3339
stop_watch_ptr_->tic("processing_time");
3440
}
@@ -42,6 +48,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions
4248
max_rings_num_ = static_cast<uint16_t>(declare_parameter("max_rings_num", 128));
4349
max_points_num_per_ring_ =
4450
static_cast<size_t>(declare_parameter("max_points_num_per_ring", 4000));
51+
publish_excluded_points_ =
52+
static_cast<bool>(declare_parameter("publish_excluded_points", false));
4553
}
4654

4755
using std::placeholders::_1;
@@ -196,6 +204,17 @@ void RingOutlierFilterComponent::faster_filter(
196204
sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32,
197205
"intensity", 1, sensor_msgs::msg::PointField::FLOAT32);
198206

207+
if (publish_excluded_points_) {
208+
auto excluded_points = extractExcludedPoints(*input, output, 0.01);
209+
// set fields
210+
sensor_msgs::PointCloud2Modifier excluded_pcd_modifier(excluded_points);
211+
excluded_pcd_modifier.setPointCloud2Fields(
212+
num_fields, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1,
213+
sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32,
214+
"intensity", 1, sensor_msgs::msg::PointField::FLOAT32);
215+
excluded_points_publisher_->publish(excluded_points);
216+
}
217+
199218
// add processing time for debug
200219
if (debug_publisher_) {
201220
const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);
@@ -241,13 +260,65 @@ rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallba
241260
if (get_param(p, "num_points_threshold", num_points_threshold_)) {
242261
RCLCPP_DEBUG(get_logger(), "Setting new num_points_threshold to: %d.", num_points_threshold_);
243262
}
263+
if (get_param(p, "publish_excluded_points", publish_excluded_points_)) {
264+
RCLCPP_DEBUG(
265+
get_logger(), "Setting new publish_excluded_points to: %d.", publish_excluded_points_);
266+
}
244267

245268
rcl_interfaces::msg::SetParametersResult result;
246269
result.successful = true;
247270
result.reason = "success";
248271

249272
return result;
250273
}
274+
275+
sensor_msgs::msg::PointCloud2 RingOutlierFilterComponent::extractExcludedPoints(
276+
const sensor_msgs::msg::PointCloud2 & input, const sensor_msgs::msg::PointCloud2 & output,
277+
float epsilon)
278+
{
279+
// Convert ROS PointCloud2 message to PCL point cloud for easier manipulation
280+
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
281+
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
282+
pcl::fromROSMsg(input, *input_cloud);
283+
pcl::fromROSMsg(output, *output_cloud);
284+
285+
pcl::PointCloud<pcl::PointXYZ>::Ptr excluded_points(new pcl::PointCloud<pcl::PointXYZ>);
286+
287+
pcl::search::Search<pcl::PointXYZ>::Ptr tree;
288+
if (output_cloud->isOrganized()) {
289+
tree.reset(new pcl::search::OrganizedNeighbor<pcl::PointXYZ>());
290+
} else {
291+
tree.reset(new pcl::search::KdTree<pcl::PointXYZ>(false));
292+
}
293+
tree->setInputCloud(output_cloud);
294+
std::vector<int> nn_indices(1);
295+
std::vector<float> nn_distances(1);
296+
for (const auto & point : input_cloud->points) {
297+
if (!tree->nearestKSearch(point, 1, nn_indices, nn_distances)) {
298+
continue;
299+
}
300+
if (nn_distances[0] > epsilon) {
301+
excluded_points->points.push_back(point);
302+
}
303+
}
304+
305+
sensor_msgs::msg::PointCloud2 excluded_points_msg;
306+
pcl::toROSMsg(*excluded_points, excluded_points_msg);
307+
308+
// Set the metadata for the excluded points message based on the input cloud
309+
excluded_points_msg.height = 1;
310+
excluded_points_msg.width =
311+
static_cast<uint32_t>(output.data.size() / output.height / output.point_step);
312+
excluded_points_msg.row_step = static_cast<uint32_t>(output.data.size() / output.height);
313+
excluded_points_msg.is_bigendian = input.is_bigendian;
314+
excluded_points_msg.is_dense = input.is_dense;
315+
excluded_points_msg.header = input.header;
316+
excluded_points_msg.header.frame_id =
317+
!tf_input_frame_.empty() ? tf_input_frame_ : tf_input_orig_frame_;
318+
319+
return excluded_points_msg;
320+
}
321+
251322
} // namespace pointcloud_preprocessor
252323

253324
#include <rclcpp_components/register_node_macro.hpp>

0 commit comments

Comments
 (0)