Skip to content

Commit 283ff47

Browse files
refactor(pointcloud_preprocessor): publish noise points in ring outlier 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 <kyoichi.sugahara@tier4.jp> --------- Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
1 parent 35d2143 commit 283ff47

File tree

4 files changed

+96
-91
lines changed

4 files changed

+96
-91
lines changed

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

+8-8
Original file line numberDiff line numberDiff line change
@@ -22,14 +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` |
32-
| `publish_excluded_points` | bool | false | Flag to publish excluded pointcloud for debugging purpose. Due to performance concerns, please set to false during experiments. |
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. |
3333

3434
## Assumptions / Known limits
3535

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

+6-4
Original file line numberDiff line numberDiff line change
@@ -44,14 +44,14 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter
4444

4545
private:
4646
/** \brief publisher of excluded pointcloud for debug reason. **/
47-
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr excluded_points_publisher_;
47+
rclcpp::Publisher<PointCloud2>::SharedPtr outlier_pointcloud_publisher_;
4848

4949
double distance_ratio_;
5050
double object_length_threshold_;
5151
int num_points_threshold_;
5252
uint16_t max_rings_num_;
5353
size_t max_points_num_per_ring_;
54-
bool publish_excluded_points_;
54+
bool publish_outlier_pointcloud_;
5555

5656
/** \brief Parameter service callback result : needed to be hold */
5757
OnSetParametersCallbackHandle::SharedPtr set_param_res_;
@@ -73,8 +73,10 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter
7373

7474
return x * x + y * y + z * z >= object_length_threshold_ * object_length_threshold_;
7575
}
76-
PointCloud2 extractExcludedPoints(
77-
const PointCloud2 & input, const PointCloud2 & output, float epsilon);
76+
77+
void setUpPointCloudFormat(
78+
const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size,
79+
size_t num_fields);
7880

7981
public:
8082
PCL_MAKE_ALIGNED_OPERATOR_NEW

sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp

+6-3
Original file line numberDiff line numberDiff line change
@@ -139,7 +139,8 @@ void DualReturnOutlierFilterComponent::filter(
139139
}
140140
}
141141

142-
uint32_t horizontal_res = static_cast<uint32_t>((max_azimuth - min_azimuth) / horizontal_bins);
142+
uint32_t horizontal_resolution =
143+
static_cast<uint32_t>((max_azimuth - min_azimuth) / horizontal_bins);
143144

144145
pcl::PointCloud<PointXYZIRADRT>::Ptr pcl_output(new pcl::PointCloud<PointXYZIRADRT>);
145146
pcl_output->points.reserve(pcl_input->points.size());
@@ -231,7 +232,8 @@ void DualReturnOutlierFilterComponent::filter(
231232
continue;
232233
}
233234
while ((uint)deleted_azimuths[current_deleted_index] <
234-
((i + static_cast<uint>(min_azimuth / horizontal_res) + 1) * horizontal_res) &&
235+
((i + static_cast<uint>(min_azimuth / horizontal_resolution) + 1) *
236+
horizontal_resolution) &&
235237
current_deleted_index < (deleted_azimuths.size() - 1)) {
236238
noise_frequency[i] = noise_frequency[i] + 1;
237239
current_deleted_index++;
@@ -240,7 +242,8 @@ void DualReturnOutlierFilterComponent::filter(
240242
while ((temp_segment.points[current_temp_segment_index].azimuth < 0.f
241243
? 0.f
242244
: temp_segment.points[current_temp_segment_index].azimuth) <
243-
((i + 1 + static_cast<uint>(min_azimuth / horizontal_res)) * horizontal_res) &&
245+
((i + 1 + static_cast<uint>(min_azimuth / horizontal_resolution)) *
246+
horizontal_resolution) &&
244247
current_temp_segment_index < (temp_segment.points.size() - 1)) {
245248
if (noise_frequency[i] < weak_first_local_noise_threshold_) {
246249
pcl_output->points.push_back(temp_segment.points[current_temp_segment_index]);

sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp

+76-76
Original file line numberDiff line numberDiff line change
@@ -33,8 +33,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions
3333
using tier4_autoware_utils::StopWatch;
3434
stop_watch_ptr_ = std::make_unique<StopWatch<std::chrono::milliseconds>>();
3535
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);
36+
outlier_pointcloud_publisher_ =
37+
this->create_publisher<PointCloud2>("debug/ring_outlier_filter", 1);
3838
stop_watch_ptr_->tic("cyclic_time");
3939
stop_watch_ptr_->tic("processing_time");
4040
}
@@ -48,8 +48,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions
4848
max_rings_num_ = static_cast<uint16_t>(declare_parameter("max_rings_num", 128));
4949
max_points_num_per_ring_ =
5050
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));
51+
publish_outlier_pointcloud_ =
52+
static_cast<bool>(declare_parameter("publish_outlier_pointcloud", false));
5353
}
5454

5555
using std::placeholders::_1;
@@ -73,6 +73,14 @@ void RingOutlierFilterComponent::faster_filter(
7373
output.data.resize(output.point_step * input->width);
7474
size_t output_size = 0;
7575

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+
}
83+
7684
const auto ring_offset =
7785
input->fields.at(static_cast<size_t>(autoware_point_types::PointIndex::Ring)).offset;
7886
const auto azimuth_offset =
@@ -153,6 +161,27 @@ void RingOutlierFilterComponent::faster_filter(
153161

154162
output_size += output.point_step;
155163
}
164+
} else if (publish_outlier_pointcloud_) {
165+
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]);
168+
auto input_ptr =
169+
reinterpret_cast<const PointXYZI *>(&input->data[indices[walk_first_idx]]);
170+
if (transform_info.need_transform) {
171+
Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1);
172+
p = transform_info.eigen_transform * p;
173+
outlier_ptr->x = p[0];
174+
outlier_ptr->y = p[1];
175+
outlier_ptr->z = p[2];
176+
} else {
177+
*outlier_ptr = *input_ptr;
178+
}
179+
const float & intensity = *reinterpret_cast<const float *>(
180+
&input->data[indices[walk_first_idx] + intensity_offset]);
181+
outlier_ptr->intensity = intensity;
182+
183+
outlier_points_size += outlier_points.point_step;
184+
}
156185
}
157186

158187
walk_first_idx = idx + 1;
@@ -182,37 +211,32 @@ void RingOutlierFilterComponent::faster_filter(
182211

183212
output_size += output.point_step;
184213
}
214+
} else if (publish_outlier_pointcloud_) {
215+
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]]);
218+
if (transform_info.need_transform) {
219+
Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1);
220+
p = transform_info.eigen_transform * p;
221+
outlier_ptr->x = p[0];
222+
outlier_ptr->y = p[1];
223+
outlier_ptr->z = p[2];
224+
} else {
225+
*outlier_ptr = *input_ptr;
226+
}
227+
const float & intensity =
228+
*reinterpret_cast<const float *>(&input->data[indices[i] + intensity_offset]);
229+
outlier_ptr->intensity = intensity;
230+
outlier_points_size += outlier_points.point_step;
231+
}
185232
}
186233
}
187234

188-
output.data.resize(output_size);
189-
190-
// Note that `input->header.frame_id` is data before converted when `transform_info.need_transform
191-
// == true`
192-
output.header.frame_id = !tf_input_frame_.empty() ? tf_input_frame_ : tf_input_orig_frame_;
193-
194-
output.height = 1;
195-
output.width = static_cast<uint32_t>(output.data.size() / output.height / output.point_step);
196-
output.is_bigendian = input->is_bigendian;
197-
output.is_dense = input->is_dense;
198-
199-
// set fields
200-
sensor_msgs::PointCloud2Modifier pcd_modifier(output);
201-
constexpr int num_fields = 4;
202-
pcd_modifier.setPointCloud2Fields(
203-
num_fields, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1,
204-
sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32,
205-
"intensity", 1, sensor_msgs::msg::PointField::FLOAT32);
235+
setUpPointCloudFormat(input, output, output_size, /*num_fields=*/4);
206236

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);
237+
if (publish_outlier_pointcloud_) {
238+
setUpPointCloudFormat(input, outlier_points, outlier_points_size, /*num_fields=*/4);
239+
outlier_pointcloud_publisher_->publish(outlier_points);
216240
}
217241

218242
// add processing time for debug
@@ -260,9 +284,9 @@ rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallba
260284
if (get_param(p, "num_points_threshold", num_points_threshold_)) {
261285
RCLCPP_DEBUG(get_logger(), "Setting new num_points_threshold to: %d.", num_points_threshold_);
262286
}
263-
if (get_param(p, "publish_excluded_points", publish_excluded_points_)) {
287+
if (get_param(p, "publish_outlier_pointcloud", publish_outlier_pointcloud_)) {
264288
RCLCPP_DEBUG(
265-
get_logger(), "Setting new publish_excluded_points to: %d.", publish_excluded_points_);
289+
get_logger(), "Setting new publish_outlier_pointcloud to: %d.", publish_outlier_pointcloud_);
266290
}
267291

268292
rcl_interfaces::msg::SetParametersResult result;
@@ -272,51 +296,27 @@ rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallba
272296
return result;
273297
}
274298

275-
sensor_msgs::msg::PointCloud2 RingOutlierFilterComponent::extractExcludedPoints(
276-
const sensor_msgs::msg::PointCloud2 & input, const sensor_msgs::msg::PointCloud2 & output,
277-
float epsilon)
299+
void RingOutlierFilterComponent::setUpPointCloudFormat(
300+
const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size,
301+
size_t num_fields)
278302
{
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 =
303+
formatted_points.data.resize(points_size);
304+
// Note that `input->header.frame_id` is data before converted when `transform_info.need_transform
305+
// == true`
306+
formatted_points.header.frame_id =
317307
!tf_input_frame_.empty() ? tf_input_frame_ : tf_input_orig_frame_;
318-
319-
return excluded_points_msg;
308+
formatted_points.data.resize(formatted_points.point_step * input->width);
309+
formatted_points.height = 1;
310+
formatted_points.width =
311+
static_cast<uint32_t>(formatted_points.data.size() / formatted_points.point_step);
312+
formatted_points.is_bigendian = input->is_bigendian;
313+
formatted_points.is_dense = input->is_dense;
314+
315+
sensor_msgs::PointCloud2Modifier pcd_modifier(formatted_points);
316+
pcd_modifier.setPointCloud2Fields(
317+
num_fields, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1,
318+
sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32,
319+
"intensity", 1, sensor_msgs::msg::PointField::FLOAT32);
320320
}
321321

322322
} // namespace pointcloud_preprocessor

0 commit comments

Comments
 (0)