Skip to content

Commit 81a75f7

Browse files
feat(pointcloud_preprocessor): calculate visibility score in ring outlier filter (#7011)
* calculate visibility score in ring outlier filter Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> --------- Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
1 parent 0518585 commit 81a75f7

File tree

4 files changed

+205
-41
lines changed

4 files changed

+205
-41
lines changed

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

+48-8
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,40 @@ A method of operating scan in chronological order and removing noise based on th
1010

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

13+
Another feature of this node is that it calculates visibility score based on outlier pointcloud and publish score as a topic.
14+
15+
### visibility score calculation algorithm
16+
17+
The pointcloud is divided into vertical bins (rings) and horizontal bins (azimuth divisions).
18+
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.
19+
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.
20+
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).
21+
22+
```plantuml
23+
@startuml
24+
start
25+
26+
:Convert input point cloud to PCL format;
27+
28+
:Initialize vertical and horizontal bins;
29+
30+
:Split point cloud into rings;
31+
32+
while (For each ring) is (not empty)
33+
:Calculate frequency of points in each azimuth bin;
34+
:Update frequency image matrix;
35+
endwhile
36+
37+
:Apply noise threshold to create binary image;
38+
39+
:Count non-zero pixels in frequency image;
40+
41+
:Calculate visibility score as complement of filled pixel ratio;
42+
43+
stop
44+
@enduml
45+
```
46+
1347
## Inputs / Outputs
1448

1549
This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md).
@@ -22,14 +56,20 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref
2256

2357
### Core Parameters
2458

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_outlier_pointcloud` | bool | false | Flag to publish outlier pointcloud. Due to performance concerns, please set to false during experiments. |
59+
| Name | Type | Default Value | Description |
60+
| ---------------------------- | ------- | ------------- | ----------------------------------------------------------------------------------------------------------------------------- |
61+
| `distance_ratio` | double | 1.03 | |
62+
| `object_length_threshold` | double | 0.1 | |
63+
| `num_points_threshold` | int | 4 | |
64+
| `max_rings_num` | uint_16 | 128 | |
65+
| `max_points_num_per_ring` | size_t | 4000 | Set this value large enough such that `HFoV / resolution < max_points_num_per_ring` |
66+
| `publish_outlier_pointcloud` | bool | false | Flag to publish outlier pointcloud and visibility score. Due to performance concerns, please set to false during experiments. |
67+
| `min_azimuth_deg` | float | 0.0 | The left limit of azimuth for visibility score calculation |
68+
| `max_azimuth_deg` | float | 360.0 | The right limit of azimuth for visibility score calculation |
69+
| `max_distance` | float | 12.0 | The limit distance for visibility score calculation |
70+
| `vertical_bins` | int | 128 | The number of vertical bin for visibility histogram |
71+
| `horizontal_bins` | int | 36 | The number of horizontal bin for visibility histogram |
72+
| `noise_threshold` | int | 2 | The threshold value for distinguishing noise from valid points in the frequency image |
3373

3474
## Assumptions / Known limits
3575

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

+20
Original file line numberDiff line numberDiff line change
@@ -19,9 +19,17 @@
1919
#include "pointcloud_preprocessor/filter.hpp"
2020
#include "pointcloud_preprocessor/transform_info.hpp"
2121

22+
#include <image_transport/image_transport.hpp>
2223
#include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>
2324

25+
#if __has_include(<cv_bridge/cv_bridge.hpp>)
26+
#include <cv_bridge/cv_bridge.hpp>
27+
#else
28+
#include <cv_bridge/cv_bridge.h>
29+
#endif
30+
2431
#include <memory>
32+
#include <string>
2533
#include <utility>
2634
#include <vector>
2735

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

53+
rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr visibility_pub_;
54+
4555
private:
4656
/** \brief publisher of excluded pointcloud for debug reason. **/
4757
rclcpp::Publisher<PointCloud2>::SharedPtr outlier_pointcloud_publisher_;
@@ -53,6 +63,15 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter
5363
size_t max_points_num_per_ring_;
5464
bool publish_outlier_pointcloud_;
5565

66+
// for visibility score
67+
int noise_threshold_;
68+
int vertical_bins_;
69+
int horizontal_bins_;
70+
71+
float min_azimuth_deg_;
72+
float max_azimuth_deg_;
73+
float max_distance_;
74+
5675
/** \brief Parameter service callback result : needed to be hold */
5776
OnSetParametersCallbackHandle::SharedPtr set_param_res_;
5877

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

81101
public:
82102
PCL_MAKE_ALIGNED_OPERATOR_NEW

sensing/pointcloud_preprocessor/package.xml

-1
Original file line numberDiff line numberDiff line change
@@ -22,7 +22,6 @@
2222
<buildtool_depend>ament_cmake_auto</buildtool_depend>
2323
<buildtool_depend>autoware_cmake</buildtool_depend>
2424

25-
<depend>autoware_auto_geometry</depend>
2625
<depend>autoware_point_types</depend>
2726
<depend>cgal</depend>
2827
<depend>cv_bridge</depend>

sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp

+137-32
Original file line numberDiff line numberDiff line change
@@ -14,16 +14,16 @@
1414

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

17-
#include "autoware_auto_geometry/common_3d.hpp"
17+
#include "autoware_point_types/types.hpp"
1818

1919
#include <sensor_msgs/point_cloud2_iterator.hpp>
2020

21-
#include <pcl/search/pcl_search.h>
22-
2321
#include <algorithm>
2422
#include <vector>
2523
namespace pointcloud_preprocessor
2624
{
25+
using autoware_point_types::PointXYZIRADRT;
26+
2727
RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions & options)
2828
: Filter("RingOutlierFilter", options)
2929
{
@@ -35,6 +35,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions
3535
debug_publisher_ = std::make_unique<DebugPublisher>(this, "ring_outlier_filter");
3636
outlier_pointcloud_publisher_ =
3737
this->create_publisher<PointCloud2>("debug/ring_outlier_filter", 1);
38+
visibility_pub_ = create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
39+
"ring_outlier_filter/debug/visibility", rclcpp::SensorDataQoS());
3840
stop_watch_ptr_->tic("cyclic_time");
3941
stop_watch_ptr_->tic("processing_time");
4042
}
@@ -50,6 +52,13 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions
5052
static_cast<size_t>(declare_parameter("max_points_num_per_ring", 4000));
5153
publish_outlier_pointcloud_ =
5254
static_cast<bool>(declare_parameter("publish_outlier_pointcloud", false));
55+
56+
min_azimuth_deg_ = static_cast<float>(declare_parameter("min_azimuth_deg", 0.0));
57+
max_azimuth_deg_ = static_cast<float>(declare_parameter("max_azimuth_deg", 360.0));
58+
max_distance_ = static_cast<float>(declare_parameter("max_distance", 12.0));
59+
vertical_bins_ = static_cast<int>(declare_parameter("vertical_bins", 128));
60+
horizontal_bins_ = static_cast<int>(declare_parameter("horizontal_bins", 36));
61+
noise_threshold_ = static_cast<int>(declare_parameter("noise_threshold", 2));
5362
}
5463

5564
using std::placeholders::_1;
@@ -73,13 +82,7 @@ void RingOutlierFilterComponent::faster_filter(
7382
output.data.resize(output.point_step * input->width);
7483
size_t output_size = 0;
7584

76-
// Set up the noise points cloud, if noise points are to be published.
77-
PointCloud2 outlier_points;
78-
size_t outlier_points_size = 0;
79-
if (publish_outlier_pointcloud_) {
80-
outlier_points.point_step = sizeof(PointXYZI);
81-
outlier_points.data.resize(outlier_points.point_step * input->width);
82-
}
85+
pcl::PointCloud<PointXYZIRADRT>::Ptr outlier_pcl(new pcl::PointCloud<PointXYZIRADRT>);
8386

8487
const auto ring_offset =
8588
input->fields.at(static_cast<size_t>(autoware_point_types::PointIndex::Ring)).offset;
@@ -89,6 +92,10 @@ void RingOutlierFilterComponent::faster_filter(
8992
input->fields.at(static_cast<size_t>(autoware_point_types::PointIndex::Distance)).offset;
9093
const auto intensity_offset =
9194
input->fields.at(static_cast<size_t>(autoware_point_types::PointIndex::Intensity)).offset;
95+
const auto return_type_offset =
96+
input->fields.at(static_cast<size_t>(autoware_point_types::PointIndex::ReturnType)).offset;
97+
const auto time_stamp_offset =
98+
input->fields.at(static_cast<size_t>(autoware_point_types::PointIndex::TimeStamp)).offset;
9299

93100
std::vector<std::vector<size_t>> ring2indices;
94101
ring2indices.reserve(max_rings_num_);
@@ -163,24 +170,32 @@ void RingOutlierFilterComponent::faster_filter(
163170
}
164171
} else if (publish_outlier_pointcloud_) {
165172
for (int i = walk_first_idx; i <= walk_last_idx; i++) {
166-
auto outlier_ptr =
167-
reinterpret_cast<PointXYZI *>(&outlier_points.data[outlier_points_size]);
173+
PointXYZIRADRT outlier_point;
168174
auto input_ptr =
169-
reinterpret_cast<const PointXYZI *>(&input->data[indices[walk_first_idx]]);
175+
reinterpret_cast<const PointXYZIRADRT *>(&input->data[indices[walk_first_idx]]);
170176
if (transform_info.need_transform) {
171177
Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1);
172178
p = transform_info.eigen_transform * p;
173-
outlier_ptr->x = p[0];
174-
outlier_ptr->y = p[1];
175-
outlier_ptr->z = p[2];
179+
outlier_point.x = p[0];
180+
outlier_point.y = p[1];
181+
outlier_point.z = p[2];
176182
} else {
177-
*outlier_ptr = *input_ptr;
183+
outlier_point = *input_ptr;
178184
}
179-
const float & intensity = *reinterpret_cast<const float *>(
180-
&input->data[indices[walk_first_idx] + intensity_offset]);
181-
outlier_ptr->intensity = intensity;
182185

183-
outlier_points_size += outlier_points.point_step;
186+
outlier_point.intensity = *reinterpret_cast<const float *>(
187+
&input->data[indices[walk_first_idx] + intensity_offset]);
188+
outlier_point.ring = *reinterpret_cast<const uint16_t *>(
189+
&input->data[indices[walk_first_idx] + ring_offset]);
190+
outlier_point.azimuth = *reinterpret_cast<const float *>(
191+
&input->data[indices[walk_first_idx] + azimuth_offset]);
192+
outlier_point.distance = *reinterpret_cast<const float *>(
193+
&input->data[indices[walk_first_idx] + distance_offset]);
194+
outlier_point.return_type = *reinterpret_cast<const uint8_t *>(
195+
&input->data[indices[walk_first_idx] + return_type_offset]);
196+
outlier_point.time_stamp = *reinterpret_cast<const float *>(
197+
&input->data[indices[walk_first_idx] + time_stamp_offset]);
198+
outlier_pcl->push_back(outlier_point);
184199
}
185200
}
186201

@@ -213,30 +228,47 @@ void RingOutlierFilterComponent::faster_filter(
213228
}
214229
} else if (publish_outlier_pointcloud_) {
215230
for (int i = walk_first_idx; i < walk_last_idx; i++) {
216-
auto outlier_ptr = reinterpret_cast<PointXYZI *>(&outlier_points.data[outlier_points_size]);
217-
auto input_ptr = reinterpret_cast<const PointXYZI *>(&input->data[indices[i]]);
231+
PointXYZIRADRT outlier_point;
232+
233+
auto input_ptr = reinterpret_cast<const PointXYZIRADRT *>(&input->data[indices[i]]);
218234
if (transform_info.need_transform) {
219235
Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1);
220236
p = transform_info.eigen_transform * p;
221-
outlier_ptr->x = p[0];
222-
outlier_ptr->y = p[1];
223-
outlier_ptr->z = p[2];
237+
outlier_point.x = p[0];
238+
outlier_point.y = p[1];
239+
outlier_point.z = p[2];
224240
} else {
225-
*outlier_ptr = *input_ptr;
241+
outlier_point = *input_ptr;
226242
}
227-
const float & intensity =
243+
outlier_point.intensity =
228244
*reinterpret_cast<const float *>(&input->data[indices[i] + intensity_offset]);
229-
outlier_ptr->intensity = intensity;
230-
outlier_points_size += outlier_points.point_step;
245+
outlier_point.ring =
246+
*reinterpret_cast<const uint16_t *>(&input->data[indices[i] + ring_offset]);
247+
outlier_point.azimuth =
248+
*reinterpret_cast<const float *>(&input->data[indices[i] + azimuth_offset]);
249+
outlier_point.distance =
250+
*reinterpret_cast<const float *>(&input->data[indices[i] + distance_offset]);
251+
outlier_point.return_type =
252+
*reinterpret_cast<const uint8_t *>(&input->data[indices[i] + return_type_offset]);
253+
outlier_point.time_stamp =
254+
*reinterpret_cast<const float *>(&input->data[indices[i] + time_stamp_offset]);
255+
outlier_pcl->push_back(outlier_point);
231256
}
232257
}
233258
}
234259

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

237262
if (publish_outlier_pointcloud_) {
238-
setUpPointCloudFormat(input, outlier_points, outlier_points_size, /*num_fields=*/4);
239-
outlier_pointcloud_publisher_->publish(outlier_points);
263+
PointCloud2 outlier;
264+
pcl::toROSMsg(*outlier_pcl, outlier);
265+
outlier.header = input->header;
266+
outlier_pointcloud_publisher_->publish(outlier);
267+
268+
tier4_debug_msgs::msg::Float32Stamped visibility_msg;
269+
visibility_msg.data = calculateVisibilityScore(outlier);
270+
visibility_msg.stamp = input->header.stamp;
271+
visibility_pub_->publish(visibility_msg);
240272
}
241273

242274
// add processing time for debug
@@ -288,6 +320,24 @@ rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallba
288320
RCLCPP_DEBUG(
289321
get_logger(), "Setting new publish_outlier_pointcloud to: %d.", publish_outlier_pointcloud_);
290322
}
323+
if (get_param(p, "vertical_bins", vertical_bins_)) {
324+
RCLCPP_DEBUG(get_logger(), "Setting new vertical_bins to: %d.", vertical_bins_);
325+
}
326+
if (get_param(p, "horizontal_bins", horizontal_bins_)) {
327+
RCLCPP_DEBUG(get_logger(), "Setting new horizontal_bins to: %d.", horizontal_bins_);
328+
}
329+
if (get_param(p, "noise_threshold", noise_threshold_)) {
330+
RCLCPP_DEBUG(get_logger(), "Setting new noise_threshold to: %d.", noise_threshold_);
331+
}
332+
if (get_param(p, "max_azimuth_deg", max_azimuth_deg_)) {
333+
RCLCPP_DEBUG(get_logger(), "Setting new max_azimuth_deg to: %f.", max_azimuth_deg_);
334+
}
335+
if (get_param(p, "min_azimuth_deg", min_azimuth_deg_)) {
336+
RCLCPP_DEBUG(get_logger(), "Setting new min_azimuth_deg to: %f.", min_azimuth_deg_);
337+
}
338+
if (get_param(p, "max_distance", max_distance_)) {
339+
RCLCPP_DEBUG(get_logger(), "Setting new max_distance to: %f.", max_distance_);
340+
}
291341

292342
rcl_interfaces::msg::SetParametersResult result;
293343
result.successful = true;
@@ -319,6 +369,61 @@ void RingOutlierFilterComponent::setUpPointCloudFormat(
319369
"intensity", 1, sensor_msgs::msg::PointField::FLOAT32);
320370
}
321371

372+
float RingOutlierFilterComponent::calculateVisibilityScore(
373+
const sensor_msgs::msg::PointCloud2 & input)
374+
{
375+
pcl::PointCloud<PointXYZIRADRT>::Ptr input_cloud(new pcl::PointCloud<PointXYZIRADRT>);
376+
pcl::fromROSMsg(input, *input_cloud);
377+
378+
const uint32_t vertical_bins = vertical_bins_;
379+
const uint32_t horizontal_bins = horizontal_bins_;
380+
const float max_azimuth = max_azimuth_deg_ * 100.0;
381+
const float min_azimuth = min_azimuth_deg_ * 100.0;
382+
383+
const uint32_t horizontal_resolution =
384+
static_cast<uint32_t>((max_azimuth - min_azimuth) / horizontal_bins);
385+
386+
std::vector<pcl::PointCloud<PointXYZIRADRT>> ring_point_clouds(vertical_bins);
387+
cv::Mat frequency_image(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));
388+
389+
// Split points into rings
390+
for (const auto & point : input_cloud->points) {
391+
ring_point_clouds.at(point.ring).push_back(point);
392+
}
393+
394+
// Calculate frequency for each bin in each ring
395+
for (const auto & ring_points : ring_point_clouds) {
396+
if (ring_points.empty()) continue;
397+
398+
const uint ring_id = ring_points.front().ring;
399+
std::vector<int> frequency_in_ring(horizontal_bins, 0);
400+
401+
for (const auto & point : ring_points.points) {
402+
if (point.azimuth < min_azimuth || point.azimuth >= max_azimuth) continue;
403+
if (point.distance >= max_distance_) continue;
404+
405+
const uint bin_index =
406+
static_cast<uint>((point.azimuth - min_azimuth) / horizontal_resolution);
407+
408+
frequency_in_ring[bin_index]++;
409+
frequency_in_ring[bin_index] =
410+
std::min(frequency_in_ring[bin_index], 255); // Ensure value is within uchar range
411+
412+
frequency_image.at<uchar>(ring_id, bin_index) =
413+
static_cast<uchar>(frequency_in_ring[bin_index]);
414+
}
415+
}
416+
417+
cv::Mat binary_image;
418+
cv::inRange(frequency_image, noise_threshold_, 255, binary_image);
419+
420+
const int num_pixels = cv::countNonZero(frequency_image);
421+
const float num_filled_pixels =
422+
static_cast<float>(num_pixels) / static_cast<float>(vertical_bins * horizontal_bins);
423+
424+
return 1.0f - num_filled_pixels;
425+
}
426+
322427
} // namespace pointcloud_preprocessor
323428

324429
#include <rclcpp_components/register_node_macro.hpp>

0 commit comments

Comments
 (0)