Skip to content

Commit d85eabb

Browse files
change from publish_noise_points to publish_outlier_pointcloud
Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
1 parent 5743ba7 commit d85eabb

File tree

3 files changed

+42
-38
lines changed

3 files changed

+42
-38
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_noise_points` | bool | false | Flag to publish point cloud noise. 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

+2-2
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<PointCloud2>::SharedPtr noise_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_noise_points_;
54+
bool publish_outlier_pointcloud_;
5555

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

sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp

+32-28
Original file line numberDiff line numberDiff line change
@@ -33,7 +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-
noise_points_publisher_ = this->create_publisher<PointCloud2>("noise/ring_outlier_filter", 1);
36+
outlier_pointcloud_publisher_ =
37+
this->create_publisher<PointCloud2>("noise/ring_outlier_filter", 1);
3738
stop_watch_ptr_->tic("cyclic_time");
3839
stop_watch_ptr_->tic("processing_time");
3940
}
@@ -47,7 +48,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions
4748
max_rings_num_ = static_cast<uint16_t>(declare_parameter("max_rings_num", 128));
4849
max_points_num_per_ring_ =
4950
static_cast<size_t>(declare_parameter("max_points_num_per_ring", 4000));
50-
publish_noise_points_ = static_cast<bool>(declare_parameter("publish_noise_points", false));
51+
publish_outlier_pointcloud_ =
52+
static_cast<bool>(declare_parameter("publish_outlier_pointcloud", false));
5153
}
5254

5355
using std::placeholders::_1;
@@ -72,11 +74,11 @@ void RingOutlierFilterComponent::faster_filter(
7274
size_t output_size = 0;
7375

7476
// Set up the noise points cloud, if noise points are to be published.
75-
PointCloud2 noise_points;
76-
size_t noise_points_size = 0;
77-
if (publish_noise_points_) {
78-
noise_points.point_step = sizeof(PointXYZI);
79-
noise_points.data.resize(noise_points.point_step * input->width);
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);
8082
}
8183

8284
const auto ring_offset =
@@ -159,25 +161,26 @@ void RingOutlierFilterComponent::faster_filter(
159161

160162
output_size += output.point_step;
161163
}
162-
} else if (publish_noise_points_) {
164+
} else if (publish_outlier_pointcloud_) {
163165
for (int i = walk_first_idx; i <= walk_last_idx; i++) {
164-
auto noise_ptr = reinterpret_cast<PointXYZI *>(&noise_points.data[noise_points_size]);
166+
auto outlier_ptr =
167+
reinterpret_cast<PointXYZI *>(&outlier_points.data[outlier_points_size]);
165168
auto input_ptr =
166169
reinterpret_cast<const PointXYZI *>(&input->data[indices[walk_first_idx]]);
167170
if (transform_info.need_transform) {
168171
Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1);
169172
p = transform_info.eigen_transform * p;
170-
noise_ptr->x = p[0];
171-
noise_ptr->y = p[1];
172-
noise_ptr->z = p[2];
173+
outlier_ptr->x = p[0];
174+
outlier_ptr->y = p[1];
175+
outlier_ptr->z = p[2];
173176
} else {
174-
*noise_ptr = *input_ptr;
177+
*outlier_ptr = *input_ptr;
175178
}
176179
const float & intensity = *reinterpret_cast<const float *>(
177180
&input->data[indices[walk_first_idx] + intensity_offset]);
178-
noise_ptr->intensity = intensity;
181+
outlier_ptr->intensity = intensity;
179182

180-
noise_points_size += noise_points.point_step;
183+
outlier_points_size += outlier_points.point_step;
181184
}
182185
}
183186

@@ -208,32 +211,32 @@ void RingOutlierFilterComponent::faster_filter(
208211

209212
output_size += output.point_step;
210213
}
211-
} else if (publish_noise_points_) {
214+
} else if (publish_outlier_pointcloud_) {
212215
for (int i = walk_first_idx; i < walk_last_idx; i++) {
213-
auto noise_ptr = reinterpret_cast<PointXYZI *>(&noise_points.data[noise_points_size]);
216+
auto outlier_ptr = reinterpret_cast<PointXYZI *>(&outlier_points.data[outlier_points_size]);
214217
auto input_ptr = reinterpret_cast<const PointXYZI *>(&input->data[indices[i]]);
215218
if (transform_info.need_transform) {
216219
Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1);
217220
p = transform_info.eigen_transform * p;
218-
noise_ptr->x = p[0];
219-
noise_ptr->y = p[1];
220-
noise_ptr->z = p[2];
221+
outlier_ptr->x = p[0];
222+
outlier_ptr->y = p[1];
223+
outlier_ptr->z = p[2];
221224
} else {
222-
*noise_ptr = *input_ptr;
225+
*outlier_ptr = *input_ptr;
223226
}
224227
const float & intensity =
225228
*reinterpret_cast<const float *>(&input->data[indices[i] + intensity_offset]);
226-
noise_ptr->intensity = intensity;
227-
noise_points_size += noise_points.point_step;
229+
outlier_ptr->intensity = intensity;
230+
outlier_points_size += outlier_points.point_step;
228231
}
229232
}
230233
}
231234

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

234-
if (publish_noise_points_) {
235-
setUpPointCloudFormat(input, noise_points, noise_points_size, /*num_fields=*/4);
236-
noise_points_publisher_->publish(noise_points);
237+
if (publish_outlier_pointcloud_) {
238+
setUpPointCloudFormat(input, outlier_points, outlier_points_size, /*num_fields=*/4);
239+
outlier_pointcloud_publisher_->publish(outlier_points);
237240
}
238241

239242
// add processing time for debug
@@ -281,8 +284,9 @@ rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallba
281284
if (get_param(p, "num_points_threshold", num_points_threshold_)) {
282285
RCLCPP_DEBUG(get_logger(), "Setting new num_points_threshold to: %d.", num_points_threshold_);
283286
}
284-
if (get_param(p, "publish_noise_points", publish_noise_points_)) {
285-
RCLCPP_DEBUG(get_logger(), "Setting new publish_noise_points to: %d.", publish_noise_points_);
287+
if (get_param(p, "publish_outlier_pointcloud", publish_outlier_pointcloud_)) {
288+
RCLCPP_DEBUG(
289+
get_logger(), "Setting new publish_outlier_pointcloud to: %d.", publish_outlier_pointcloud_);
286290
}
287291

288292
rcl_interfaces::msg::SetParametersResult result;

0 commit comments

Comments
 (0)