From 50c406455a72492dc6e53a8d488cdc12a8e7a602 Mon Sep 17 00:00:00 2001 From: Bhavesh Misra Date: Sat, 25 Nov 2023 17:30:25 +0530 Subject: [PATCH] Issue 4718 (#5851) * Initial_Commit_Creating_Benchmark_from_test * Changes * Changes_2 * Deleted_vscode * Ran_make_format * Ran_make_format * Pushing_changed_file_name * Deleted_format * Ran_make_format * Updated clang_format_14, and unmade_the_changes * Using_the_chrono_now_way * Ran_format.sh --------- Co-authored-by: Bhavesh Misra --- benchmarks/CMakeLists.txt | 5 ++ benchmarks/search/radius_search.cpp | 62 +++++++++++++++++++++++ test/search/test_organized_index.cpp | 75 ---------------------------- 3 files changed, 67 insertions(+), 75 deletions(-) create mode 100644 benchmarks/search/radius_search.cpp diff --git a/benchmarks/CMakeLists.txt b/benchmarks/CMakeLists.txt index b85bc0354ca..95f421db5f6 100644 --- a/benchmarks/CMakeLists.txt +++ b/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") + diff --git a/benchmarks/search/radius_search.cpp b/benchmarks/search/radius_search.cpp new file mode 100644 index 00000000000..9641375bed3 --- /dev/null +++ b/benchmarks/search/radius_search.cpp @@ -0,0 +1,62 @@ +#include +#include +#include +#include + +#include + +#include + +static void +BM_OrganizedNeighborSearch(benchmark::State& state, const std::string& file) +{ + pcl::PointCloud::Ptr cloudIn(new pcl::PointCloud); + pcl::PCDReader reader; + reader.read(file, *cloudIn); + + pcl::search::OrganizedNeighbor organizedNeighborSearch; + organizedNeighborSearch.setInputCloud(cloudIn); + + double radiusSearchTime = 0; + std::vector 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 k_indices; + std::vector 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(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(); +} diff --git a/test/search/test_organized_index.cpp b/test/search/test_organized_index.cpp index 582099e7bc5..6b63d00d204 100644 --- a/test/search/test_organized_index.cpp +++ b/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 organizedNeighborSearch; - - std::vector k_indices; - std::vector k_sqr_distances; - - std::vector k_indices_bruteforce; - std::vector 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::Ptr cloudIn (new PointCloud ()); - - 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(rand ()) / static_cast(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(rand ()) / static_cast(RAND_MAX)); - - // bruteforce radius search - std::vector 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 cloudNWRRadius; - - organizedNeighborSearch.setInputCloud (cloudIn); - organizedNeighborSearch.radiusSearch ((*cloudIn)[randomIdx], searchRadius, cloudNWRSearch, cloudNWRRadius, std::numeric_limits::max()); - - organizedNeighborSearch.setInputCloud (cloudIn); - organizedNeighborSearch.radiusSearch ((*cloudIn)[randomIdx], searchRadius, cloudNWRSearch, cloudNWRRadius, std::numeric_limits::max()); - } -} - TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search_Kinect_Data) {