From 7881dd76410a09c0b586e60f8111d26e4b0d8063 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Fri, 10 Jun 2022 14:33:46 +0900 Subject: [PATCH] feat: reduce calc cost of occ grid outlier filter (#59) Signed-off-by: Yukihiro Saito Co-authored-by: Yukihiro Saito --- .../src/occupancy_grid_map_outlier_filter_nodelet.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index 9bea2304296b8..50650254028e5 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/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(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(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));