Removed distances argument from radiusSearch

Signed-off-by: Shintaro Sakoda <[email protected]>

include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h

@@ -278,23 +278,21 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid<PointT> {
    * \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<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const;
+  int radiusSearch(const PointT &point, double radius, std::vector<LeafConstPtr> &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.
    * \param[in] cloud the given query point
    * \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<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const;
+  int radiusSearch(const PointCloud &cloud, int index, double radius, std::vector<LeafConstPtr> &k_leaves, unsigned int max_nn = 0) const;
 
   PointCloud getVoxelPCD() const;
 

include/multigrid_pclomp/multi_voxel_grid_covariance_omp_impl.hpp

@@ -154,10 +154,11 @@ void MultiVoxelGridCovariance<PointT>::createKdtree() {
 }
 
 template<typename PointT>
-int MultiVoxelGridCovariance<PointT>::radiusSearch(const PointT &point, double radius, std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances, unsigned int max_nn) const {
+int MultiVoxelGridCovariance<PointT>::radiusSearch(const PointT &point, double radius, std::vector<LeafConstPtr> &k_leaves, unsigned int max_nn) const {
   k_leaves.clear();
 
   // Find neighbors within radius in the occupied voxel centroid cloud
+  std::vector<float> k_sqr_distances;
   std::vector<int> k_indices;
   int k = kdtree_.radiusSearch(point, radius, k_indices, k_sqr_distances, max_nn);
 
@@ -175,9 +176,9 @@ int MultiVoxelGridCovariance<PointT>::radiusSearch(const PointT &point, double r
 }
 
 template<typename PointT>
-int MultiVoxelGridCovariance<PointT>::radiusSearch(const PointCloud &cloud, int index, double radius, std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances, unsigned int max_nn) const {
+int MultiVoxelGridCovariance<PointT>::radiusSearch(const PointCloud &cloud, int index, double radius, std::vector<LeafConstPtr> &k_leaves, unsigned int max_nn) const {
   if(index >= static_cast<int>(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<typename PointT>

include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp

@@ -322,10 +322,9 @@ double MultiGridNormalDistributionsTransform<PointSource, PointTarget>::computeD
     // Searching for neighbors of the current transformed point
     auto &x_trans_pt = trans_cloud[idx];
     std::vector<TargetGridLeafConstPtr> neighborhood;
-    std::vector<float> 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<PointSource, PointTarget>::computeHes
 
     // Find neighbors (Radius search has been experimentally faster than direct neighbor checking.
     std::vector<TargetGridLeafConstPtr> neighborhood;
-    std::vector<float> 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<PointSource, PointTarget>::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<PointSource, PointTarget>::calculat
 
     // Find neighbors (Radius search has been experimentally faster than direct neighbor checking.
     std::vector<TargetGridLeafConstPtr> neighborhood;
-    std::vector<float> 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<PointSource, PointTarget>::calculat
 
     // Find neighbors (Radius search has been experimentally faster than direct neighbor checking.
     std::vector<TargetGridLeafConstPtr> neighborhood;
-    std::vector<float> 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;