diff --git a/.gitignore b/.gitignore index 104b8c4..4654392 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,4 @@ build/* python/*.so +/.cache/ +test/pcd/*.pcd diff --git a/CMakeLists.txt b/CMakeLists.txt index d0ca908..b694b6e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,14 +16,37 @@ else() message(STATUS "TBB not found. If you want to use #define LIKD_TREE_USE_TBB, please install TBB.") endif() -find_package(PCL QUIET COMPONENTS common io) +set(LIKD_TREE_PCL_FOUND OFF) +set(LIKD_TREE_PCL_VISUALIZATION_FOUND OFF) + +find_package(PCL QUIET COMPONENTS common io visualization) if(PCL_FOUND) - message(STATUS "Found PCL: ${PCL_VERSION}") - include_directories(${PCL_INCLUDE_DIRS}) - link_directories(${PCL_LIBRARY_DIRS}) - add_definitions(${PCL_DEFINITIONS}) + message(STATUS "Found PCL with visualization: ${PCL_VERSION}") + set(LIKD_TREE_PCL_FOUND ON) + set(LIKD_TREE_PCL_VISUALIZATION_FOUND ON) + set(LIKD_TREE_PCL_INCLUDE_DIRS ${PCL_INCLUDE_DIRS}) + set(LIKD_TREE_PCL_LIBRARY_DIRS ${PCL_LIBRARY_DIRS}) + set(LIKD_TREE_PCL_DEFINITIONS ${PCL_DEFINITIONS}) + set(LIKD_TREE_PCL_LIBRARIES ${PCL_LIBRARIES}) else() - message(STATUS "PCL not found; test will build with a lightweight Point struct unless PCL is available.") + find_package(PCL QUIET COMPONENTS common io) + if(PCL_FOUND) + message(STATUS "Found PCL: ${PCL_VERSION}") + message(STATUS "PCL visualization not found; nearest_search_pcd_demo will not be built.") + set(LIKD_TREE_PCL_FOUND ON) + set(LIKD_TREE_PCL_INCLUDE_DIRS ${PCL_INCLUDE_DIRS}) + set(LIKD_TREE_PCL_LIBRARY_DIRS ${PCL_LIBRARY_DIRS}) + set(LIKD_TREE_PCL_DEFINITIONS ${PCL_DEFINITIONS}) + set(LIKD_TREE_PCL_LIBRARIES ${PCL_LIBRARIES}) + else() + message(STATUS "PCL not found; test will build with a lightweight Point struct unless PCL is available.") + endif() +endif() + +if(LIKD_TREE_PCL_FOUND) + include_directories(${LIKD_TREE_PCL_INCLUDE_DIRS}) + link_directories(${LIKD_TREE_PCL_LIBRARY_DIRS}) + add_definitions(${LIKD_TREE_PCL_DEFINITIONS}) endif() add_library(kdtree INTERFACE) @@ -32,14 +55,28 @@ if(TBB_FOUND) target_link_libraries(kdtree INTERFACE TBB::tbb) endif() +find_package(Threads REQUIRED) + # Demo executable (always built) add_executable(demo test/demo.cpp ) -target_link_libraries(demo kdtree ${PCL_LIBRARIES}) -find_package(Threads REQUIRED) +target_link_libraries(demo kdtree ${LIKD_TREE_PCL_LIBRARIES}) target_link_libraries(demo Threads::Threads) +if(LIKD_TREE_PCL_VISUALIZATION_FOUND) + add_executable(nearest_search_pcd_demo + test/nearest_search_pcd_demo.cpp + ) + target_link_libraries(nearest_search_pcd_demo + kdtree + ${LIKD_TREE_PCL_LIBRARIES} + Threads::Threads + ) +else() + message(STATUS "nearest_search_pcd_demo disabled. Install PCL visualization to enable it.") +endif() + # Benchmark executable (only built with -DBUILD_BENCHMARK=ON) option(BUILD_BENCHMARK "Build benchmark comparing likd-tree vs ikd-tree" OFF) if(BUILD_BENCHMARK) @@ -80,8 +117,7 @@ if(BUILD_BENCHMARK) thirdparty/ikd-Tree/ikd-Tree/ikd_Tree.cpp ) target_include_directories(benchmark PRIVATE thirdparty/ikd-Tree/ikd-Tree) - target_link_libraries(benchmark kdtree ${PCL_LIBRARIES} Threads::Threads) + target_link_libraries(benchmark kdtree ${LIKD_TREE_PCL_LIBRARIES} Threads::Threads) else() message(STATUS "Benchmark disabled. Use -DBUILD_BENCHMARK=ON to enable.") endif() - diff --git a/README.md b/README.md index c7361eb..8c08e5c 100644 --- a/README.md +++ b/README.md @@ -168,6 +168,51 @@ cmake --build build ./build/demo ``` +### Nearest Neighbor Search on PCD + +If PCL visualization is available, CMake also builds a PCD-based nearest +neighbor demo: + +```bash +cmake -B build +cmake --build build --target nearest_search_pcd_demo +./build/nearest_search_pcd_demo +``` + +Example: + +```bash +./build/nearest_search_pcd_demo ./test/pcd/globalMap.pcd 0.9 0.1 0 +``` + +The demo loads a PCD map, builds a `likd-tree`, queries the nearest point to +the input coordinate, and verifies the result with brute-force search. + +Example output: + +```text +Loaded points: 1742788 +Finite points used: 1742788 +Build time: 318.466372 ms +Query time: 0.010660 ms +Query: (0.900000, 0.100000, 0.000000) +Nearest: (0.950123, -0.076546, -1.369322) +Distance: 1.381566 +Brute-force distance: 1.381566 +Brute-force check: MATCH +``` + +In the visualization, the original cloud is shown in gray, the query point in +green, the nearest neighbor in red, and the line between them in yellow. + +![nearest-search-pcd](imgs/nearest_search_pcd_result.png) + +For terminal-only validation without opening a viewer, pass `--no-vis`: + +```bash +./build/nearest_search_pcd_demo --no-vis +``` + ### Run Benchmark (Compare with ikd-tree) ```bash diff --git a/imgs/nearest_search_pcd_result.png b/imgs/nearest_search_pcd_result.png new file mode 100644 index 0000000..91bd6e7 Binary files /dev/null and b/imgs/nearest_search_pcd_result.png differ diff --git a/test/nearest_search_pcd_demo.cpp b/test/nearest_search_pcd_demo.cpp new file mode 100644 index 0000000..843ffc3 --- /dev/null +++ b/test/nearest_search_pcd_demo.cpp @@ -0,0 +1,214 @@ +// nearest_search_pcd_demo.cpp - PCD nearest-neighbor demo for likd-tree + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "likd_tree.hpp" + +using PointType = pcl::PointXYZ; +using Clock = std::chrono::high_resolution_clock; + +namespace { + +void printUsage(const char* program) { + std::cerr << "Usage:\n" + << " " << program << " [--no-vis]\n\n" + << "Example:\n" + << " " << program << " map.pcd 10.0 -5.0 2.0\n"; +} + +bool parseFloat(const char* text, float& value) { + try { + size_t parsed = 0; + value = std::stof(text, &parsed); + return parsed == std::string(text).size(); + } catch (const std::exception&) { + return false; + } +} + +bool isFinite(const PointType& point) { + return std::isfinite(point.x) && std::isfinite(point.y) && + std::isfinite(point.z); +} + +float sqrDist(const PointType& a, const PointType& b) { + const float dx = a.x - b.x; + const float dy = a.y - b.y; + const float dz = a.z - b.z; + return dx * dx + dy * dy + dz * dz; +} + +const PointType* bruteForceNearest(const PointVector& points, + const PointType& query, + float& best_dist2) { + const PointType* best_point = nullptr; + best_dist2 = std::numeric_limits::infinity(); + + for (const auto& point : points) { + const float dist2 = sqrDist(point, query); + if (dist2 < best_dist2) { + best_dist2 = dist2; + best_point = &point; + } + } + + return best_point; +} + +double elapsedMs(Clock::time_point start, Clock::time_point end) { + return std::chrono::duration(end - start).count(); +} + +void visualizeNearest(const pcl::PointCloud::ConstPtr& cloud, + const PointType& query, + const PointType& nearest) { + pcl::visualization::PCLVisualizer viewer( + "likd-tree Nearest Neighbor Search"); + viewer.setBackgroundColor(0.03, 0.03, 0.03); + viewer.addCoordinateSystem(1.0); + + pcl::visualization::PointCloudColorHandlerCustom cloud_color( + cloud, 170, 170, 170); + viewer.addPointCloud(cloud, cloud_color, "cloud"); + viewer.setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud"); + + pcl::PointCloud::Ptr query_cloud(new pcl::PointCloud); + query_cloud->push_back(query); + pcl::visualization::PointCloudColorHandlerCustom query_color( + query_cloud, 0, 255, 0); + viewer.addPointCloud(query_cloud, query_color, "query"); + viewer.setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 12, "query"); + + pcl::PointCloud::Ptr nearest_cloud(new pcl::PointCloud); + nearest_cloud->push_back(nearest); + pcl::visualization::PointCloudColorHandlerCustom nearest_color( + nearest_cloud, 255, 0, 0); + viewer.addPointCloud(nearest_cloud, nearest_color, "nearest"); + viewer.setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 12, "nearest"); + + viewer.addLine(query, nearest, 255.0, 255.0, 0.0, + "query_to_nearest"); + viewer.setShapeRenderingProperties( + pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, "query_to_nearest"); + viewer.resetCamera(); + + while (!viewer.wasStopped()) { + viewer.spinOnce(16); + std::this_thread::sleep_for(std::chrono::milliseconds(16)); + } +} + +} // namespace + +int main(int argc, char** argv) { + if (argc != 5 && argc != 6) { + printUsage(argv[0]); + return 1; + } + + bool visualize = true; + if (argc == 6) { + if (std::string(argv[5]) != "--no-vis") { + std::cerr << "Error: unknown option: " << argv[5] << '\n'; + printUsage(argv[0]); + return 1; + } + visualize = false; + } + + PointType query; + if (!parseFloat(argv[2], query.x) || !parseFloat(argv[3], query.y) || + !parseFloat(argv[4], query.z)) { + std::cerr << "Error: qx, qy and qz must be valid floating-point values.\n"; + printUsage(argv[0]); + return 1; + } + if (!isFinite(query)) { + std::cerr << "Error: query point coordinates must be finite values.\n"; + return 1; + } + + pcl::PointCloud::Ptr input_cloud(new pcl::PointCloud); + if (pcl::io::loadPCDFile(argv[1], *input_cloud) < 0) { + std::cerr << "Error: failed to read PCD file: " << argv[1] << '\n'; + return 1; + } + + PointVector points; + points.reserve(input_cloud->size()); + + pcl::PointCloud::Ptr valid_cloud(new pcl::PointCloud); + valid_cloud->reserve(input_cloud->size()); + + for (const auto& point : input_cloud->points) { + if (!isFinite(point)) { + continue; + } + points.push_back(point); + valid_cloud->push_back(point); + } + + if (points.empty()) { + std::cerr << "Error: PCD file does not contain any finite XYZ points.\n"; + return 1; + } + + KDTree tree; + + const auto build_start = Clock::now(); + tree.build(points); + const auto build_end = Clock::now(); + + const auto query_start = Clock::now(); + const auto [nearest_ptr, distance] = tree.nearestNeighbors(query); + const auto query_end = Clock::now(); + + if (!nearest_ptr) { + std::cerr << "Error: nearest-neighbor search returned no result.\n"; + return 1; + } + + float brute_dist2 = 0.0f; + const PointType* brute_nearest = + bruteForceNearest(points, query, brute_dist2); + const float brute_distance = std::sqrt(brute_dist2); + const float tolerance = + std::max(1e-5f, 1e-5f * std::max(distance, brute_distance)); + const bool match = + brute_nearest && std::fabs(distance - brute_distance) <= tolerance; + + std::cout << std::fixed << std::setprecision(6); + std::cout << "Loaded points: " << input_cloud->size() << '\n'; + std::cout << "Finite points used: " << points.size() << '\n'; + std::cout << "Build time: " << elapsedMs(build_start, build_end) << " ms\n"; + std::cout << "Query time: " << elapsedMs(query_start, query_end) << " ms\n"; + std::cout << "Query: (" << query.x << ", " << query.y << ", " << query.z + << ")\n"; + std::cout << "Nearest: (" << nearest_ptr->x << ", " << nearest_ptr->y + << ", " << nearest_ptr->z << ")\n"; + std::cout << "Distance: " << distance << '\n'; + std::cout << "Brute-force distance: " << brute_distance << '\n'; + std::cout << "Brute-force check: " << (match ? "MATCH" : "MISMATCH") + << '\n'; + + if (visualize) { + visualizeNearest(valid_cloud, query, *nearest_ptr); + } + return match ? 0 : 2; +}