From f371b1df2e8dbb307b6dd3548cbd7cbbae9b1708 Mon Sep 17 00:00:00 2001
From: "julia.sanchez" <julia.sanchez@kitware.com>
Date: Mon, 4 Jan 2021 17:59:54 +0100
Subject: [PATCH] [feat] Add radius search to KDTreePCLAdaptor object

---
 slam_lib/include/LidarSlam/KDTreePCLAdaptor.h | 36 +++++++++++++++++++
 1 file changed, 36 insertions(+)

diff --git a/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h b/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h
index 03f08be6..01a702d9 100644
--- a/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h
+++ b/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h
@@ -105,6 +105,42 @@ public:
   {
     return this->KnnSearch(queryPoint.data, knearest, knnIndices, knnSqDistances);
   }
+  
+  /**
+    * \brief Finds the neighbor points in a specified radius from a given query point in the KD-tree.
+    * \param[in] queryPoint Input point from which the neighborhood is extracted.
+    * \param[in] radius distance in meters for neighborhood definition.
+    * \param[out] IndicesDistances neighbor indices and relative squared distances from the query point.
+    * \return Number `N` of neighbors found.
+    */
+  inline size_t RadiusSearch(const float queryPoint[3], float radius, std::vector<std::pair<int, float>>& IndicesDistances) const
+  {
+    // Find nearest neighbors
+    nanoflann::SearchParams params;
+    return this->Index->radiusSearch(queryPoint, radius*radius, IndicesDistances, params);
+  }
+  
+  /**
+    * \brief Finds the neighbor points in a specified radius from a given query point in the KD-tree.
+    * \param[in] queryPoint Input point from which the neighborhood is extracted.
+    * \param[in] radius distance in meters for neighborhood definition.
+    * \param[out] radiusIndices neighbor indices.
+    * \param[out] radiusSqDistances neighbors squared distances from query point.
+    * \return Number `N` of neighbors found.
+    */
+  inline size_t RadiusSearch(const float queryPoint[3], float radius, std::vector<int>& radiusIndices, std::vector<float>& radiusSqDistances) const
+  {
+    std::vector<std::pair<int, float>> indicesSqDistances;
+    size_t kneighbors = this->RadiusSearch(queryPoint[3], radius, indicesSqDistances);
+    radiusIndices.reserve(kneighbors);
+    radiusSqDistances.reserve(kneighbors);
+    for(const std::pair<int,float>& idxDist : indicesSqDistances)
+    {
+      radiusIndices.push_back(idxDist.first);
+      radiusSqDistances.push_back(idxDist.second);
+    }
+    return kneighbors;
+  }
 
   /**
     * \brief Get the input pointcloud.
-- 
GitLab