Skip to content

Commit

Permalink
refactor(pointcloud_preprocessor): publish noise points in ring outli…
Browse files Browse the repository at this point in the history 
…er 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 <[email protected]>

---------

Signed-off-by: kyoichi-sugahara <[email protected]>
  • Loading branch information
@kyoichi-sugahara
kyoichi-sugahara authored Mar 13, 2024
1 parent 35d2143 commit 283ff47
Show file tree
Hide file tree
Showing 4 changed files with 96 additions and 91 deletions.
16 changes: 8 additions & 8 deletions sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
10 changes: 6 additions & 4 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 @@ -44,14 +44,14 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter

private:
/** \brief publisher of excluded pointcloud for debug reason. **/
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr excluded_points_publisher_;
rclcpp::Publisher<PointCloud2>::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_;
Expand All @@ -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
Expand Down
9 changes: 6 additions & 3 deletions sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -139,7 +139,8 @@ void DualReturnOutlierFilterComponent::filter(
}
}

uint32_t horizontal_res = static_cast<uint32_t>((max_azimuth - min_azimuth) / horizontal_bins);
uint32_t horizontal_resolution =
static_cast<uint32_t>((max_azimuth - min_azimuth) / horizontal_bins);

pcl::PointCloud<PointXYZIRADRT>::Ptr pcl_output(new pcl::PointCloud<PointXYZIRADRT>);
pcl_output->points.reserve(pcl_input->points.size());
Expand Down Expand Up @@ -231,7 +232,8 @@ void DualReturnOutlierFilterComponent::filter(
continue;
}
while ((uint)deleted_azimuths[current_deleted_index] <
((i + static_cast<uint>(min_azimuth / horizontal_res) + 1) * horizontal_res) &&
((i + static_cast<uint>(min_azimuth / horizontal_resolution) + 1) *
horizontal_resolution) &&
current_deleted_index < (deleted_azimuths.size() - 1)) {
noise_frequency[i] = noise_frequency[i] + 1;
current_deleted_index++;
Expand All @@ -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<uint>(min_azimuth / horizontal_res)) * horizontal_res) &&
((i + 1 + static_cast<uint>(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]);
Expand Down
152 changes: 76 additions & 76 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 @@ -33,8 +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);
outlier_pointcloud_publisher_ =
this->create_publisher<PointCloud2>("debug/ring_outlier_filter", 1);
stop_watch_ptr_->tic("cyclic_time");
stop_watch_ptr_->tic("processing_time");
}
Expand All @@ -48,8 +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));
publish_outlier_pointcloud_ =
static_cast<bool>(declare_parameter("publish_outlier_pointcloud", false));
}

using std::placeholders::_1;
Expand All @@ -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<size_t>(autoware_point_types::PointIndex::Ring)).offset;
const auto azimuth_offset =
Expand Down Expand Up @@ -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<PointXYZI *>(&outlier_points.data[outlier_points_size]);
auto input_ptr =
reinterpret_cast<const PointXYZI *>(&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<const float *>(
&input->data[indices[walk_first_idx] + intensity_offset]);
outlier_ptr->intensity = intensity;

outlier_points_size += outlier_points.point_step;
}
}

walk_first_idx = idx + 1;
Expand Down Expand Up @@ -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<PointXYZI *>(&outlier_points.data[outlier_points_size]);
auto input_ptr = reinterpret_cast<const PointXYZI *>(&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<const float *>(&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<uint32_t>(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
Expand Down Expand Up @@ -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;
Expand All @@ -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<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 =
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<uint32_t>(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
Expand Down

0 comments on commit 283ff47

Please sign in to comment.