Add PCD nearest neighbor search demo

.gitignore

@@ -1,2 +1,4 @@
 build/*
 python/*.so
+/.cache/
+test/pcd/*.pcd

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()
-

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 <map.pcd> <qx> <qy> <qz>
+```
+
+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 <map.pcd> <qx> <qy> <qz> --no-vis
+```
+
 ### Run Benchmark (Compare with ikd-tree)
 
 ```bash

test/nearest_search_pcd_demo.cpp

@@ -0,0 +1,214 @@
+// nearest_search_pcd_demo.cpp - PCD nearest-neighbor demo for likd-tree
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/visualization/pcl_visualizer.h>
+
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <exception>
+#include <iomanip>
+#include <iostream>
+#include <limits>
+#include <string>
+#include <thread>
+
+#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 << " <map.pcd> <qx> <qy> <qz> [--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<PointType>& points,
+                                   const PointType& query,
+                                   float& best_dist2) {
+  const PointType* best_point = nullptr;
+  best_dist2 = std::numeric_limits<float>::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<double, std::milli>(end - start).count();
+}
+
+void visualizeNearest(const pcl::PointCloud<PointType>::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<PointType> cloud_color(
+      cloud, 170, 170, 170);
+  viewer.addPointCloud<PointType>(cloud, cloud_color, "cloud");
+  viewer.setPointCloudRenderingProperties(
+      pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud");
+
+  pcl::PointCloud<PointType>::Ptr query_cloud(new pcl::PointCloud<PointType>);
+  query_cloud->push_back(query);
+  pcl::visualization::PointCloudColorHandlerCustom<PointType> query_color(
+      query_cloud, 0, 255, 0);
+  viewer.addPointCloud<PointType>(query_cloud, query_color, "query");
+  viewer.setPointCloudRenderingProperties(
+      pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 12, "query");
+
+  pcl::PointCloud<PointType>::Ptr nearest_cloud(new pcl::PointCloud<PointType>);
+  nearest_cloud->push_back(nearest);
+  pcl::visualization::PointCloudColorHandlerCustom<PointType> nearest_color(
+      nearest_cloud, 255, 0, 0);
+  viewer.addPointCloud<PointType>(nearest_cloud, nearest_color, "nearest");
+  viewer.setPointCloudRenderingProperties(
+      pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 12, "nearest");
+
+  viewer.addLine<PointType>(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<PointType>::Ptr input_cloud(new pcl::PointCloud<PointType>);
+  if (pcl::io::loadPCDFile<PointType>(argv[1], *input_cloud) < 0) {
+    std::cerr << "Error: failed to read PCD file: " << argv[1] << '\n';
+    return 1;
+  }
+
+  PointVector<PointType> points;
+  points.reserve(input_cloud->size());
+
+  pcl::PointCloud<PointType>::Ptr valid_cloud(new pcl::PointCloud<PointType>);
+  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<PointType> 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;
+}