feat: reduce calc cost of occ grid outlier filter (autowarefoundation#59

) Signed-off-by: Yukihiro Saito <[email protected]> Co-authored-by: Yukihiro Saito <[email protected]>
tkimura4 · Jun 10, 2022 · 7881dd7 · 7881dd7
1 parent 4b7cc85
commit 7881dd7
Showing 1 changed file with 4 additions and 2 deletions.
diff --git a/...ption/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/...ption/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp
@@ -126,7 +126,8 @@ void RadiusSearch2dfilter::filter(
     const int min_points_threshold = std::min(
       std::max(static_cast<int>(min_points_and_distance_ratio_ / distance + 0.5f), min_points_),
       max_points_);
-    const int points_num = kd_tree_->radiusSearch(i, search_radius_, k_indices, k_dists);
+    const int points_num =
+      kd_tree_->radiusSearch(i, search_radius_, k_indices, k_dists, min_points_threshold);
 
     if (min_points_threshold <= points_num) {
       output.points.push_back(xyz_cloud.points.at(i));
@@ -162,7 +163,8 @@ void RadiusSearch2dfilter::filter(
     const int min_points_threshold = std::min(
       std::max(static_cast<int>(min_points_and_distance_ratio_ / distance + 0.5f), min_points_),
       max_points_);
-    const int points_num = kd_tree_->radiusSearch(i, search_radius_, k_indices, k_dists);
+    const int points_num =
+      kd_tree_->radiusSearch(i, search_radius_, k_indices, k_dists, min_points_threshold);
 
     if (min_points_threshold <= points_num) {
       output.points.push_back(low_conf_xyz_cloud.points.at(i));