Skip to content

Commit

Permalink
feat(pointcloud_preprocessor): calculate visibility score in ring out…
Browse files Browse the repository at this point in the history 
…lier filter (#7011)

* calculate visibility score in ring outlier filter

Signed-off-by: kyoichi-sugahara <[email protected]>

---------

Signed-off-by: kyoichi-sugahara <[email protected]>
  • Loading branch information
@kyoichi-sugahara
kyoichi-sugahara authored May 17, 2024
1 parent 0518585 commit 81a75f7
Show file tree
Hide file tree
Showing 4 changed files with 205 additions and 41 deletions.
56 changes: 48 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 @@ -10,6 +10,40 @@ A method of operating scan in chronological order and removing noise based on th

![ring_outlier_filter](./image/outlier_filter-ring.drawio.svg)

Another feature of this node is that it calculates visibility score based on outlier pointcloud and publish score as a topic.

### visibility score calculation algorithm

The pointcloud is divided into vertical bins (rings) and horizontal bins (azimuth divisions).
The algorithm starts by splitting the input point cloud into separate rings based on the ring value of each point. Then, for each ring, it iterates through the points and calculates the frequency of points within each horizontal bin. The frequency is determined by incrementing a counter for the corresponding bin based on the point's azimuth value.
The frequency values are stored in a frequency image matrix, where each cell represents a specific ring and azimuth bin. After calculating the frequency image, the algorithm applies a noise threshold to create a binary image. Points with frequency values above the noise threshold are considered valid, while points below the threshold are considered noise.
Finally, the algorithm calculates the visibility score by counting the number of non-zero pixels in the frequency image and dividing it by the total number of pixels (vertical bins multiplied by horizontal bins).

```plantuml
@startuml
start
:Convert input point cloud to PCL format;
:Initialize vertical and horizontal bins;
:Split point cloud into rings;
while (For each ring) is (not empty)
:Calculate frequency of points in each azimuth bin;
:Update frequency image matrix;
endwhile
:Apply noise threshold to create binary image;
:Count non-zero pixels in frequency image;
:Calculate visibility score as complement of filled pixel ratio;
stop
@enduml
```

## Inputs / Outputs

This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md).
Expand All @@ -22,14 +56,20 @@ 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_outlier_pointcloud` | bool | false | Flag to publish outlier pointcloud. 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 and visibility score. Due to performance concerns, please set to false during experiments. |
| `min_azimuth_deg` | float | 0.0 | The left limit of azimuth for visibility score calculation |
| `max_azimuth_deg` | float | 360.0 | The right limit of azimuth for visibility score calculation |
| `max_distance` | float | 12.0 | The limit distance for visibility score calculation |
| `vertical_bins` | int | 128 | The number of vertical bin for visibility histogram |
| `horizontal_bins` | int | 36 | The number of horizontal bin for visibility histogram |
| `noise_threshold` | int | 2 | The threshold value for distinguishing noise from valid points in the frequency image |

## Assumptions / Known limits

Expand Down
20 changes: 20 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 @@ -19,9 +19,17 @@
#include "pointcloud_preprocessor/filter.hpp"
#include "pointcloud_preprocessor/transform_info.hpp"

#include <image_transport/image_transport.hpp>
#include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>

#if __has_include(<cv_bridge/cv_bridge.hpp>)
#include <cv_bridge/cv_bridge.hpp>
#else
#include <cv_bridge/cv_bridge.h>
#endif

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

Expand All @@ -42,6 +50,8 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter
const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output,
const TransformInfo & transform_info);

rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr visibility_pub_;

private:
/** \brief publisher of excluded pointcloud for debug reason. **/
rclcpp::Publisher<PointCloud2>::SharedPtr outlier_pointcloud_publisher_;
Expand All @@ -53,6 +63,15 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter
size_t max_points_num_per_ring_;
bool publish_outlier_pointcloud_;

// for visibility score
int noise_threshold_;
int vertical_bins_;
int horizontal_bins_;

float min_azimuth_deg_;
float max_azimuth_deg_;
float max_distance_;

/** \brief Parameter service callback result : needed to be hold */
OnSetParametersCallbackHandle::SharedPtr set_param_res_;

Expand All @@ -77,6 +96,7 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter
void setUpPointCloudFormat(
const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size,
size_t num_fields);
float calculateVisibilityScore(const PointCloud2 & input);

public:
PCL_MAKE_ALIGNED_OPERATOR_NEW
Expand Down
1 change: 0 additions & 1 deletion sensing/pointcloud_preprocessor/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,6 @@
<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
169 changes: 137 additions & 32 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,16 +14,16 @@

#include "pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp"

#include "autoware_auto_geometry/common_3d.hpp"
#include "autoware_point_types/types.hpp"

#include <sensor_msgs/point_cloud2_iterator.hpp>

#include <pcl/search/pcl_search.h>

#include <algorithm>
#include <vector>
namespace pointcloud_preprocessor
{
using autoware_point_types::PointXYZIRADRT;

RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions & options)
: Filter("RingOutlierFilter", options)
{
Expand All @@ -35,6 +35,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions
debug_publisher_ = std::make_unique<DebugPublisher>(this, "ring_outlier_filter");
outlier_pointcloud_publisher_ =
this->create_publisher<PointCloud2>("debug/ring_outlier_filter", 1);
visibility_pub_ = create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
"ring_outlier_filter/debug/visibility", rclcpp::SensorDataQoS());
stop_watch_ptr_->tic("cyclic_time");
stop_watch_ptr_->tic("processing_time");
}
Expand All @@ -50,6 +52,13 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions
static_cast<size_t>(declare_parameter("max_points_num_per_ring", 4000));
publish_outlier_pointcloud_ =
static_cast<bool>(declare_parameter("publish_outlier_pointcloud", false));

min_azimuth_deg_ = static_cast<float>(declare_parameter("min_azimuth_deg", 0.0));
max_azimuth_deg_ = static_cast<float>(declare_parameter("max_azimuth_deg", 360.0));
max_distance_ = static_cast<float>(declare_parameter("max_distance", 12.0));
vertical_bins_ = static_cast<int>(declare_parameter("vertical_bins", 128));
horizontal_bins_ = static_cast<int>(declare_parameter("horizontal_bins", 36));
noise_threshold_ = static_cast<int>(declare_parameter("noise_threshold", 2));
}

using std::placeholders::_1;
Expand All @@ -73,13 +82,7 @@ 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);
}
pcl::PointCloud<PointXYZIRADRT>::Ptr outlier_pcl(new pcl::PointCloud<PointXYZIRADRT>);

const auto ring_offset =
input->fields.at(static_cast<size_t>(autoware_point_types::PointIndex::Ring)).offset;
Expand All @@ -89,6 +92,10 @@ void RingOutlierFilterComponent::faster_filter(
input->fields.at(static_cast<size_t>(autoware_point_types::PointIndex::Distance)).offset;
const auto intensity_offset =
input->fields.at(static_cast<size_t>(autoware_point_types::PointIndex::Intensity)).offset;
const auto return_type_offset =
input->fields.at(static_cast<size_t>(autoware_point_types::PointIndex::ReturnType)).offset;
const auto time_stamp_offset =
input->fields.at(static_cast<size_t>(autoware_point_types::PointIndex::TimeStamp)).offset;

std::vector<std::vector<size_t>> ring2indices;
ring2indices.reserve(max_rings_num_);
Expand Down Expand Up @@ -163,24 +170,32 @@ void RingOutlierFilterComponent::faster_filter(
}
} 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]);
PointXYZIRADRT outlier_point;
auto input_ptr =
reinterpret_cast<const PointXYZI *>(&input->data[indices[walk_first_idx]]);
reinterpret_cast<const PointXYZIRADRT *>(&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];
outlier_point.x = p[0];
outlier_point.y = p[1];
outlier_point.z = p[2];
} else {
*outlier_ptr = *input_ptr;
outlier_point = *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;
outlier_point.intensity = *reinterpret_cast<const float *>(
&input->data[indices[walk_first_idx] + intensity_offset]);
outlier_point.ring = *reinterpret_cast<const uint16_t *>(
&input->data[indices[walk_first_idx] + ring_offset]);
outlier_point.azimuth = *reinterpret_cast<const float *>(
&input->data[indices[walk_first_idx] + azimuth_offset]);
outlier_point.distance = *reinterpret_cast<const float *>(
&input->data[indices[walk_first_idx] + distance_offset]);
outlier_point.return_type = *reinterpret_cast<const uint8_t *>(
&input->data[indices[walk_first_idx] + return_type_offset]);
outlier_point.time_stamp = *reinterpret_cast<const float *>(
&input->data[indices[walk_first_idx] + time_stamp_offset]);
outlier_pcl->push_back(outlier_point);
}
}

Expand Down Expand Up @@ -213,30 +228,47 @@ void RingOutlierFilterComponent::faster_filter(
}
} 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]]);
PointXYZIRADRT outlier_point;

auto input_ptr = reinterpret_cast<const PointXYZIRADRT *>(&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];
outlier_point.x = p[0];
outlier_point.y = p[1];
outlier_point.z = p[2];
} else {
*outlier_ptr = *input_ptr;
outlier_point = *input_ptr;
}
const float & intensity =
outlier_point.intensity =
*reinterpret_cast<const float *>(&input->data[indices[i] + intensity_offset]);
outlier_ptr->intensity = intensity;
outlier_points_size += outlier_points.point_step;
outlier_point.ring =
*reinterpret_cast<const uint16_t *>(&input->data[indices[i] + ring_offset]);
outlier_point.azimuth =
*reinterpret_cast<const float *>(&input->data[indices[i] + azimuth_offset]);
outlier_point.distance =
*reinterpret_cast<const float *>(&input->data[indices[i] + distance_offset]);
outlier_point.return_type =
*reinterpret_cast<const uint8_t *>(&input->data[indices[i] + return_type_offset]);
outlier_point.time_stamp =
*reinterpret_cast<const float *>(&input->data[indices[i] + time_stamp_offset]);
outlier_pcl->push_back(outlier_point);
}
}
}

setUpPointCloudFormat(input, output, output_size, /*num_fields=*/4);

if (publish_outlier_pointcloud_) {
setUpPointCloudFormat(input, outlier_points, outlier_points_size, /*num_fields=*/4);
outlier_pointcloud_publisher_->publish(outlier_points);
PointCloud2 outlier;
pcl::toROSMsg(*outlier_pcl, outlier);
outlier.header = input->header;
outlier_pointcloud_publisher_->publish(outlier);

tier4_debug_msgs::msg::Float32Stamped visibility_msg;
visibility_msg.data = calculateVisibilityScore(outlier);
visibility_msg.stamp = input->header.stamp;
visibility_pub_->publish(visibility_msg);
}

// add processing time for debug
Expand Down Expand Up @@ -288,6 +320,24 @@ rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallba
RCLCPP_DEBUG(
get_logger(), "Setting new publish_outlier_pointcloud to: %d.", publish_outlier_pointcloud_);
}
if (get_param(p, "vertical_bins", vertical_bins_)) {
RCLCPP_DEBUG(get_logger(), "Setting new vertical_bins to: %d.", vertical_bins_);
}
if (get_param(p, "horizontal_bins", horizontal_bins_)) {
RCLCPP_DEBUG(get_logger(), "Setting new horizontal_bins to: %d.", horizontal_bins_);
}
if (get_param(p, "noise_threshold", noise_threshold_)) {
RCLCPP_DEBUG(get_logger(), "Setting new noise_threshold to: %d.", noise_threshold_);
}
if (get_param(p, "max_azimuth_deg", max_azimuth_deg_)) {
RCLCPP_DEBUG(get_logger(), "Setting new max_azimuth_deg to: %f.", max_azimuth_deg_);
}
if (get_param(p, "min_azimuth_deg", min_azimuth_deg_)) {
RCLCPP_DEBUG(get_logger(), "Setting new min_azimuth_deg to: %f.", min_azimuth_deg_);
}
if (get_param(p, "max_distance", max_distance_)) {
RCLCPP_DEBUG(get_logger(), "Setting new max_distance to: %f.", max_distance_);
}

rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
Expand Down Expand Up @@ -319,6 +369,61 @@ void RingOutlierFilterComponent::setUpPointCloudFormat(
"intensity", 1, sensor_msgs::msg::PointField::FLOAT32);
}

float RingOutlierFilterComponent::calculateVisibilityScore(
const sensor_msgs::msg::PointCloud2 & input)
{
pcl::PointCloud<PointXYZIRADRT>::Ptr input_cloud(new pcl::PointCloud<PointXYZIRADRT>);
pcl::fromROSMsg(input, *input_cloud);

const uint32_t vertical_bins = vertical_bins_;
const uint32_t horizontal_bins = horizontal_bins_;
const float max_azimuth = max_azimuth_deg_ * 100.0;
const float min_azimuth = min_azimuth_deg_ * 100.0;

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

std::vector<pcl::PointCloud<PointXYZIRADRT>> ring_point_clouds(vertical_bins);
cv::Mat frequency_image(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));

// Split points into rings
for (const auto & point : input_cloud->points) {
ring_point_clouds.at(point.ring).push_back(point);
}

// Calculate frequency for each bin in each ring
for (const auto & ring_points : ring_point_clouds) {
if (ring_points.empty()) continue;

const uint ring_id = ring_points.front().ring;
std::vector<int> frequency_in_ring(horizontal_bins, 0);

for (const auto & point : ring_points.points) {
if (point.azimuth < min_azimuth || point.azimuth >= max_azimuth) continue;
if (point.distance >= max_distance_) continue;

const uint bin_index =
static_cast<uint>((point.azimuth - min_azimuth) / horizontal_resolution);

frequency_in_ring[bin_index]++;
frequency_in_ring[bin_index] =
std::min(frequency_in_ring[bin_index], 255); // Ensure value is within uchar range

frequency_image.at<uchar>(ring_id, bin_index) =
static_cast<uchar>(frequency_in_ring[bin_index]);
}
}

cv::Mat binary_image;
cv::inRange(frequency_image, noise_threshold_, 255, binary_image);

const int num_pixels = cv::countNonZero(frequency_image);
const float num_filled_pixels =
static_cast<float>(num_pixels) / static_cast<float>(vertical_bins * horizontal_bins);

return 1.0f - num_filled_pixels;
}

} // namespace pointcloud_preprocessor

#include <rclcpp_components/register_node_macro.hpp>
Expand Down

0 comments on commit 81a75f7

Please sign in to comment.