perf(compare_map_segmentation): replace nearestKSearch by radiusSearch (

#2957) * perf(compare_map_segmentation): replace nearestKSearch by radiusSearch radiusSearch is faster than nearestKSearch as the search stops once no neighbor within the given radius is found (which is what we want to know here). Signed-off-by: Vincent Richard <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: Vincent Richard <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Mar 6, 2023 · 037ca68 · 037ca68
1 parent 47c9065
commit 037ca68
Showing 1 changed file with 3 additions and 2 deletions.
diff --git a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp
@@ -58,8 +58,9 @@ void VoxelDistanceBasedCompareMapFilterComponent::filter(
     if (index == -1) {                 // empty voxel
       std::vector<int> nn_indices(1);  // nn means nearest neighbor
       std::vector<float> nn_distances(1);
-      tree_->nearestKSearch(pcl_input->points.at(i), 1, nn_indices, nn_distances);
-      if (distance_threshold_ * distance_threshold_ < nn_distances.at(0)) {
+      if (
+        tree_->radiusSearch(
+          pcl_input->points.at(i), distance_threshold_, nn_indices, nn_distances, 1) == 0) {
         pcl_output->points.push_back(pcl_input->points.at(i));
       }
     }