|
18 | 18 |
|
19 | 19 | #include <sensor_msgs/point_cloud2_iterator.hpp>
|
20 | 20 |
|
| 21 | +#include <pcl/search/pcl_search.h> |
| 22 | + |
21 | 23 | #include <algorithm>
|
22 | 24 | #include <vector>
|
23 | 25 | namespace pointcloud_preprocessor
|
@@ -282,23 +284,21 @@ sensor_msgs::msg::PointCloud2 RingOutlierFilterComponent::extractExcludedPoints(
|
282 | 284 |
|
283 | 285 | pcl::PointCloud<pcl::PointXYZ>::Ptr excluded_points(new pcl::PointCloud<pcl::PointXYZ>);
|
284 | 286 |
|
285 |
| - for (const auto & input_point : *input_cloud) { |
286 |
| - bool is_excluded = true; |
287 |
| - |
288 |
| - for (const auto & output_point : *output_cloud) { |
289 |
| - float distance = autoware::common::geometry::distance_3d(input_point, output_point); |
290 |
| - |
291 |
| - // If the distance is less than the threshold (epsilon), the point is not excluded |
292 |
| - if (distance < epsilon) { |
293 |
| - is_excluded = false; |
294 |
| - break; |
295 |
| - } |
| 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_dists(1); |
| 296 | + for (const auto & point : input_cloud->points) { |
| 297 | + if (!tree->nearestKSearch(point, 1, nn_indices, nn_dists)) { |
| 298 | + continue; |
296 | 299 | }
|
297 |
| - |
298 |
| - // If the point is still marked as excluded after all comparisons, add it to the excluded |
299 |
| - // pointcloud |
300 |
| - if (is_excluded) { |
301 |
| - excluded_points->push_back(input_point); |
| 300 | + if (nn_dists[0] > epsilon) { |
| 301 | + excluded_points->points.push_back(point); |
302 | 302 | }
|
303 | 303 | }
|
304 | 304 |
|
|
0 commit comments