Replace nanoflann (#42)

* replace nanoflann with original kdtree

* replace KdTreeOMP in python binding

* include array to fix build error on windows

* add kdtree test with synthetic data

* add nowait to see if it fixes error on win

* update README

* use openmp atomic

* revert. MSVC does not support openmp very well...

* disable parallel kdtree construction on windows

* update README

* rephrase

* avoid knn result copy

* refactoring

CMakeLists.txt

@@ -267,13 +267,13 @@ if(BUILD_TESTS)
       TBB::tbb
       TBB::tbbmalloc
       PCL::PCL
+      OpenMP::OpenMP_CXX
     )
 
     gtest_discover_tests(${TEST_NAME} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR})
   endforeach()
 endif()
 
-
 #############
 ## Install ##
 #############

README.md

@@ -21,7 +21,7 @@ This library uses some C++17 features. The PCL interface is not compatible with
 
 ## Dependencies
 
-- [Mandatory] [Eigen](https://eigen.tuxfamily.org/), [nanoflann](https://github.com/jlblancoc/nanoflann) ([bundled1](include/small_gicp/ann/nanoflann.hpp), [bundled2](include/small_gicp/ann/nanoflann_omp.hpp), [bundled3](include/small_gicp/ann/nanoflann_tbb.hpp)), [Sophus](https://github.com/strasdat/Sophus) ([bundled](include/small_gicp/util/lie.hpp))
+- [Mandatory] [Eigen](https://eigen.tuxfamily.org/), [nanoflann](https://github.com/jlblancoc/nanoflann) ([bundled](include/small_gicp/ann/kdtree.hpp)), [Sophus](https://github.com/strasdat/Sophus) ([bundled](include/small_gicp/util/lie.hpp))
 - [Optional] [OpenMP](https://www.openmp.org/), [Intel TBB](https://www.intel.com/content/www/us/en/developer/tools/oneapi/onetbb.html), [PCL](https://pointclouds.org/), [Iridescence](https://github.com/koide3/iridescence)
 
 ## Installation
@@ -171,8 +171,8 @@ estimate_covariances_omp(*target, num_neighbors, num_threads);
 estimate_covariances_omp(*source, num_neighbors, num_threads);
 
 // Create KdTree for target and source.
-auto target_tree = std::make_shared<KdTreeOMP<pcl::PointCloud<pcl::PointCovariance>>>(target, num_threads);
-auto source_tree = std::make_shared<KdTreeOMP<pcl::PointCloud<pcl::PointCovariance>>>(source, num_threads);
+auto target_tree = std::make_shared<KdTree<pcl::PointCloud<pcl::PointCovariance>>>(target, KdTreeBuilderOMP(num_threads));
+auto source_tree = std::make_shared<KdTree<pcl::PointCloud<pcl::PointCovariance>>>(source, KdTreeBuilderOMP(num_threads));
 
 Registration<GICPFactor, ParallelReductionOMP> registration;
 registration.reduction.num_threads = num_threads;
@@ -214,8 +214,8 @@ target = voxelgrid_sampling_omp(*target, downsampling_resolution, num_threads);
 source = voxelgrid_sampling_omp(*source, downsampling_resolution, num_threads);
 
 // Create KdTree
-auto target_tree = std::make_shared<KdTreeOMP<PointCloud>>(target, num_threads);
-auto source_tree = std::make_shared<KdTreeOMP<PointCloud>>(source, num_threads);
+auto target_tree = std::make_shared<KdTree<PointCloud>>(target, KdTreeBuilderOMP(num_threads));
+auto source_tree = std::make_shared<KdTree<PointCloud>>(source, KdTreeBuilderOMP(num_threads));
 
 // Estimate point covariances
 estimate_covariances_omp(*target, *target_tree, num_neighbors, num_threads);
@@ -334,8 +334,8 @@ Example D: Example with Open3D
 target_o3d = open3d.io.read_point_cloud('small_gicp/data/target.ply').paint_uniform_color([0, 1, 0])
 source_o3d = open3d.io.read_point_cloud('small_gicp/data/source.ply').paint_uniform_color([0, 0, 1])
 
-target, target_tree = small_gicp.preprocess_points(points_numpy=numpy.asarray(target_o3d.points), downsampling_resolution=0.25)
-source, source_tree = small_gicp.preprocess_points(points_numpy=numpy.asarray(source_o3d.points), downsampling_resolution=0.25)
+target, target_tree = small_gicp.preprocess_points(numpy.asarray(target_o3d.points), downsampling_resolution=0.25)
+source, source_tree = small_gicp.preprocess_points(numpy.asarray(source_o3d.points), downsampling_resolution=0.25)
 result = small_gicp.align(target, source, target_tree)
 
 source_o3d.transform(result.T_target_source)

include/small_gicp/ann/deprecated/kdtree.hpp

@@ -0,0 +1,105 @@
+// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide
+// SPDX-License-Identifier: MIT
+#pragma once
+
+#include <memory>
+#include <Eigen/Core>
+#include <small_gicp/points/traits.hpp>
+#include <small_gicp/ann/traits.hpp>
+#include <small_gicp/ann/nanoflann.hpp>
+
+namespace small_gicp {
+
+/// @brief Unsafe KdTree with arbitrary nanoflann Adaptor.
+/// @note  This class does not hold the ownership of the input points.
+///        You must keep the input points along with this class.
+template <class PointCloud, template <typename, typename, int, typename> class Adaptor>
+class UnsafeKdTreeGeneric {
+public:
+  using Ptr = std::shared_ptr<UnsafeKdTreeGeneric>;
+  using ConstPtr = std::shared_ptr<const UnsafeKdTreeGeneric>;
+  using ThisClass = UnsafeKdTreeGeneric<PointCloud, Adaptor>;
+  using Index = Adaptor<nanoflann::L2_Simple_Adaptor<double, ThisClass, double>, ThisClass, 3, size_t>;
+
+  /// @brief Constructor
+  /// @param points  Input points
+  explicit UnsafeKdTreeGeneric(const PointCloud& points) : points(points), index(3, *this, nanoflann::KDTreeSingleIndexAdaptorParams(10)) { index.buildIndex(); }
+
+  /// @brief Constructor
+  /// @param points  Input points
+  /// @params num_threads  Number of threads used for building the index (This argument is only valid for OMP implementation)
+  explicit UnsafeKdTreeGeneric(const PointCloud& points, int num_threads) : points(points), index(3, *this, nanoflann::KDTreeSingleIndexAdaptorParams(10)) {
+    index.buildIndex(num_threads);
+  }
+
+  ~UnsafeKdTreeGeneric() {}
+
+  // Interfaces for nanoflann
+  size_t kdtree_get_point_count() const { return traits::size(points); }
+  double kdtree_get_pt(const size_t idx, const size_t dim) const { return traits::point(points, idx)[dim]; }
+
+  template <class BBox>
+  bool kdtree_get_bbox(BBox&) const {
+    return false;
+  }
+
+  /// @brief Find k-nearest neighbors
+  size_t knn_search(const Eigen::Vector4d& pt, size_t k, size_t* k_indices, double* k_sq_dists) const { return index.knnSearch(pt.data(), k, k_indices, k_sq_dists); }
+
+private:
+  const PointCloud& points;  ///< Input points
+  Index index;               ///< KdTree index
+};
+
+/// @brief KdTree  with arbitrary nanoflann Adaptor
+template <class PointCloud, template <typename, typename, int, typename> class Adaptor>
+class KdTreeGeneric {
+public:
+  using Ptr = std::shared_ptr<KdTreeGeneric>;
+  using ConstPtr = std::shared_ptr<const KdTreeGeneric>;
+
+  /// @brief Constructor
+  /// @param points  Input points
+  explicit KdTreeGeneric(const std::shared_ptr<const PointCloud>& points) : points(points), tree(*points) {}
+
+  /// @brief Constructor
+  /// @param points  Input points
+  explicit KdTreeGeneric(const std::shared_ptr<const PointCloud>& points, int num_threads) : points(points), tree(*points, num_threads) {}
+
+  ~KdTreeGeneric() {}
+
+  /// @brief Find k-nearest neighbors
+  size_t knn_search(const Eigen::Vector4d& pt, size_t k, size_t* k_indices, double* k_sq_dists) const { return tree.knn_search(pt, k, k_indices, k_sq_dists); }
+
+private:
+  const std::shared_ptr<const PointCloud> points;       ///< Input points
+  const UnsafeKdTreeGeneric<PointCloud, Adaptor> tree;  ///< KdTree
+};
+
+/// @brief Standard KdTree (unsafe)
+template <class PointCloud>
+using UnsafeKdTree = UnsafeKdTreeGeneric<PointCloud, nanoflann::KDTreeSingleIndexAdaptor>;
+
+/// @brief Standard KdTree
+template <class PointCloud>
+using KdTree = KdTreeGeneric<PointCloud, nanoflann::KDTreeSingleIndexAdaptor>;
+
+namespace traits {
+
+template <class PointCloud, template <typename, typename, int, typename> class Adaptor>
+struct Traits<UnsafeKdTreeGeneric<PointCloud, Adaptor>> {
+  static size_t knn_search(const UnsafeKdTreeGeneric<PointCloud, Adaptor>& tree, const Eigen::Vector4d& point, size_t k, size_t* k_indices, double* k_sq_dists) {
+    return tree.knn_search(point, k, k_indices, k_sq_dists);
+  }
+};
+
+template <class PointCloud, template <typename, typename, int, typename> class Adaptor>
+struct Traits<KdTreeGeneric<PointCloud, Adaptor>> {
+  static size_t knn_search(const KdTreeGeneric<PointCloud, Adaptor>& tree, const Eigen::Vector4d& point, size_t k, size_t* k_indices, double* k_sq_dists) {
+    return tree.knn_search(point, k, k_indices, k_sq_dists);
+  }
+};
+
+}  // namespace traits
+
+}  // namespace small_gicp

include/small_gicp/ann/deprecated/kdtree_omp.hpp

@@ -0,0 +1,22 @@
+// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide
+// SPDX-License-Identifier: MIT
+#pragma once
+
+#include <Eigen/Core>
+#include <small_gicp/points/traits.hpp>
+#include <small_gicp/ann/kdtree.hpp>
+#include <small_gicp/ann/nanoflann_omp.hpp>
+
+namespace small_gicp {
+
+/// @brief Unsafe KdTree with multi-thread tree construction with OpenMP backend.
+/// @note  This class only parallelizes the tree construction. The search is still single-threaded as in the normal KdTree.
+template <class PointCloud>
+using UnsafeKdTreeOMP = UnsafeKdTreeGeneric<PointCloud, nanoflann::KDTreeSingleIndexAdaptorOMP>;
+
+/// @brief KdTree with multi-thread tree construction with OpenMP backend.
+/// @note  This class only parallelizes the tree construction. The search is still single-threaded as in the normal KdTree.
+template <class PointCloud>
+using KdTreeOMP = KdTreeGeneric<PointCloud, nanoflann::KDTreeSingleIndexAdaptorOMP>;
+
+}  // namespace small_gicp

include/small_gicp/ann/deprecated/kdtree_tbb.hpp

@@ -0,0 +1,22 @@
+// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide
+// SPDX-License-Identifier: MIT
+#pragma once
+
+#include <Eigen/Core>
+#include <small_gicp/points/traits.hpp>
+#include <small_gicp/ann/kdtree.hpp>
+#include <small_gicp/ann/nanoflann_tbb.hpp>
+
+namespace small_gicp {
+
+/// @brief Unsafe KdTree with multi-thread tree construction with TBB backend.
+/// @note  This class only parallelizes the tree construction. The search is still single-threaded as in the normal KdTree.
+template <class PointCloud>
+using UnsafeKdTreeTBB = UnsafeKdTreeGeneric<PointCloud, nanoflann::KDTreeSingleIndexAdaptorTBB>;
+
+/// @brief KdTree with multi-thread tree construction with TBB backend.
+/// @note  This class only parallelizes the tree construction. The search is still single-threaded as in the normal KdTree.
+template <class PointCloud>
+using KdTreeTBB = KdTreeGeneric<PointCloud, nanoflann::KDTreeSingleIndexAdaptorTBB>;
+
+}  // namespace small_gicp