From e94acd04c483da28fa6155aec744cc88bb68a2d6 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Thu, 21 Mar 2024 12:43:52 +0900 Subject: [PATCH] Removed distances argument from radiusSearch Signed-off-by: Shintaro Sakoda --- .../multi_voxel_grid_covariance_omp.h | 6 ++---- .../multi_voxel_grid_covariance_omp_impl.hpp | 7 ++++--- include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp | 13 +++++-------- 3 files changed, 11 insertions(+), 15 deletions(-) diff --git a/include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h b/include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h index 33bfaee9..283453f4 100644 --- a/include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h +++ b/include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h @@ -278,11 +278,10 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid { * \param[in] point the given query point * \param[in] radius the radius of the sphere bounding all of p_q's neighbors * \param[out] k_leaves the resultant leaves of the neighboring points - * \param[out] k_sqr_distances the resultant squared distances to the neighboring points * \param[in] max_nn * \return number of neighbors found */ - int radiusSearch(const PointT &point, double radius, std::vector &k_leaves, std::vector &k_sqr_distances, unsigned int max_nn = 0) const; + int radiusSearch(const PointT &point, double radius, std::vector &k_leaves, unsigned int max_nn = 0) const; /** \brief Search for all the nearest occupied voxels of the query point in a given radius. * \note Only voxels containing a sufficient number of points are used. @@ -290,11 +289,10 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid { * \param[in] index a valid index in cloud representing a valid (i.e., finite) query point * \param[in] radius the radius of the sphere bounding all of p_q's neighbors * \param[out] k_leaves the resultant leaves of the neighboring points - * \param[out] k_sqr_distances the resultant squared distances to the neighboring points * \param[in] max_nn * \return number of neighbors found */ - int radiusSearch(const PointCloud &cloud, int index, double radius, std::vector &k_leaves, std::vector &k_sqr_distances, unsigned int max_nn = 0) const; + int radiusSearch(const PointCloud &cloud, int index, double radius, std::vector &k_leaves, unsigned int max_nn = 0) const; PointCloud getVoxelPCD() const; diff --git a/include/multigrid_pclomp/multi_voxel_grid_covariance_omp_impl.hpp b/include/multigrid_pclomp/multi_voxel_grid_covariance_omp_impl.hpp index 8d495cb2..45997c39 100644 --- a/include/multigrid_pclomp/multi_voxel_grid_covariance_omp_impl.hpp +++ b/include/multigrid_pclomp/multi_voxel_grid_covariance_omp_impl.hpp @@ -154,10 +154,11 @@ void MultiVoxelGridCovariance::createKdtree() { } template -int MultiVoxelGridCovariance::radiusSearch(const PointT &point, double radius, std::vector &k_leaves, std::vector &k_sqr_distances, unsigned int max_nn) const { +int MultiVoxelGridCovariance::radiusSearch(const PointT &point, double radius, std::vector &k_leaves, unsigned int max_nn) const { k_leaves.clear(); // Find neighbors within radius in the occupied voxel centroid cloud + std::vector k_sqr_distances; std::vector k_indices; int k = kdtree_.radiusSearch(point, radius, k_indices, k_sqr_distances, max_nn); @@ -175,9 +176,9 @@ int MultiVoxelGridCovariance::radiusSearch(const PointT &point, double r } template -int MultiVoxelGridCovariance::radiusSearch(const PointCloud &cloud, int index, double radius, std::vector &k_leaves, std::vector &k_sqr_distances, unsigned int max_nn) const { +int MultiVoxelGridCovariance::radiusSearch(const PointCloud &cloud, int index, double radius, std::vector &k_leaves, unsigned int max_nn) const { if(index >= static_cast(cloud.points.size()) || index < 0) return (0); - return (radiusSearch(cloud.points[index], radius, k_leaves, k_sqr_distances, max_nn)); + return (radiusSearch(cloud.points[index], radius, k_leaves, max_nn)); } template diff --git a/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp b/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp index 54d56e88..578bdb22 100644 --- a/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp +++ b/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp @@ -322,10 +322,9 @@ double MultiGridNormalDistributionsTransform::computeD // Searching for neighbors of the current transformed point auto &x_trans_pt = trans_cloud[idx]; std::vector neighborhood; - std::vector nn_distances; // Neighborhood search method other than kdtree is disabled in multigrid_ndt_omp - target_cells_.radiusSearch(x_trans_pt, params_.resolution, neighborhood, nn_distances); + target_cells_.radiusSearch(x_trans_pt, params_.resolution, neighborhood); if(neighborhood.empty()) { continue; @@ -606,10 +605,9 @@ void MultiGridNormalDistributionsTransform::computeHes // Find neighbors (Radius search has been experimentally faster than direct neighbor checking. std::vector neighborhood; - std::vector distances; // Neighborhood search method other than kdtree is disabled in multigrid_ndt_omp - target_cells_.radiusSearch(x_trans_pt, params_.resolution, neighborhood, distances); + target_cells_.radiusSearch(x_trans_pt, params_.resolution, neighborhood); if(neighborhood.empty()) { continue; @@ -618,6 +616,7 @@ void MultiGridNormalDistributionsTransform::computeHes auto &x_pt = (*input_)[idx]; // For math Eigen::Vector3d x(x_pt.x, x_pt.y, x_pt.z); + const Eigen::Vector3d x_trans(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z); auto &point_gradient = t_point_gradients[tid]; auto &point_hessian = t_point_hessians[tid]; @@ -939,10 +938,9 @@ double MultiGridNormalDistributionsTransform::calculat // Find neighbors (Radius search has been experimentally faster than direct neighbor checking. std::vector neighborhood; - std::vector distances; // Neighborhood search method other than kdtree is disabled in multigrid_ndt_omp - target_cells_.radiusSearch(x_trans_pt, params_.resolution, neighborhood, distances); + target_cells_.radiusSearch(x_trans_pt, params_.resolution, neighborhood); if(neighborhood.empty()) { continue; @@ -999,10 +997,9 @@ double MultiGridNormalDistributionsTransform::calculat // Find neighbors (Radius search has been experimentally faster than direct neighbor checking. std::vector neighborhood; - std::vector distances; // Neighborhood search method other than kdtree is disabled in multigrid_ndt_omp - target_cells_.radiusSearch(x_trans_pt, params_.resolution, neighborhood, distances); + target_cells_.radiusSearch(x_trans_pt, params_.resolution, neighborhood); if(neighborhood.empty()) { continue;