@@ -33,8 +33,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions
33
33
using tier4_autoware_utils::StopWatch;
34
34
stop_watch_ptr_ = std::make_unique<StopWatch<std::chrono::milliseconds>>();
35
35
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 );
38
38
stop_watch_ptr_->tic (" cyclic_time" );
39
39
stop_watch_ptr_->tic (" processing_time" );
40
40
}
@@ -48,8 +48,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions
48
48
max_rings_num_ = static_cast <uint16_t >(declare_parameter (" max_rings_num" , 128 ));
49
49
max_points_num_per_ring_ =
50
50
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 ));
53
53
}
54
54
55
55
using std::placeholders::_1;
@@ -73,6 +73,14 @@ void RingOutlierFilterComponent::faster_filter(
73
73
output.data .resize (output.point_step * input->width );
74
74
size_t output_size = 0 ;
75
75
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
+
76
84
const auto ring_offset =
77
85
input->fields .at (static_cast <size_t >(autoware_point_types::PointIndex::Ring)).offset ;
78
86
const auto azimuth_offset =
@@ -153,6 +161,27 @@ void RingOutlierFilterComponent::faster_filter(
153
161
154
162
output_size += output.point_step ;
155
163
}
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
+ }
156
185
}
157
186
158
187
walk_first_idx = idx + 1 ;
@@ -182,37 +211,32 @@ void RingOutlierFilterComponent::faster_filter(
182
211
183
212
output_size += output.point_step ;
184
213
}
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
+ }
185
232
}
186
233
}
187
234
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 );
206
236
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);
216
240
}
217
241
218
242
// add processing time for debug
@@ -260,9 +284,9 @@ rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallba
260
284
if (get_param (p, " num_points_threshold" , num_points_threshold_)) {
261
285
RCLCPP_DEBUG (get_logger (), " Setting new num_points_threshold to: %d." , num_points_threshold_);
262
286
}
263
- if (get_param (p, " publish_excluded_points " , publish_excluded_points_ )) {
287
+ if (get_param (p, " publish_outlier_pointcloud " , publish_outlier_pointcloud_ )) {
264
288
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_ );
266
290
}
267
291
268
292
rcl_interfaces::msg::SetParametersResult result;
@@ -272,51 +296,27 @@ rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallba
272
296
return result;
273
297
}
274
298
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 )
278
302
{
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 =
317
307
!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);
320
320
}
321
321
322
322
} // namespace pointcloud_preprocessor
0 commit comments