Add benchmarking for knn and knn nanoflann (#6267)

* Add benchmarking for knn and knn nanoflann

* Knn benchmark fixes

* Remove nans from the radiusSearch benchmark

* Fix formatting. Fix cloud in organization. Fix to only use one argument since the other was ignored.

---------

Co-authored-by: jmackay2 <jmackay2>

Author: jmackay2

benchmarks/CMakeLists.txt

@@ -29,7 +29,6 @@ PCL_ADD_BENCHMARK(filters_radius_outlier_removal FILES filters/radius_outlier_re
                             "${PCL_SOURCE_DIR}/test/milk_cartoon_all_small_clorox.pcd")
 
 PCL_ADD_BENCHMARK(search_radius_search FILES search/radius_search.cpp
-                  LINK_WITH pcl_io pcl_search
-                  ARGUMENTS "${PCL_SOURCE_DIR}/test/table_scene_mug_stereo_textured.pcd"
-                            "${PCL_SOURCE_DIR}/test/milk_cartoon_all_small_clorox.pcd")
-
+                  LINK_WITH pcl_io pcl_search pcl_filters
+                  ARGUMENTS "${PCL_SOURCE_DIR}/test/table_scene_mug_stereo_textured.pcd")
+          

benchmarks/search/radius_search.cpp

@@ -1,4 +1,7 @@
+#include <pcl/filters/filter.h>
 #include <pcl/io/pcd_io.h>
+#include <pcl/search/kdtree.h>
+#include <pcl/search/kdtree_nanoflann.h>
 #include <pcl/search/organized.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -7,56 +10,175 @@
 
 #include <chrono>
 
-static void
-BM_OrganizedNeighborSearch(benchmark::State& state, const std::string& file)
+void
+print_help()
 {
-  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudIn(new pcl::PointCloud<pcl::PointXYZ>);
-  pcl::PCDReader reader;
-  reader.read(file, *cloudIn);
+  std::cout << "Usage: benchmark_radius_search <pcd filename>\n";
+  std::cout << "Additional argument options:\n";
+  std::cout << "Usage: benchmark_radius_search <pcd filename> <radius search> "
+               "<neighbor limit>\n";
+}
 
+static void
+BM_OrganizedNeighborSearch(benchmark::State& state,
+                           const pcl::PointCloud<pcl::PointXYZ>::Ptr cloudIn,
+                           const double searchRadius,
+                           const size_t neighborLimit)
+{
   pcl::search::OrganizedNeighbor<pcl::PointXYZ> organizedNeighborSearch;
   organizedNeighborSearch.setInputCloud(cloudIn);
 
-  double radiusSearchTime = 0;
-  std::vector<int> indices(cloudIn->size()); // Fixed indices from 0 to cloud size
-  std::iota(indices.begin(), indices.end(), 0);
   int radiusSearchIdx = 0;
 
+  pcl::Indices k_indices;
+  std::vector<float> k_sqr_distances;
   for (auto _ : state) {
-    int searchIdx = indices[radiusSearchIdx++ % indices.size()];
-    double searchRadius = 0.1; // or any fixed radius like 0.05
+    state.PauseTiming();
+    int searchIdx = radiusSearchIdx++ % cloudIn->size();
+    while (!pcl::isFinite((*cloudIn)[searchIdx])) {
+      searchIdx = radiusSearchIdx++ % cloudIn->size();
+    }
+    state.ResumeTiming();
+    organizedNeighborSearch.radiusSearch(
+        (*cloudIn)[searchIdx], searchRadius, k_indices, k_sqr_distances, neighborLimit);
+  }
+}
 
-    pcl::Indices k_indices;
-    std::vector<float> k_sqr_distances;
+static void
+BM_KdTree(benchmark::State& state,
+          const pcl::PointCloud<pcl::PointXYZ>::Ptr cloudIn,
+          const double searchRadius,
+          const size_t neighborLimit)
+{
+  pcl::search::KdTree<pcl::PointXYZ> kdtree(false);
+  kdtree.setInputCloud(cloudIn);
 
+  int radiusSearchIdx = 0;
+
+  pcl::Indices k_indices;
+  std::vector<float> k_sqr_distances;
+  for (auto _ : state) {
+    state.PauseTiming();
+    int searchIdx = radiusSearchIdx++ % cloudIn->size();
+    while (!pcl::isFinite((*cloudIn)[searchIdx])) {
+      searchIdx = radiusSearchIdx++ % cloudIn->size();
+    }
+    state.ResumeTiming();
+    kdtree.radiusSearch(
+        searchIdx, searchRadius, k_indices, k_sqr_distances, neighborLimit);
+  }
+}
+
+static void
+BM_KdTreeAll(benchmark::State& state,
+             const pcl::PointCloud<pcl::PointXYZ>::Ptr cloudIn,
+             const double searchRadius,
+             const size_t neighborLimit)
+{
+  pcl::search::KdTree<pcl::PointXYZ> kdtree(false);
+  kdtree.setInputCloud(cloudIn);
+
+  // Leaving indices empty to have it search through all points
+  pcl::Indices indices;
+  std::vector<pcl::Indices> k_indices;
+  std::vector<std::vector<float>> k_sqr_distances;
+  for (auto _ : state) {
     auto start_time = std::chrono::high_resolution_clock::now();
-    organizedNeighborSearch.radiusSearch(
-        (*cloudIn)[searchIdx], searchRadius, k_indices, k_sqr_distances);
+    kdtree.radiusSearch(
+        *cloudIn, indices, searchRadius, k_indices, k_sqr_distances, neighborLimit);
     auto end_time = std::chrono::high_resolution_clock::now();
-
-    radiusSearchTime +=
-        std::chrono::duration_cast<std::chrono::nanoseconds>(end_time - start_time)
-            .count();
+    std::chrono::duration<double> elapsed = end_time - start_time;
+    state.SetIterationTime(elapsed.count() / cloudIn->size());
   }
 
-  state.SetItemsProcessed(state.iterations());
-  state.SetIterationTime(
-      radiusSearchTime /
-      (state.iterations() * indices.size())); // Normalize by total points processed
+  state.SetItemsProcessed(cloudIn->size());
+}
+
+#if PCL_HAS_NANOFLANN
+static void
+BM_KdTreeNanoflann(benchmark::State& state,
+                   const pcl::PointCloud<pcl::PointXYZ>::Ptr cloudIn,
+                   const double searchRadius,
+                   const size_t neighborLimit)
+{
+  pcl::search::KdTreeNanoflann<pcl::PointXYZ> kdtree;
+  kdtree.setInputCloud(cloudIn);
+
+  int radiusSearchIdx = 0;
+
+  pcl::Indices k_indices;
+  std::vector<float> k_sqr_distances;
+  for (auto _ : state) {
+    state.PauseTiming();
+    int searchIdx = radiusSearchIdx++ % cloudIn->size();
+    while (!pcl::isFinite((*cloudIn)[searchIdx])) {
+      searchIdx = radiusSearchIdx++ % cloudIn->size();
+    }
+    state.ResumeTiming();
+    kdtree.radiusSearch(
+        searchIdx, searchRadius, k_indices, k_sqr_distances, neighborLimit);
+  }
 }
+#endif
 
 int
 main(int argc, char** argv)
 {
   if (argc < 2) {
     std::cerr << "No test file given. Please provide a PCD file for the benchmark."
               << std::endl;
+    print_help();
     return -1;
   }
 
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudIn(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PCDReader reader;
+  reader.read(argv[1], *cloudIn);
+
+  // Filter out nans
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFiltered(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::Indices indices;
+  pcl::removeNaNFromPointCloud(*cloudIn, *cloudFiltered, indices);
+
+  double searchRadius = 0.1;
+  if (argc > 2) {
+    try {
+      searchRadius = std::abs(std::stod(argv[2]));
+    } catch (const std::invalid_argument&) {
+      std::cerr << "Error: Invalid search radius. Setting it to a default of 0.1."
+                << std::endl;
+    }
+  }
+
+  size_t neighborLimit = 0u;
+  if (argc > 3) {
+    try {
+      neighborLimit = std::stoul(argv[3]);
+    } catch (const std::invalid_argument&) {
+      std::cerr << "Error: Invalid neighbor limit. Setting it to a default of int max."
+                << std::endl;
+    }
+  }
+
+  benchmark::RegisterBenchmark("OrganizedNeighborSearch",
+                               &BM_OrganizedNeighborSearch,
+                               cloudIn,
+                               searchRadius,
+                               neighborLimit)
+      ->Unit(benchmark::kMicrosecond);
+  benchmark::RegisterBenchmark(
+      "KdTree", &BM_KdTree, cloudIn, searchRadius, neighborLimit)
+      ->Unit(benchmark::kMicrosecond);
+  benchmark::RegisterBenchmark(
+      "KdTreeAll", &BM_KdTreeAll, cloudFiltered, searchRadius, neighborLimit)
+      ->Unit(benchmark::kMicrosecond)
+      ->UseManualTime()
+      ->Iterations(1);
+#if PCL_HAS_NANOFLANN
   benchmark::RegisterBenchmark(
-      "BM_OrganizedNeighborSearch", &BM_OrganizedNeighborSearch, argv[1])
-      ->Unit(benchmark::kMillisecond);
+      "KdTreeNanoflann", &BM_KdTreeNanoflann, cloudIn, searchRadius, neighborLimit)
+      ->Unit(benchmark::kMicrosecond);
+#endif
   benchmark::Initialize(&argc, argv);
   benchmark::RunSpecifiedBenchmarks();
 }