Skip to content

Commit

Permalink
feat(pointcloud_preprocessor): publish excluded points in ring_outlie…
Browse files Browse the repository at this point in the history 
…r_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>
  • Loading branch information
@kyoichi-sugahara
kyoichi-sugahara authored Feb 28, 2024
1 parent a25d0ec commit 0410e92
Show file tree
Hide file tree
Showing 4 changed files with 87 additions and 7 deletions.
15 changes: 8 additions & 7 deletions sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
7 changes: 7 additions & 0 deletions ...eprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@

#include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>

#include <memory>
#include <utility>
#include <vector>

Expand All @@ -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_;
Expand All @@ -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
Expand Down
1 change: 1 addition & 0 deletions sensing/pointcloud_preprocessor/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
71 changes: 71 additions & 0 deletions sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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");
}
Expand All @@ -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;
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -241,13 +260,65 @@ 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;
result.reason = "success";

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>
Expand Down

0 comments on commit 0410e92

Please sign in to comment.