Convert test to benchmark (radius search)

benchmarks/CMakeLists.txt

@@ -25,3 +25,8 @@ PCL_ADD_BENCHMARK(filters_voxel_grid FILES filters/voxel_grid.cpp
                   ARGUMENTS "${PCL_SOURCE_DIR}/test/table_scene_mug_stereo_textured.pcd"
                             "${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")
+

benchmarks/search/radius_search.cpp

@@ -0,0 +1,62 @@
+#include <pcl/io/pcd_io.h>
+#include <pcl/search/organized.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <benchmark/benchmark.h>
+
+#include <chrono>
+
+static void
+BM_OrganizedNeighborSearch(benchmark::State& state, const std::string& file)
+{
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudIn(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PCDReader reader;
+  reader.read(file, *cloudIn);
+
+  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;
+
+  for (auto _ : state) {
+    int searchIdx = indices[radiusSearchIdx++ % indices.size()];
+    double searchRadius = 0.1; // or any fixed radius like 0.05
+
+    std::vector<int> k_indices;
+    std::vector<float> k_sqr_distances;
+
+    auto start_time = std::chrono::high_resolution_clock::now();
+    organizedNeighborSearch.radiusSearch(
+        (*cloudIn)[searchIdx], searchRadius, k_indices, k_sqr_distances);
+    auto end_time = std::chrono::high_resolution_clock::now();
+
+    radiusSearchTime +=
+        std::chrono::duration_cast<std::chrono::nanoseconds>(end_time - start_time)
+            .count();
+  }
+
+  state.SetItemsProcessed(state.iterations());
+  state.SetIterationTime(
+      radiusSearchTime /
+      (state.iterations() * indices.size())); // Normalize by total points processed
+}
+
+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;
+    return -1;
+  }
+
+  benchmark::RegisterBenchmark(
+      "BM_OrganizedNeighborSearch", &BM_OrganizedNeighborSearch, argv[1])
+      ->Unit(benchmark::kMillisecond);
+  benchmark::Initialize(&argc, argv);
+  benchmark::RunSpecifiedBenchmarks();
+}

test/search/test_organized_index.cpp

@@ -340,81 +340,6 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search)
   }
 }
 
-
-TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search_Benchmark_Test)
-{
-  constexpr unsigned int test_runs = 10;
-  const unsigned int seed = time (nullptr);
-  srand (seed);
-
-  search::OrganizedNeighbor<PointXYZ> organizedNeighborSearch;
-
-  std::vector<int> k_indices;
-  std::vector<float> k_sqr_distances;
-
-  std::vector<int> k_indices_bruteforce;
-  std::vector<float> k_sqr_distances_bruteforce;
-
-  // typical focal length from kinect
-  constexpr double oneOverFocalLength = 0.0018;
-
-  for (unsigned int test_id = 0; test_id < test_runs; test_id++)
-  {
-    // generate point cloud
-
-    PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
-
-    cloudIn->width = 1024;
-    cloudIn->height = 768;
-    cloudIn->points.clear();
-    cloudIn->points.resize (cloudIn->width * cloudIn->height);
-
-    int centerX = cloudIn->width >> 1;
-    int centerY = cloudIn->height >> 1;
-
-    int idx = 0;
-    for (int ypos = -centerY; ypos < centerY; ypos++)
-      for (int xpos = -centerX; xpos < centerX; xpos++)
-      {
-        double z = 5.0 * ( (static_cast<double>(rand ()) / static_cast<double>(RAND_MAX)))+5;
-        double y = ypos*oneOverFocalLength*z;
-        double x = xpos*oneOverFocalLength*z;
-
-        (*cloudIn)[idx++]= PointXYZ (x, y, z);
-      }
-
-    const unsigned int randomIdx = rand() % (cloudIn->width * cloudIn->height);
-
-    const PointXYZ& searchPoint = (*cloudIn)[randomIdx];
-
-    const double searchRadius = 1.0 * (static_cast<double>(rand ()) / static_cast<double>(RAND_MAX));
-
-    // bruteforce radius search
-    std::vector<int> cloudSearchBruteforce;
-    cloudSearchBruteforce.clear();
-
-    for (auto it = cloudIn->points.cbegin(); it != cloudIn->points.cend(); ++it)
-    {
-      const auto pointDist = pcl_tests::point_distance(*it, searchPoint);
-
-      if (pointDist <= searchRadius)
-      {
-        // add point candidates to vector list
-        cloudSearchBruteforce.push_back (std::distance(cloudIn->points.cbegin(), it));
-      }
-    }
-
-    pcl::Indices cloudNWRSearch;
-    std::vector<float> cloudNWRRadius;
-
-    organizedNeighborSearch.setInputCloud (cloudIn);
-    organizedNeighborSearch.radiusSearch ((*cloudIn)[randomIdx], searchRadius, cloudNWRSearch, cloudNWRRadius, std::numeric_limits<unsigned int>::max());
-
-    organizedNeighborSearch.setInputCloud (cloudIn);
-    organizedNeighborSearch.radiusSearch ((*cloudIn)[randomIdx], searchRadius, cloudNWRSearch, cloudNWRRadius, std::numeric_limits<unsigned int>::max());
-  }
-}
-
 TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search_Kinect_Data)
 {