diff --git a/modules/alphamat/src/3rdparty/KDTreeVectorOfVectorsAdaptor.h b/modules/alphamat/src/3rdparty/KDTreeVectorOfVectorsAdaptor.h index 893aae70ce6..fa3d14caa8b 100644 --- a/modules/alphamat/src/3rdparty/KDTreeVectorOfVectorsAdaptor.h +++ b/modules/alphamat/src/3rdparty/KDTreeVectorOfVectorsAdaptor.h @@ -29,89 +29,106 @@ #pragma once #include "nanoflann.hpp" - #include -// ===== This example shows how to use nanoflann with these types of containers: ======= -//typedef std::vector > my_vector_of_vectors_t; -//typedef std::vector my_vector_of_vectors_t; // This requires #include -// ===================================================================================== - - -/** A simple vector-of-vectors adaptor for nanoflann, without duplicating the storage. - * The i'th vector represents a point in the state space. - * - * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality for the points in the data set, allowing more compiler optimizations. - * \tparam num_t The type of the point coordinates (typically, double or float). - * \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. - * \tparam IndexType The type for indices in the KD-tree index (typically, size_t of int) - */ -template +// ===== This example shows how to use nanoflann with these types of containers: +// using my_vector_of_vectors_t = std::vector > ; +// +// The next one requires #include +// using my_vector_of_vectors_t = std::vector ; +// ============================================================================= + +/** A simple vector-of-vectors adaptor for nanoflann, without duplicating the + * storage. The i'th vector represents a point in the state space. + * + * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality + * for the points in the data set, allowing more compiler optimizations. + * \tparam num_t The type of the point coordinates (typ. double or float). + * \tparam Distance The distance metric to use: nanoflann::metric_L1, + * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. + * \tparam IndexType The type for indices in the KD-tree index + * (typically, size_t of int) + */ +template < + class VectorOfVectorsType, typename num_t = double, int DIM = -1, + class Distance = nanoflann::metric_L2, typename IndexType = size_t> struct KDTreeVectorOfVectorsAdaptor { - typedef KDTreeVectorOfVectorsAdaptor self_t; - typedef typename Distance::template traits::distance_t metric_t; - typedef nanoflann::KDTreeSingleIndexAdaptor< metric_t, self_t, DIM, IndexType> index_t; - - index_t* index; //! The kd-tree index for the user to call its methods as usual with any other FLANN index. - - /// Constructor: takes a const ref to the vector of vectors object with the data points - KDTreeVectorOfVectorsAdaptor(const size_t /* dimensionality */, const VectorOfVectorsType &mat, const int leaf_max_size = 10) : m_data(mat) - { - assert(mat.size() != 0 && mat[0].size() != 0); - const size_t dims = mat[0].size(); - if (DIM>0 && static_cast(dims) != DIM) - throw std::runtime_error("Data set dimensionality does not match the 'DIM' template argument"); - index = new index_t( static_cast(dims), *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size ) ); - index->buildIndex(); - } - - ~KDTreeVectorOfVectorsAdaptor() { - delete index; - } - - const VectorOfVectorsType &m_data; - - /** Query for the \a num_closest closest points to a given point (entered as query_point[0:dim-1]). - * Note that this is a short-cut method for index->findNeighbors(). - * The user can also call index->... methods as desired. - * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface. - */ - //inline void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq, const int nChecks_IGNORED = 10) const - inline void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq) const - { - nanoflann::KNNResultSet resultSet(num_closest); - resultSet.init(out_indices, out_distances_sq); - index->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); - } - - /** @name Interface expected by KDTreeSingleIndexAdaptor - * @{ */ - - const self_t & derived() const { - return *this; - } - self_t & derived() { - return *this; - } - - // Must return the number of data points - inline size_t kdtree_get_point_count() const { - return m_data.size(); - } - - // Returns the dim'th component of the idx'th point in the class: - inline num_t kdtree_get_pt(const size_t idx, const size_t dim) const { - return m_data[idx][dim]; - } - - // Optional bounding-box computation: return false to default to a standard bbox computation loop. - // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again. - // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds) - template - bool kdtree_get_bbox(BBOX & /*bb*/) const { - return false; - } - - /** @} */ + using self_t = KDTreeVectorOfVectorsAdaptor< + VectorOfVectorsType, num_t, DIM, Distance, IndexType>; + using metric_t = + typename Distance::template traits::distance_t; + using index_t = + nanoflann::KDTreeSingleIndexAdaptor; + + /** The kd-tree index for the user to call its methods as usual with any + * other FLANN index */ + index_t* index = nullptr; + + /// Constructor: takes a const ref to the vector of vectors object with the + /// data points + KDTreeVectorOfVectorsAdaptor( + const size_t /* dimensionality */, const VectorOfVectorsType& mat, + const int leaf_max_size = 10, const unsigned int n_thread_build = 1) + : m_data(mat) + { + assert(mat.size() != 0 && mat[0].size() != 0); + const size_t dims = mat[0].size(); + if (DIM > 0 && static_cast(dims) != DIM) + throw std::runtime_error( + "Data set dimensionality does not match the 'DIM' template " + "argument"); + index = new index_t( + static_cast(dims), *this /* adaptor */, + nanoflann::KDTreeSingleIndexAdaptorParams( + leaf_max_size, nanoflann::KDTreeSingleIndexAdaptorFlags::None, + n_thread_build)); + } + + ~KDTreeVectorOfVectorsAdaptor() { delete index; } + + const VectorOfVectorsType& m_data; + + /** Query for the \a num_closest closest points to a given point + * (entered as query_point[0:dim-1]). + * Note that this is a short-cut method for index->findNeighbors(). + * The user can also call index->... methods as desired. + */ + inline void query( + const num_t* query_point, const size_t num_closest, + IndexType* out_indices, num_t* out_distances_sq) const + { + nanoflann::KNNResultSet resultSet(num_closest); + resultSet.init(out_indices, out_distances_sq); + index->findNeighbors(resultSet, query_point); + } + + /** @name Interface expected by KDTreeSingleIndexAdaptor + * @{ */ + + const self_t& derived() const { return *this; } + self_t& derived() { return *this; } + + // Must return the number of data points + inline size_t kdtree_get_point_count() const { return m_data.size(); } + + // Returns the dim'th component of the idx'th point in the class: + inline num_t kdtree_get_pt(const size_t idx, const size_t dim) const + { + return m_data[idx][dim]; + } + + // Optional bounding-box computation: return false to default to a standard + // bbox computation loop. + // Return true if the BBOX was already computed by the class and returned + // in "bb" so it can be avoided to redo it again. Look at bb.size() to + // find out the expected dimensionality (e.g. 2 or 3 for point clouds) + template + bool kdtree_get_bbox(BBOX& /*bb*/) const + { + return false; + } + + /** @} */ + }; // end of KDTreeVectorOfVectorsAdaptor diff --git a/modules/alphamat/src/3rdparty/nanoflann.hpp b/modules/alphamat/src/3rdparty/nanoflann.hpp index a8e4667dda6..2343e074218 100644 --- a/modules/alphamat/src/3rdparty/nanoflann.hpp +++ b/modules/alphamat/src/3rdparty/nanoflann.hpp @@ -3,7 +3,7 @@ * * Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved. * Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved. - * Copyright 2011-2016 Jose Luis Blanco (joseluisblancoc@gmail.com). + * Copyright 2011-2025 Jose Luis Blanco (joseluisblancoc@gmail.com). * All rights reserved. * * THE BSD LICENSE @@ -38,31 +38,33 @@ * #include in your code. * * See: - * - C++ API organized by modules - * - Online README - * - Doxygen - * documentation + * - [Online README](https://github.com/jlblancoc/nanoflann) + * - [C++ API documentation](https://jlblancoc.github.io/nanoflann/) */ -#ifndef NANOFLANN_HPP_ -#define NANOFLANN_HPP_ +#pragma once #include #include +#include #include -#include // for abs() -#include // for fwrite() -#include // for abs() -#include -#include // std::reference_wrapper +#include // for abs() +#include +#include // for abs() +#include // std::reference_wrapper +#include +#include +#include // std::numeric_limits +#include #include +#include #include /** Library version: 0xMmP (M=Major,m=minor,P=patch) */ -#define NANOFLANN_VERSION 0x132 +#define NANOFLANN_VERSION 0x171 -// Avoid conflicting declaration of min/max macros in windows headers -#if !defined(NOMINMAX) && \ +// Avoid conflicting declaration of min/max macros in Windows headers +#if !defined(NOMINMAX) && \ (defined(_WIN32) || defined(_WIN32_) || defined(WIN32) || defined(_WIN64)) #define NOMINMAX #ifdef max @@ -70,39 +72,57 @@ #undef min #endif #endif +// Avoid conflicts with X11 headers +#ifdef None +#undef None +#endif -namespace nanoflann { -/** @addtogroup nanoflann_grp nanoflann C++ library for ANN +namespace nanoflann +{ +/** @addtogroup nanoflann_grp nanoflann C++ library for KD-trees * @{ */ /** the PI constant (required to avoid MSVC missing symbols) */ -template T pi_const() { - return static_cast(3.14159265358979323846); +template +T pi_const() +{ + return static_cast(3.14159265358979323846); } /** * Traits if object is resizable and assignable (typically has a resize | assign * method) */ -template struct has_resize : std::false_type {}; +template +struct has_resize : std::false_type +{ +}; template struct has_resize().resize(1), 0)> - : std::true_type {}; + : std::true_type +{ +}; -template struct has_assign : std::false_type {}; +template +struct has_assign : std::false_type +{ +}; template struct has_assign().assign(1, 0), 0)> - : std::true_type {}; + : std::true_type +{ +}; /** * Free function to resize a resizable object */ template -inline typename std::enable_if::value, void>::type -resize(Container &c, const size_t nElements) { - c.resize(nElements); +inline typename std::enable_if::value, void>::type resize( + Container& c, const size_t nElements) +{ + c.resize(nElements); } /** @@ -111,18 +131,20 @@ resize(Container &c, const size_t nElements) { */ template inline typename std::enable_if::value, void>::type -resize(Container &c, const size_t nElements) { - if (nElements != c.size()) - throw std::logic_error("Try to change the size of a std::array."); + resize(Container& c, const size_t nElements) +{ + if (nElements != c.size()) + throw std::logic_error("Try to change the size of a std::array."); } /** * Free function to assign to a container */ template -inline typename std::enable_if::value, void>::type -assign(Container &c, const size_t nElements, const T &value) { - c.assign(nElements, value); +inline typename std::enable_if::value, void>::type assign( + Container& c, const size_t nElements, const T& value) +{ + c.assign(nElements, value); } /** @@ -130,145 +152,294 @@ assign(Container &c, const size_t nElements, const T &value) { */ template inline typename std::enable_if::value, void>::type -assign(Container &c, const size_t nElements, const T &value) { - for (size_t i = 0; i < nElements; i++) - c[i] = value; + assign(Container& c, const size_t nElements, const T& value) +{ + for (size_t i = 0; i < nElements; i++) c[i] = value; } +/** operator "<" for std::sort() */ +struct IndexDist_Sorter +{ + /** PairType will be typically: ResultItem */ + template + bool operator()(const PairType& p1, const PairType& p2) const + { + return p1.second < p2.second; + } +}; + +/** + * Each result element in RadiusResultSet. Note that distances and indices + * are named `first` and `second` to keep backward-compatibility with the + * `std::pair<>` type used in the past. In contrast, this structure is ensured + * to be `std::is_standard_layout` so it can be used in wrappers to other + * languages. + * See: https://github.com/jlblancoc/nanoflann/issues/166 + */ +template +struct ResultItem +{ + ResultItem() = default; + ResultItem(const IndexType index, const DistanceType distance) + : first(index), second(distance) + { + } + + IndexType first; //!< Index of the sample in the dataset + DistanceType second; //!< Distance from sample to query point +}; + /** @addtogroup result_sets_grp Result set classes * @{ */ -template -class KNNResultSet { -public: - typedef _DistanceType DistanceType; - typedef _IndexType IndexType; - typedef _CountType CountType; - -private: - IndexType *indices; - DistanceType *dists; - CountType capacity; - CountType count; - -public: - inline KNNResultSet(CountType capacity_) - : indices(0), dists(0), capacity(capacity_), count(0) {} - - inline void init(IndexType *indices_, DistanceType *dists_) { - indices = indices_; - dists = dists_; - count = 0; - if (capacity) - dists[capacity - 1] = (std::numeric_limits::max)(); - } - - inline CountType size() const { return count; } - - inline bool full() const { return count == capacity; } - - /** - * Called during search to add an element matching the criteria. - * @return true if the search should be continued, false if the results are - * sufficient - */ - inline bool addPoint(DistanceType dist, IndexType index) { - CountType i; - for (i = count; i > 0; --i) { -#ifdef NANOFLANN_FIRST_MATCH // If defined and two points have the same - // distance, the one with the lowest-index will be - // returned first. - if ((dists[i - 1] > dist) || - ((dist == dists[i - 1]) && (indices[i - 1] > index))) { + +/** Result set for KNN searches (N-closest neighbors) */ +template < + typename _DistanceType, typename _IndexType = size_t, + typename _CountType = size_t> +class KNNResultSet +{ + public: + using DistanceType = _DistanceType; + using IndexType = _IndexType; + using CountType = _CountType; + + private: + IndexType* indices; + DistanceType* dists; + CountType capacity; + CountType count; + + public: + explicit KNNResultSet(CountType capacity_) + : indices(nullptr), dists(nullptr), capacity(capacity_), count(0) + { + } + + void init(IndexType* indices_, DistanceType* dists_) + { + indices = indices_; + dists = dists_; + count = 0; + } + + CountType size() const { return count; } + bool empty() const { return count == 0; } + bool full() const { return count == capacity; } + + /** + * Called during search to add an element matching the criteria. + * @return true if the search should be continued, false if the results are + * sufficient + */ + bool addPoint(DistanceType dist, IndexType index) + { + CountType i; + for (i = count; i > 0; --i) + { + /** If defined and two points have the same distance, the one with + * the lowest-index will be returned first. */ +#ifdef NANOFLANN_FIRST_MATCH + if ((dists[i - 1] > dist) || + ((dist == dists[i - 1]) && (indices[i - 1] > index))) + { #else - if (dists[i - 1] > dist) { + if (dists[i - 1] > dist) + { #endif - if (i < capacity) { - dists[i] = dists[i - 1]; - indices[i] = indices[i - 1]; + if (i < capacity) + { + dists[i] = dists[i - 1]; + indices[i] = indices[i - 1]; + } + } + else + break; } - } else - break; - } - if (i < capacity) { - dists[i] = dist; - indices[i] = index; + if (i < capacity) + { + dists[i] = dist; + indices[i] = index; + } + if (count < capacity) count++; + + // tell caller that the search shall continue + return true; } - if (count < capacity) - count++; - // tell caller that the search shall continue - return true; - } + //! Returns the worst distance among found solutions if the search result is + //! full, or the maximum possible distance, if not full yet. + DistanceType worstDist() const + { + return (count < capacity || !count) + ? std::numeric_limits::max() + : dists[count - 1]; + } - inline DistanceType worstDist() const { return dists[capacity - 1]; } + void sort() + { + // already sorted + } }; -/** operator "<" for std::sort() */ -struct IndexDist_Sorter { - /** PairType will be typically: std::pair */ - template - inline bool operator()(const PairType &p1, const PairType &p2) const { - return p1.second < p2.second; - } +/** Result set for RKNN searches (N-closest neighbors with a maximum radius) */ +template < + typename _DistanceType, typename _IndexType = size_t, + typename _CountType = size_t> +class RKNNResultSet +{ + public: + using DistanceType = _DistanceType; + using IndexType = _IndexType; + using CountType = _CountType; + + private: + IndexType* indices; + DistanceType* dists; + CountType capacity; + CountType count; + DistanceType maximumSearchDistanceSquared; + + public: + explicit RKNNResultSet( + CountType capacity_, DistanceType maximumSearchDistanceSquared_) + : indices(nullptr), + dists(nullptr), + capacity(capacity_), + count(0), + maximumSearchDistanceSquared(maximumSearchDistanceSquared_) + { + } + + void init(IndexType* indices_, DistanceType* dists_) + { + indices = indices_; + dists = dists_; + count = 0; + if (capacity) dists[capacity - 1] = maximumSearchDistanceSquared; + } + + CountType size() const { return count; } + bool empty() const { return count == 0; } + bool full() const { return count == capacity; } + + /** + * Called during search to add an element matching the criteria. + * @return true if the search should be continued, false if the results are + * sufficient + */ + bool addPoint(DistanceType dist, IndexType index) + { + CountType i; + for (i = count; i > 0; --i) + { + /** If defined and two points have the same distance, the one with + * the lowest-index will be returned first. */ +#ifdef NANOFLANN_FIRST_MATCH + if ((dists[i - 1] > dist) || + ((dist == dists[i - 1]) && (indices[i - 1] > index))) + { +#else + if (dists[i - 1] > dist) + { +#endif + if (i < capacity) + { + dists[i] = dists[i - 1]; + indices[i] = indices[i - 1]; + } + } + else + break; + } + if (i < capacity) + { + dists[i] = dist; + indices[i] = index; + } + if (count < capacity) count++; + + // tell caller that the search shall continue + return true; + } + + //! Returns the worst distance among found solutions if the search result is + //! full, or the maximum possible distance, if not full yet. + DistanceType worstDist() const + { + return (count < capacity || !count) ? maximumSearchDistanceSquared + : dists[count - 1]; + } + + void sort() + { + // already sorted + } }; /** * A result-set class used when performing a radius based search. */ template -class RadiusResultSet { -public: - typedef _DistanceType DistanceType; - typedef _IndexType IndexType; - -public: - const DistanceType radius; - - std::vector> &m_indices_dists; - - inline RadiusResultSet( - DistanceType radius_, - std::vector> &indices_dists) - : radius(radius_), m_indices_dists(indices_dists) { - init(); - } - - inline void init() { clear(); } - inline void clear() { m_indices_dists.clear(); } - - inline size_t size() const { return m_indices_dists.size(); } - - inline bool full() const { return true; } - - /** - * Called during search to add an element matching the criteria. - * @return true if the search should be continued, false if the results are - * sufficient - */ - inline bool addPoint(DistanceType dist, IndexType index) { - if (dist < radius) - m_indices_dists.push_back(std::make_pair(index, dist)); - return true; - } - - inline DistanceType worstDist() const { return radius; } - - /** - * Find the worst result (furtherest neighbor) without copying or sorting - * Pre-conditions: size() > 0 - */ - std::pair worst_item() const { - if (m_indices_dists.empty()) - throw std::runtime_error("Cannot invoke RadiusResultSet::worst_item() on " - "an empty list of results."); - typedef - typename std::vector>::const_iterator - DistIt; - DistIt it = std::max_element(m_indices_dists.begin(), m_indices_dists.end(), - IndexDist_Sorter()); - return *it; - } +class RadiusResultSet +{ + public: + using DistanceType = _DistanceType; + using IndexType = _IndexType; + + public: + const DistanceType radius; + + std::vector>& m_indices_dists; + + explicit RadiusResultSet( + DistanceType radius_, + std::vector>& indices_dists) + : radius(radius_), m_indices_dists(indices_dists) + { + init(); + } + + void init() { clear(); } + void clear() { m_indices_dists.clear(); } + + size_t size() const { return m_indices_dists.size(); } + size_t empty() const { return m_indices_dists.empty(); } + + bool full() const { return true; } + + /** + * Called during search to add an element matching the criteria. + * @return true if the search should be continued, false if the results are + * sufficient + */ + bool addPoint(DistanceType dist, IndexType index) + { + if (dist < radius) m_indices_dists.emplace_back(index, dist); + return true; + } + + DistanceType worstDist() const { return radius; } + + /** + * Find the worst result (farthest neighbor) without copying or sorting + * Pre-conditions: size() > 0 + */ + ResultItem worst_item() const + { + if (m_indices_dists.empty()) + throw std::runtime_error( + "Cannot invoke RadiusResultSet::worst_item() on " + "an empty list of results."); + auto it = std::max_element( + m_indices_dists.begin(), m_indices_dists.end(), IndexDist_Sorter()); + return *it; + } + + void sort() + { + std::sort( + m_indices_dists.begin(), m_indices_dists.end(), IndexDist_Sorter()); + } }; /** @} */ @@ -276,266 +447,344 @@ class RadiusResultSet { /** @addtogroup loadsave_grp Load/save auxiliary functions * @{ */ template -void save_value(FILE *stream, const T &value, size_t count = 1) { - fwrite(&value, sizeof(value), count, stream); +void save_value(std::ostream& stream, const T& value) +{ + stream.write(reinterpret_cast(&value), sizeof(T)); } template -void save_value(FILE *stream, const std::vector &value) { - size_t size = value.size(); - fwrite(&size, sizeof(size_t), 1, stream); - fwrite(&value[0], sizeof(T), size, stream); +void save_value(std::ostream& stream, const std::vector& value) +{ + size_t size = value.size(); + stream.write(reinterpret_cast(&size), sizeof(size_t)); + stream.write(reinterpret_cast(value.data()), sizeof(T) * size); } template -void load_value(FILE *stream, T &value, size_t count = 1) { - size_t read_cnt = fread(&value, sizeof(value), count, stream); - if (read_cnt != count) { - throw std::runtime_error("Cannot read from file"); - } +void load_value(std::istream& stream, T& value) +{ + stream.read(reinterpret_cast(&value), sizeof(T)); } -template void load_value(FILE *stream, std::vector &value) { - size_t size; - size_t read_cnt = fread(&size, sizeof(size_t), 1, stream); - if (read_cnt != 1) { - throw std::runtime_error("Cannot read from file"); - } - value.resize(size); - read_cnt = fread(&value[0], sizeof(T), size, stream); - if (read_cnt != size) { - throw std::runtime_error("Cannot read from file"); - } +template +void load_value(std::istream& stream, std::vector& value) +{ + size_t size; + stream.read(reinterpret_cast(&size), sizeof(size_t)); + value.resize(size); + stream.read(reinterpret_cast(value.data()), sizeof(T) * size); } /** @} */ /** @addtogroup metric_grp Metric (distance) classes * @{ */ -struct Metric {}; +struct Metric +{ +}; /** Manhattan distance functor (generic version, optimized for * high-dimensionality data sets). Corresponding distance traits: - * nanoflann::metric_L1 \tparam T Type of the elements (e.g. double, float, - * uint8_t) \tparam _DistanceType Type of distance variables (must be signed) - * (e.g. float, double, int64_t) + * nanoflann::metric_L1 + * + * \tparam T Type of the elements (e.g. double, float, uint8_t) + * \tparam DataSource Source of the data, i.e. where the vectors are stored + * \tparam _DistanceType Type of distance variables (must be signed) + * \tparam IndexType Type of the arguments with which the data can be + * accessed (e.g. float, double, int64_t, T*) */ -template -struct L1_Adaptor { - typedef T ElementType; - typedef _DistanceType DistanceType; - - const DataSource &data_source; - - L1_Adaptor(const DataSource &_data_source) : data_source(_data_source) {} - - inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size, - DistanceType worst_dist = -1) const { - DistanceType result = DistanceType(); - const T *last = a + size; - const T *lastgroup = last - 3; - size_t d = 0; - - /* Process 4 items with each loop for efficiency. */ - while (a < lastgroup) { - const DistanceType diff0 = - std::abs(a[0] - data_source.kdtree_get_pt(b_idx, d++)); - const DistanceType diff1 = - std::abs(a[1] - data_source.kdtree_get_pt(b_idx, d++)); - const DistanceType diff2 = - std::abs(a[2] - data_source.kdtree_get_pt(b_idx, d++)); - const DistanceType diff3 = - std::abs(a[3] - data_source.kdtree_get_pt(b_idx, d++)); - result += diff0 + diff1 + diff2 + diff3; - a += 4; - if ((worst_dist > 0) && (result > worst_dist)) { +template < + class T, class DataSource, typename _DistanceType = T, + typename IndexType = size_t> +struct L1_Adaptor +{ + using ElementType = T; + using DistanceType = _DistanceType; + + const DataSource& data_source; + + L1_Adaptor(const DataSource& _data_source) : data_source(_data_source) {} + + DistanceType evalMetric( + const T* a, const IndexType b_idx, size_t size, + DistanceType worst_dist = -1) const + { + DistanceType result = DistanceType(); + const T* last = a + size; + const T* lastgroup = last - 3; + size_t d = 0; + + /* Process 4 items with each loop for efficiency. */ + while (a < lastgroup) + { + const DistanceType diff0 = + std::abs(a[0] - data_source.kdtree_get_pt(b_idx, d++)); + const DistanceType diff1 = + std::abs(a[1] - data_source.kdtree_get_pt(b_idx, d++)); + const DistanceType diff2 = + std::abs(a[2] - data_source.kdtree_get_pt(b_idx, d++)); + const DistanceType diff3 = + std::abs(a[3] - data_source.kdtree_get_pt(b_idx, d++)); + result += diff0 + diff1 + diff2 + diff3; + a += 4; + if ((worst_dist > 0) && (result > worst_dist)) { return result; } + } + /* Process last 0-3 components. Not needed for standard vector lengths. + */ + while (a < last) + { + result += std::abs(*a++ - data_source.kdtree_get_pt(b_idx, d++)); + } return result; - } } - /* Process last 0-3 components. Not needed for standard vector lengths. */ - while (a < last) { - result += std::abs(*a++ - data_source.kdtree_get_pt(b_idx, d++)); - } - return result; - } - template - inline DistanceType accum_dist(const U a, const V b, const size_t) const { - return std::abs(a - b); - } + template + DistanceType accum_dist(const U a, const V b, const size_t) const + { + return std::abs(a - b); + } }; -/** Squared Euclidean distance functor (generic version, optimized for +/** **Squared** Euclidean distance functor (generic version, optimized for * high-dimensionality data sets). Corresponding distance traits: - * nanoflann::metric_L2 \tparam T Type of the elements (e.g. double, float, - * uint8_t) \tparam _DistanceType Type of distance variables (must be signed) - * (e.g. float, double, int64_t) + * nanoflann::metric_L2 + * + * \tparam T Type of the elements (e.g. double, float, uint8_t) + * \tparam DataSource Source of the data, i.e. where the vectors are stored + * \tparam _DistanceType Type of distance variables (must be signed) + * \tparam IndexType Type of the arguments with which the data can be + * accessed (e.g. float, double, int64_t, T*) */ -template -struct L2_Adaptor { - typedef T ElementType; - typedef _DistanceType DistanceType; - - const DataSource &data_source; - - L2_Adaptor(const DataSource &_data_source) : data_source(_data_source) {} - - inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size, - DistanceType worst_dist = -1) const { - DistanceType result = DistanceType(); - const T *last = a + size; - const T *lastgroup = last - 3; - size_t d = 0; - - /* Process 4 items with each loop for efficiency. */ - while (a < lastgroup) { - const DistanceType diff0 = a[0] - data_source.kdtree_get_pt(b_idx, d++); - const DistanceType diff1 = a[1] - data_source.kdtree_get_pt(b_idx, d++); - const DistanceType diff2 = a[2] - data_source.kdtree_get_pt(b_idx, d++); - const DistanceType diff3 = a[3] - data_source.kdtree_get_pt(b_idx, d++); - result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3; - a += 4; - if ((worst_dist > 0) && (result > worst_dist)) { +template < + class T, class DataSource, typename _DistanceType = T, + typename IndexType = size_t> +struct L2_Adaptor +{ + using ElementType = T; + using DistanceType = _DistanceType; + + const DataSource& data_source; + + L2_Adaptor(const DataSource& _data_source) : data_source(_data_source) {} + + DistanceType evalMetric( + const T* a, const IndexType b_idx, size_t size, + DistanceType worst_dist = -1) const + { + DistanceType result = DistanceType(); + const T* last = a + size; + const T* lastgroup = last - 3; + size_t d = 0; + + /* Process 4 items with each loop for efficiency. */ + while (a < lastgroup) + { + const DistanceType diff0 = + a[0] - data_source.kdtree_get_pt(b_idx, d++); + const DistanceType diff1 = + a[1] - data_source.kdtree_get_pt(b_idx, d++); + const DistanceType diff2 = + a[2] - data_source.kdtree_get_pt(b_idx, d++); + const DistanceType diff3 = + a[3] - data_source.kdtree_get_pt(b_idx, d++); + result += + diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3; + a += 4; + if ((worst_dist > 0) && (result > worst_dist)) { return result; } + } + /* Process last 0-3 components. Not needed for standard vector lengths. + */ + while (a < last) + { + const DistanceType diff0 = + *a++ - data_source.kdtree_get_pt(b_idx, d++); + result += diff0 * diff0; + } return result; - } } - /* Process last 0-3 components. Not needed for standard vector lengths. */ - while (a < last) { - const DistanceType diff0 = *a++ - data_source.kdtree_get_pt(b_idx, d++); - result += diff0 * diff0; - } - return result; - } - template - inline DistanceType accum_dist(const U a, const V b, const size_t) const { - return (a - b) * (a - b); - } + template + DistanceType accum_dist(const U a, const V b, const size_t) const + { + return (a - b) * (a - b); + } }; -/** Squared Euclidean (L2) distance functor (suitable for low-dimensionality +/** **Squared** Euclidean (L2) distance functor (suitable for low-dimensionality * datasets, like 2D or 3D point clouds) Corresponding distance traits: - * nanoflann::metric_L2_Simple \tparam T Type of the elements (e.g. double, - * float, uint8_t) \tparam _DistanceType Type of distance variables (must be - * signed) (e.g. float, double, int64_t) + * nanoflann::metric_L2_Simple + * + * \tparam T Type of the elements (e.g. double, float, uint8_t) + * \tparam DataSource Source of the data, i.e. where the vectors are stored + * \tparam _DistanceType Type of distance variables (must be signed) + * \tparam IndexType Type of the arguments with which the data can be + * accessed (e.g. float, double, int64_t, T*) */ -template -struct L2_Simple_Adaptor { - typedef T ElementType; - typedef _DistanceType DistanceType; - - const DataSource &data_source; - - L2_Simple_Adaptor(const DataSource &_data_source) - : data_source(_data_source) {} - - inline DistanceType evalMetric(const T *a, const size_t b_idx, - size_t size) const { - DistanceType result = DistanceType(); - for (size_t i = 0; i < size; ++i) { - const DistanceType diff = a[i] - data_source.kdtree_get_pt(b_idx, i); - result += diff * diff; - } - return result; - } - - template - inline DistanceType accum_dist(const U a, const V b, const size_t) const { - return (a - b) * (a - b); - } +template < + class T, class DataSource, typename _DistanceType = T, + typename IndexType = size_t> +struct L2_Simple_Adaptor +{ + using ElementType = T; + using DistanceType = _DistanceType; + + const DataSource& data_source; + + L2_Simple_Adaptor(const DataSource& _data_source) + : data_source(_data_source) + { + } + + DistanceType evalMetric( + const T* a, const IndexType b_idx, size_t size) const + { + DistanceType result = DistanceType(); + for (size_t i = 0; i < size; ++i) + { + const DistanceType diff = + a[i] - data_source.kdtree_get_pt(b_idx, i); + result += diff * diff; + } + return result; + } + + template + DistanceType accum_dist(const U a, const V b, const size_t) const + { + return (a - b) * (a - b); + } }; /** SO2 distance functor * Corresponding distance traits: nanoflann::metric_SO2 - * \tparam T Type of the elements (e.g. double, float) + * + * \tparam T Type of the elements (e.g. double, float, uint8_t) + * \tparam DataSource Source of the data, i.e. where the vectors are stored * \tparam _DistanceType Type of distance variables (must be signed) (e.g. * float, double) orientation is constrained to be in [-pi, pi] + * \tparam IndexType Type of the arguments with which the data can be + * accessed (e.g. float, double, int64_t, T*) */ -template -struct SO2_Adaptor { - typedef T ElementType; - typedef _DistanceType DistanceType; - - const DataSource &data_source; - - SO2_Adaptor(const DataSource &_data_source) : data_source(_data_source) {} - - inline DistanceType evalMetric(const T *a, const size_t b_idx, - size_t size) const { - return accum_dist(a[size - 1], data_source.kdtree_get_pt(b_idx, size - 1), - size - 1); - } - - /** Note: this assumes that input angles are already in the range [-pi,pi] */ - template - inline DistanceType accum_dist(const U a, const V b, const size_t) const { - DistanceType result = DistanceType(); - DistanceType PI = pi_const(); - result = b - a; - if (result > PI) - result -= 2 * PI; - else if (result < -PI) - result += 2 * PI; - return result; - } +template < + class T, class DataSource, typename _DistanceType = T, + typename IndexType = size_t> +struct SO2_Adaptor +{ + using ElementType = T; + using DistanceType = _DistanceType; + + const DataSource& data_source; + + SO2_Adaptor(const DataSource& _data_source) : data_source(_data_source) {} + + DistanceType evalMetric( + const T* a, const IndexType b_idx, size_t size) const + { + return accum_dist( + a[size - 1], data_source.kdtree_get_pt(b_idx, size - 1), size - 1); + } + + /** Note: this assumes that input angles are already in the range [-pi,pi] + */ + template + DistanceType accum_dist(const U a, const V b, const size_t) const + { + DistanceType result = DistanceType(); + DistanceType PI = pi_const(); + result = b - a; + if (result > PI) + result -= 2 * PI; + else if (result < -PI) + result += 2 * PI; + return result; + } }; /** SO3 distance functor (Uses L2_Simple) * Corresponding distance traits: nanoflann::metric_SO3 - * \tparam T Type of the elements (e.g. double, float) + * + * \tparam T Type of the elements (e.g. double, float, uint8_t) + * \tparam DataSource Source of the data, i.e. where the vectors are stored * \tparam _DistanceType Type of distance variables (must be signed) (e.g. * float, double) + * \tparam IndexType Type of the arguments with which the data can be + * accessed (e.g. float, double, int64_t, T*) */ -template -struct SO3_Adaptor { - typedef T ElementType; - typedef _DistanceType DistanceType; - - L2_Simple_Adaptor distance_L2_Simple; - - SO3_Adaptor(const DataSource &_data_source) - : distance_L2_Simple(_data_source) {} +template < + class T, class DataSource, typename _DistanceType = T, + typename IndexType = size_t> +struct SO3_Adaptor +{ + using ElementType = T; + using DistanceType = _DistanceType; + + L2_Simple_Adaptor + distance_L2_Simple; + + SO3_Adaptor(const DataSource& _data_source) + : distance_L2_Simple(_data_source) + { + } - inline DistanceType evalMetric(const T *a, const size_t b_idx, - size_t size) const { - return distance_L2_Simple.evalMetric(a, b_idx, size); - } + DistanceType evalMetric( + const T* a, const IndexType b_idx, size_t size) const + { + return distance_L2_Simple.evalMetric(a, b_idx, size); + } - template - inline DistanceType accum_dist(const U a, const V b, const size_t idx) const { - return distance_L2_Simple.accum_dist(a, b, idx); - } + template + DistanceType accum_dist(const U a, const V b, const size_t idx) const + { + return distance_L2_Simple.accum_dist(a, b, idx); + } }; /** Metaprogramming helper traits class for the L1 (Manhattan) metric */ -struct metric_L1 : public Metric { - template struct traits { - typedef L1_Adaptor distance_t; - }; +struct metric_L1 : public Metric +{ + template + struct traits + { + using distance_t = L1_Adaptor; + }; }; -/** Metaprogramming helper traits class for the L2 (Euclidean) metric */ -struct metric_L2 : public Metric { - template struct traits { - typedef L2_Adaptor distance_t; - }; +/** Metaprogramming helper traits class for the L2 (Euclidean) **squared** + * distance metric */ +struct metric_L2 : public Metric +{ + template + struct traits + { + using distance_t = L2_Adaptor; + }; }; -/** Metaprogramming helper traits class for the L2_simple (Euclidean) metric */ -struct metric_L2_Simple : public Metric { - template struct traits { - typedef L2_Simple_Adaptor distance_t; - }; +/** Metaprogramming helper traits class for the L2_simple (Euclidean) + * **squared** distance metric */ +struct metric_L2_Simple : public Metric +{ + template + struct traits + { + using distance_t = L2_Simple_Adaptor; + }; }; /** Metaprogramming helper traits class for the SO3_InnerProdQuat metric */ -struct metric_SO2 : public Metric { - template struct traits { - typedef SO2_Adaptor distance_t; - }; +struct metric_SO2 : public Metric +{ + template + struct traits + { + using distance_t = SO2_Adaptor; + }; }; /** Metaprogramming helper traits class for the SO3_InnerProdQuat metric */ -struct metric_SO3 : public Metric { - template struct traits { - typedef SO3_Adaptor distance_t; - }; +struct metric_SO3 : public Metric +{ + template + struct traits + { + using distance_t = SO3_Adaptor; + }; }; /** @} */ @@ -543,44 +792,56 @@ struct metric_SO3 : public Metric { /** @addtogroup param_grp Parameter structs * @{ */ +enum class KDTreeSingleIndexAdaptorFlags +{ + None = 0, + SkipInitialBuildIndex = 1 +}; + +inline std::underlying_type::type operator&( + KDTreeSingleIndexAdaptorFlags lhs, KDTreeSingleIndexAdaptorFlags rhs) +{ + using underlying = + typename std::underlying_type::type; + return static_cast(lhs) & static_cast(rhs); +} + /** Parameters (see README.md) */ -struct KDTreeSingleIndexAdaptorParams { - KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size = 10) - : leaf_max_size(_leaf_max_size) {} +struct KDTreeSingleIndexAdaptorParams +{ + KDTreeSingleIndexAdaptorParams( + size_t _leaf_max_size = 10, + KDTreeSingleIndexAdaptorFlags _flags = + KDTreeSingleIndexAdaptorFlags::None, + unsigned int _n_thread_build = 1) + : leaf_max_size(_leaf_max_size), + flags(_flags), + n_thread_build(_n_thread_build) + { + } - size_t leaf_max_size; + size_t leaf_max_size; + KDTreeSingleIndexAdaptorFlags flags; + unsigned int n_thread_build; }; /** Search options for KDTreeSingleIndexAdaptor::findNeighbors() */ -struct SearchParams { - /** Note: The first argument (checks_IGNORED_) is ignored, but kept for - * compatibility with the FLANN interface */ - SearchParams(int checks_IGNORED_ = 32, float eps_ = 0, bool sorted_ = true) - : checks(checks_IGNORED_), eps(eps_), sorted(sorted_) {} - - int checks; //!< Ignored parameter (Kept for compatibility with the FLANN - //!< interface). - float eps; //!< search for eps-approximate neighbours (default: 0) - bool sorted; //!< only for radius search, require neighbours sorted by - //!< distance (default: true) +struct SearchParameters +{ + SearchParameters(float eps_ = 0, bool sorted_ = true) + : eps(eps_), sorted(sorted_) + { + } + + float eps; //!< search for eps-approximate neighbours (default: 0) + bool sorted; //!< only for radius search, require neighbours sorted by + //!< distance (default: true) }; /** @} */ /** @addtogroup memalloc_grp Memory allocation * @{ */ -/** - * Allocates (using C's malloc) a generic type T. - * - * Params: - * count = number of instances to allocate. - * Returns: pointer (of type T*) to memory buffer - */ -template inline T *allocate(size_t count = 1) { - T *mem = static_cast(::malloc(sizeof(T) * count)); - return mem; -} - /** * Pooled storage allocator * @@ -595,115 +856,117 @@ template inline T *allocate(size_t count = 1) { * is no need to track down all the objects to free them. * */ +class PooledAllocator +{ + static constexpr size_t WORDSIZE = 16; // WORDSIZE must >= 8 + static constexpr size_t BLOCKSIZE = 8192; + + /* We maintain memory alignment to word boundaries by requiring that all + allocations be in multiples of the machine wordsize. */ + /* Size of machine word in bytes. Must be power of 2. */ + /* Minimum number of bytes requested at a time from the system. Must be + * multiple of WORDSIZE. */ + + using Size = size_t; + + Size remaining_ = 0; //!< Number of bytes left in current block of storage + void* base_ = nullptr; //!< Pointer to base of current block of storage + void* loc_ = nullptr; //!< Current location in block to next allocate + + void internal_init() + { + remaining_ = 0; + base_ = nullptr; + usedMemory = 0; + wastedMemory = 0; + } + + public: + Size usedMemory = 0; + Size wastedMemory = 0; -const size_t WORDSIZE = 16; -const size_t BLOCKSIZE = 8192; - -class PooledAllocator { - /* We maintain memory alignment to word boundaries by requiring that all - allocations be in multiples of the machine wordsize. */ - /* Size of machine word in bytes. Must be power of 2. */ - /* Minimum number of bytes requested at a time from the system. Must be - * multiple of WORDSIZE. */ - - size_t remaining; /* Number of bytes left in current block of storage. */ - void *base; /* Pointer to base of current block of storage. */ - void *loc; /* Current location in block to next allocate memory. */ - - void internal_init() { - remaining = 0; - base = NULL; - usedMemory = 0; - wastedMemory = 0; - } - -public: - size_t usedMemory; - size_t wastedMemory; - - /** - Default constructor. Initializes a new pool. - */ - PooledAllocator() { internal_init(); } - - /** - * Destructor. Frees all the memory allocated in this pool. - */ - ~PooledAllocator() { free_all(); } - - /** Frees all allocated memory chunks */ - void free_all() { - while (base != NULL) { - void *prev = - *(static_cast(base)); /* Get pointer to prev block. */ - ::free(base); - base = prev; - } - internal_init(); - } - - /** - * Returns a pointer to a piece of new memory of the given size in bytes - * allocated from the pool. - */ - void *malloc(const size_t req_size) { - /* Round size up to a multiple of wordsize. The following expression - only works for WORDSIZE that is a power of 2, by masking last bits of - incremented size to zero. + /** + Default constructor. Initializes a new pool. */ - const size_t size = (req_size + (WORDSIZE - 1)) & ~(WORDSIZE - 1); + PooledAllocator() { internal_init(); } - /* Check whether a new block must be allocated. Note that the first word - of a block is reserved for a pointer to the previous block. + /** + * Destructor. Frees all the memory allocated in this pool. */ - if (size > remaining) { - - wastedMemory += remaining; - - /* Allocate new storage. */ - const size_t blocksize = - (size + sizeof(void *) + (WORDSIZE - 1) > BLOCKSIZE) - ? size + sizeof(void *) + (WORDSIZE - 1) - : BLOCKSIZE; - - // use the standard C malloc to allocate memory - void *m = ::malloc(blocksize); - if (!m) { - fprintf(stderr, "Failed to allocate memory.\n"); - return NULL; - } - - /* Fill first word of new block with pointer to previous block. */ - static_cast(m)[0] = base; - base = m; - - size_t shift = 0; - // int size_t = (WORDSIZE - ( (((size_t)m) + sizeof(void*)) & - // (WORDSIZE-1))) & (WORDSIZE-1); - - remaining = blocksize - sizeof(void *) - shift; - loc = (static_cast(m) + sizeof(void *) + shift); - } - void *rloc = loc; - loc = static_cast(loc) + size; - remaining -= size; - - usedMemory += size; - - return rloc; - } - - /** - * Allocates (using this pool) a generic type T. - * - * Params: - * count = number of instances to allocate. - * Returns: pointer (of type T*) to memory buffer - */ - template T *allocate(const size_t count = 1) { - T *mem = static_cast(this->malloc(sizeof(T) * count)); - return mem; - } + ~PooledAllocator() { free_all(); } + + /** Frees all allocated memory chunks */ + void free_all() + { + while (base_ != nullptr) + { + // Get pointer to prev block + void* prev = *(static_cast(base_)); + ::free(base_); + base_ = prev; + } + internal_init(); + } + + /** + * Returns a pointer to a piece of new memory of the given size in bytes + * allocated from the pool. + */ + void* malloc(const size_t req_size) + { + /* Round size up to a multiple of wordsize. The following expression + only works for WORDSIZE that is a power of 2, by masking last bits + of incremented size to zero. + */ + const Size size = (req_size + (WORDSIZE - 1)) & ~(WORDSIZE - 1); + + /* Check whether a new block must be allocated. Note that the first + word of a block is reserved for a pointer to the previous block. + */ + if (size > remaining_) + { + wastedMemory += remaining_; + + /* Allocate new storage. */ + const Size blocksize = + size > BLOCKSIZE ? size + WORDSIZE : BLOCKSIZE + WORDSIZE; + + // use the standard C malloc to allocate memory + void* m = ::malloc(blocksize); + if (!m) + { + throw std::bad_alloc(); + } + + /* Fill first word of new block with pointer to previous block. */ + static_cast(m)[0] = base_; + base_ = m; + + remaining_ = blocksize - WORDSIZE; + loc_ = static_cast(m) + WORDSIZE; + } + void* rloc = loc_; + loc_ = static_cast(loc_) + size; + remaining_ -= size; + + usedMemory += size; + + return rloc; + } + + /** + * Allocates (using this pool) a generic type T. + * + * Params: + * count = number of instances to allocate. + * Returns: pointer (of type T*) to memory buffer + */ + template + T* allocate(const size_t count = 1) + { + T* mem = static_cast(this->malloc(sizeof(T) * count)); + return mem; + } }; /** @} */ @@ -713,12 +976,16 @@ class PooledAllocator { /** Used to declare fixed-size arrays when DIM>0, dynamically-allocated vectors * when DIM=-1. Fixed size version for a generic DIM: */ -template struct array_or_vector_selector { - typedef std::array container_t; +template +struct array_or_vector +{ + using type = std::array; }; /** Dynamic size version */ -template struct array_or_vector_selector<-1, T> { - typedef std::vector container_t; +template +struct array_or_vector<-1, T> +{ + using type = std::vector; }; /** @} */ @@ -729,347 +996,506 @@ template struct array_or_vector_selector<-1, T> { * and KDTreeSingleIndexDynamicAdaptor_. * * \tparam Derived The name of the class which inherits this class. - * \tparam DatasetAdaptor The user-provided adaptor (see comments above). + * \tparam DatasetAdaptor The user-provided adaptor, which must be ensured to + * have a lifetime equal or longer than the instance of this class. * \tparam Distance The distance metric to use, these are all classes derived - * from nanoflann::Metric \tparam DIM Dimensionality of data points (e.g. 3 for - * 3D points) \tparam IndexType Will be typically size_t or int + * from nanoflann::Metric + * \tparam DIM Dimensionality of data points (e.g. 3 for 3D points) + * \tparam IndexType Type of the arguments with which the data can be + * accessed (e.g. float, double, int64_t, T*) */ +template < + class Derived, typename Distance, class DatasetAdaptor, int32_t DIM = -1, + typename index_t = uint32_t> +class KDTreeBaseClass +{ + public: + /** Frees the previously-built index. Automatically called within + * buildIndex(). */ + void freeIndex(Derived& obj) + { + obj.pool_.free_all(); + obj.root_node_ = nullptr; + obj.size_at_index_build_ = 0; + } + + using ElementType = typename Distance::ElementType; + using DistanceType = typename Distance::DistanceType; + using IndexType = index_t; + + /** + * Array of indices to vectors in the dataset_. + */ + std::vector vAcc_; + + using Offset = typename decltype(vAcc_)::size_type; + using Size = typename decltype(vAcc_)::size_type; + using Dimension = int32_t; + + /*--------------------------- + * Internal Data Structures + * --------------------------*/ + struct Node + { + /** Union used because a node can be either a LEAF node or a non-leaf + * node, so both data fields are never used simultaneously */ + union + { + struct leaf + { + Offset left, right; //!< Indices of points in leaf node + } lr; + struct nonleaf + { + Dimension divfeat; //!< Dimension used for subdivision. + /// The values used for subdivision. + DistanceType divlow, divhigh; + } sub; + } node_type; + + /** Child nodes (both=nullptr mean its a leaf node) */ + Node *child1 = nullptr, *child2 = nullptr; + }; + + using NodePtr = Node*; + using NodeConstPtr = const Node*; + + struct Interval + { + ElementType low, high; + }; + + NodePtr root_node_ = nullptr; + + Size leaf_max_size_ = 0; + + /// Number of thread for concurrent tree build + Size n_thread_build_ = 1; + /// Number of current points in the dataset + Size size_ = 0; + /// Number of points in the dataset when the index was built + Size size_at_index_build_ = 0; + Dimension dim_ = 0; //!< Dimensionality of each data point + + /** Define "BoundingBox" as a fixed-size or variable-size container + * depending on "DIM" */ + using BoundingBox = typename array_or_vector::type; + + /** Define "distance_vector_t" as a fixed-size or variable-size container + * depending on "DIM" */ + using distance_vector_t = typename array_or_vector::type; + + /** The KD-tree used to find neighbours */ + BoundingBox root_bbox_; + + /** + * Pooled memory allocator. + * + * Using a pooled memory allocator is more efficient + * than allocating memory directly when there is a large + * number small of memory allocations. + */ + PooledAllocator pool_; + + /** Returns number of points in dataset */ + Size size(const Derived& obj) const { return obj.size_; } + + /** Returns the length of each point in the dataset */ + Size veclen(const Derived& obj) const { return DIM > 0 ? DIM : obj.dim_; } + + /// Helper accessor to the dataset points: + ElementType dataset_get( + const Derived& obj, IndexType element, Dimension component) const + { + return obj.dataset_.kdtree_get_pt(element, component); + } + + /** + * Computes the index memory usage + * Returns: memory used by the index + */ + Size usedMemory(const Derived& obj) const + { + return obj.pool_.usedMemory + obj.pool_.wastedMemory + + obj.dataset_.kdtree_get_point_count() * + sizeof(IndexType); // pool memory and vind array memory + } + + /** + * Compute the minimum and maximum element values in the specified dimension + */ + void computeMinMax( + const Derived& obj, Offset ind, Size count, Dimension element, + ElementType& min_elem, ElementType& max_elem) const + { + min_elem = dataset_get(obj, vAcc_[ind], element); + max_elem = min_elem; + for (Offset i = 1; i < count; ++i) + { + ElementType val = dataset_get(obj, vAcc_[ind + i], element); + if (val < min_elem) min_elem = val; + if (val > max_elem) max_elem = val; + } + } + + /** + * Create a tree node that subdivides the list of vecs from vind[first] + * to vind[last]. The routine is called recursively on each sublist. + * + * @param left index of the first vector + * @param right index of the last vector + * @param bbox bounding box used as input for splitting and output for parent node + */ + NodePtr divideTree( + Derived& obj, const Offset left, const Offset right, BoundingBox& bbox) + { + assert(left < obj.dataset_.kdtree_get_point_count()); + + NodePtr node = obj.pool_.template allocate(); // allocate memory + const auto dims = (DIM > 0 ? DIM : obj.dim_); + + /* If too few exemplars remain, then make this a leaf node. */ + if ((right - left) <= static_cast(obj.leaf_max_size_)) + { + node->child1 = node->child2 = nullptr; /* Mark as leaf node. */ + node->node_type.lr.left = left; + node->node_type.lr.right = right; + + // compute bounding-box of leaf points + for (Dimension i = 0; i < dims; ++i) + { + bbox[i].low = dataset_get(obj, obj.vAcc_[left], i); + bbox[i].high = dataset_get(obj, obj.vAcc_[left], i); + } + for (Offset k = left + 1; k < right; ++k) + { + for (Dimension i = 0; i < dims; ++i) + { + const auto val = dataset_get(obj, obj.vAcc_[k], i); + if (bbox[i].low > val) bbox[i].low = val; + if (bbox[i].high < val) bbox[i].high = val; + } + } + } + else + { + /* Determine the index, dimension and value for split plane */ + Offset idx; + Dimension cutfeat; + DistanceType cutval; + middleSplit_(obj, left, right - left, idx, cutfeat, cutval, bbox); + + node->node_type.sub.divfeat = cutfeat; + + /* Recurse on left */ + BoundingBox left_bbox(bbox); + left_bbox[cutfeat].high = cutval; + node->child1 = this->divideTree(obj, left, left + idx, left_bbox); + + /* Recurse on right */ + BoundingBox right_bbox(bbox); + right_bbox[cutfeat].low = cutval; + node->child2 = this->divideTree(obj, left + idx, right, right_bbox); + + node->node_type.sub.divlow = left_bbox[cutfeat].high; + node->node_type.sub.divhigh = right_bbox[cutfeat].low; + + for (Dimension i = 0; i < dims; ++i) + { + bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low); + bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high); + } + } + + return node; + } -template -class KDTreeBaseClass { - -public: - /** Frees the previously-built index. Automatically called within - * buildIndex(). */ - void freeIndex(Derived &obj) { - obj.pool.free_all(); - obj.root_node = NULL; - obj.m_size_at_index_build = 0; - } - - typedef typename Distance::ElementType ElementType; - typedef typename Distance::DistanceType DistanceType; - - /*--------------------- Internal Data Structures --------------------------*/ - struct Node { - /** Union used because a node can be either a LEAF node or a non-leaf node, - * so both data fields are never used simultaneously */ - union { - struct leaf { - IndexType left, right; //!< Indices of points in leaf node - } lr; - struct nonleaf { - int divfeat; //!< Dimension used for subdivision. - DistanceType divlow, divhigh; //!< The values used for subdivision. - } sub; - } node_type; - Node *child1, *child2; //!< Child nodes (both=NULL mean its a leaf node) - }; - - typedef Node *NodePtr; - - struct Interval { - ElementType low, high; - }; - - /** - * Array of indices to vectors in the dataset. - */ - std::vector vind; - - NodePtr root_node; - - size_t m_leaf_max_size; - - size_t m_size; //!< Number of current points in the dataset - size_t m_size_at_index_build; //!< Number of points in the dataset when the - //!< index was built - int dim; //!< Dimensionality of each data point - - /** Define "BoundingBox" as a fixed-size or variable-size container depending - * on "DIM" */ - typedef - typename array_or_vector_selector::container_t BoundingBox; - - /** Define "distance_vector_t" as a fixed-size or variable-size container - * depending on "DIM" */ - typedef typename array_or_vector_selector::container_t - distance_vector_t; - - /** The KD-tree used to find neighbours */ - - BoundingBox root_bbox; - - /** - * Pooled memory allocator. - * - * Using a pooled memory allocator is more efficient - * than allocating memory directly when there is a large - * number small of memory allocations. - */ - PooledAllocator pool; - - /** Returns number of points in dataset */ - size_t size(const Derived &obj) const { return obj.m_size; } - - /** Returns the length of each point in the dataset */ - size_t veclen(const Derived &obj) { - return static_cast(DIM > 0 ? DIM : obj.dim); - } - - /// Helper accessor to the dataset points: - inline ElementType dataset_get(const Derived &obj, size_t idx, - int component) const { - return obj.dataset.kdtree_get_pt(idx, component); - } - - /** - * Computes the inde memory usage - * Returns: memory used by the index - */ - size_t usedMemory(Derived &obj) { - return obj.pool.usedMemory + obj.pool.wastedMemory + - obj.dataset.kdtree_get_point_count() * - sizeof(IndexType); // pool memory and vind array memory - } - - void computeMinMax(const Derived &obj, IndexType *ind, IndexType count, - int element, ElementType &min_elem, - ElementType &max_elem) { - min_elem = dataset_get(obj, ind[0], element); - max_elem = dataset_get(obj, ind[0], element); - for (IndexType i = 1; i < count; ++i) { - ElementType val = dataset_get(obj, ind[i], element); - if (val < min_elem) - min_elem = val; - if (val > max_elem) - max_elem = val; - } - } - - /** - * Create a tree node that subdivides the list of vecs from vind[first] - * to vind[last]. The routine is called recursively on each sublist. - * - * @param left index of the first vector - * @param right index of the last vector - */ - NodePtr divideTree(Derived &obj, const IndexType left, const IndexType right, - BoundingBox &bbox) { - NodePtr node = obj.pool.template allocate(); // allocate memory - - /* If too few exemplars remain, then make this a leaf node. */ - if ((right - left) <= static_cast(obj.m_leaf_max_size)) { - node->child1 = node->child2 = NULL; /* Mark as leaf node. */ - node->node_type.lr.left = left; - node->node_type.lr.right = right; - - // compute bounding-box of leaf points - for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { - bbox[i].low = dataset_get(obj, obj.vind[left], i); - bbox[i].high = dataset_get(obj, obj.vind[left], i); - } - for (IndexType k = left + 1; k < right; ++k) { - for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { - if (bbox[i].low > dataset_get(obj, obj.vind[k], i)) - bbox[i].low = dataset_get(obj, obj.vind[k], i); - if (bbox[i].high < dataset_get(obj, obj.vind[k], i)) - bbox[i].high = dataset_get(obj, obj.vind[k], i); + /** + * Create a tree node that subdivides the list of vecs from vind[first] to + * vind[last] concurrently. The routine is called recursively on each + * sublist. + * + * @param left index of the first vector + * @param right index of the last vector + * @param bbox bounding box used as input for splitting and output for parent node + * @param thread_count count of std::async threads + * @param mutex mutex for mempool allocation + */ + NodePtr divideTreeConcurrent( + Derived& obj, const Offset left, const Offset right, BoundingBox& bbox, + std::atomic& thread_count, std::mutex& mutex) + { + std::unique_lock lock(mutex); + NodePtr node = obj.pool_.template allocate(); // allocate memory + lock.unlock(); + + const auto dims = (DIM > 0 ? DIM : obj.dim_); + + /* If too few exemplars remain, then make this a leaf node. */ + if ((right - left) <= static_cast(obj.leaf_max_size_)) + { + node->child1 = node->child2 = nullptr; /* Mark as leaf node. */ + node->node_type.lr.left = left; + node->node_type.lr.right = right; + + // compute bounding-box of leaf points + for (Dimension i = 0; i < dims; ++i) + { + bbox[i].low = dataset_get(obj, obj.vAcc_[left], i); + bbox[i].high = dataset_get(obj, obj.vAcc_[left], i); + } + for (Offset k = left + 1; k < right; ++k) + { + for (Dimension i = 0; i < dims; ++i) + { + const auto val = dataset_get(obj, obj.vAcc_[k], i); + if (bbox[i].low > val) bbox[i].low = val; + if (bbox[i].high < val) bbox[i].high = val; + } + } + } + else + { + /* Determine the index, dimension and value for split plane */ + Offset idx; + Dimension cutfeat; + DistanceType cutval; + middleSplit_(obj, left, right - left, idx, cutfeat, cutval, bbox); + + node->node_type.sub.divfeat = cutfeat; + + std::future right_future; + + /* Recurse on right concurrently, if possible */ + + BoundingBox right_bbox(bbox); + right_bbox[cutfeat].low = cutval; + if (++thread_count < n_thread_build_) + { + /* Concurrent thread for right recursion */ + + right_future = std::async( + std::launch::async, &KDTreeBaseClass::divideTreeConcurrent, + this, std::ref(obj), left + idx, right, + std::ref(right_bbox), std::ref(thread_count), + std::ref(mutex)); + } + else { --thread_count; } + + /* Recurse on left in this thread */ + + BoundingBox left_bbox(bbox); + left_bbox[cutfeat].high = cutval; + node->child1 = this->divideTreeConcurrent( + obj, left, left + idx, left_bbox, thread_count, mutex); + + if (right_future.valid()) + { + /* Block and wait for concurrent right from above */ + + node->child2 = right_future.get(); + --thread_count; + } + else + { + /* Otherwise, recurse on right in this thread */ + + node->child2 = this->divideTreeConcurrent( + obj, left + idx, right, right_bbox, thread_count, mutex); + } + + node->node_type.sub.divlow = left_bbox[cutfeat].high; + node->node_type.sub.divhigh = right_bbox[cutfeat].low; + + for (Dimension i = 0; i < dims; ++i) + { + bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low); + bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high); + } } - } - } else { - IndexType idx; - int cutfeat; - DistanceType cutval; - middleSplit_(obj, &obj.vind[0] + left, right - left, idx, cutfeat, cutval, - bbox); - - node->node_type.sub.divfeat = cutfeat; - - BoundingBox left_bbox(bbox); - left_bbox[cutfeat].high = cutval; - node->child1 = divideTree(obj, left, left + idx, left_bbox); - - BoundingBox right_bbox(bbox); - right_bbox[cutfeat].low = cutval; - node->child2 = divideTree(obj, left + idx, right, right_bbox); - - node->node_type.sub.divlow = left_bbox[cutfeat].high; - node->node_type.sub.divhigh = right_bbox[cutfeat].low; - - for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { - bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low); - bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high); - } - } - - return node; - } - - void middleSplit_(Derived &obj, IndexType *ind, IndexType count, - IndexType &index, int &cutfeat, DistanceType &cutval, - const BoundingBox &bbox) { - const DistanceType EPS = static_cast(0.00001); - ElementType max_span = bbox[0].high - bbox[0].low; - for (int i = 1; i < (DIM > 0 ? DIM : obj.dim); ++i) { - ElementType span = bbox[i].high - bbox[i].low; - if (span > max_span) { - max_span = span; - } - } - ElementType max_spread = -1; - cutfeat = 0; - for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { - ElementType span = bbox[i].high - bbox[i].low; - if (span > (1 - EPS) * max_span) { - ElementType min_elem, max_elem; - computeMinMax(obj, ind, count, i, min_elem, max_elem); - ElementType spread = max_elem - min_elem; - ; - if (spread > max_spread) { - cutfeat = i; - max_spread = spread; + + return node; + } + + void middleSplit_( + const Derived& obj, const Offset ind, const Size count, Offset& index, + Dimension& cutfeat, DistanceType& cutval, const BoundingBox& bbox) + { + const auto dims = (DIM > 0 ? DIM : obj.dim_); + const auto EPS = static_cast(0.00001); + ElementType max_span = bbox[0].high - bbox[0].low; + for (Dimension i = 1; i < dims; ++i) + { + ElementType span = bbox[i].high - bbox[i].low; + if (span > max_span) { max_span = span; } } - } - } - // split in the middle - DistanceType split_val = (bbox[cutfeat].low + bbox[cutfeat].high) / 2; - ElementType min_elem, max_elem; - computeMinMax(obj, ind, count, cutfeat, min_elem, max_elem); - - if (split_val < min_elem) - cutval = min_elem; - else if (split_val > max_elem) - cutval = max_elem; - else - cutval = split_val; - - IndexType lim1, lim2; - planeSplit(obj, ind, count, cutfeat, cutval, lim1, lim2); - - if (lim1 > count / 2) - index = lim1; - else if (lim2 < count / 2) - index = lim2; - else - index = count / 2; - } - - /** - * Subdivide the list of points by a plane perpendicular on axe corresponding - * to the 'cutfeat' dimension at 'cutval' position. - * - * On return: - * dataset[ind[0..lim1-1]][cutfeat]cutval - */ - void planeSplit(Derived &obj, IndexType *ind, const IndexType count, - int cutfeat, DistanceType &cutval, IndexType &lim1, - IndexType &lim2) { - /* Move vector indices for left subtree to front of list. */ - IndexType left = 0; - IndexType right = count - 1; - for (;;) { - while (left <= right && dataset_get(obj, ind[left], cutfeat) < cutval) - ++left; - while (right && left <= right && - dataset_get(obj, ind[right], cutfeat) >= cutval) - --right; - if (left > right || !right) - break; // "!right" was added to support unsigned Index types - std::swap(ind[left], ind[right]); - ++left; - --right; - } - /* If either list is empty, it means that all remaining features - * are identical. Split in the middle to maintain a balanced tree. + ElementType max_spread = -1; + cutfeat = 0; + ElementType min_elem = 0, max_elem = 0; + for (Dimension i = 0; i < dims; ++i) + { + ElementType span = bbox[i].high - bbox[i].low; + if (span >= (1 - EPS) * max_span) + { + ElementType min_elem_, max_elem_; + computeMinMax(obj, ind, count, i, min_elem_, max_elem_); + ElementType spread = max_elem_ - min_elem_; + if (spread > max_spread) + { + cutfeat = i; + max_spread = spread; + min_elem = min_elem_; + max_elem = max_elem_; + } + } + } + // split in the middle + DistanceType split_val = (bbox[cutfeat].low + bbox[cutfeat].high) / 2; + + if (split_val < min_elem) + cutval = min_elem; + else if (split_val > max_elem) + cutval = max_elem; + else + cutval = split_val; + + Offset lim1, lim2; + planeSplit(obj, ind, count, cutfeat, cutval, lim1, lim2); + + if (lim1 > count / 2) + index = lim1; + else if (lim2 < count / 2) + index = lim2; + else + index = count / 2; + } + + /** + * Subdivide the list of points by a plane perpendicular on the axis + * corresponding to the 'cutfeat' dimension at 'cutval' position. + * + * On return: + * dataset[ind[0..lim1-1]][cutfeat] < cutval + * dataset[ind[lim1..lim2-1]][cutfeat] == cutval + * dataset[ind[lim2..count]][cutfeat] > cutval */ - lim1 = left; - right = count - 1; - for (;;) { - while (left <= right && dataset_get(obj, ind[left], cutfeat) <= cutval) - ++left; - while (right && left <= right && - dataset_get(obj, ind[right], cutfeat) > cutval) - --right; - if (left > right || !right) - break; // "!right" was added to support unsigned Index types - std::swap(ind[left], ind[right]); - ++left; - --right; - } - lim2 = left; - } - - DistanceType computeInitialDistances(const Derived &obj, - const ElementType *vec, - distance_vector_t &dists) const { - assert(vec); - DistanceType distsq = DistanceType(); - - for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { - if (vec[i] < obj.root_bbox[i].low) { - dists[i] = obj.distance.accum_dist(vec[i], obj.root_bbox[i].low, i); - distsq += dists[i]; - } - if (vec[i] > obj.root_bbox[i].high) { - dists[i] = obj.distance.accum_dist(vec[i], obj.root_bbox[i].high, i); - distsq += dists[i]; - } - } - return distsq; - } - - void save_tree(Derived &obj, FILE *stream, NodePtr tree) { - save_value(stream, *tree); - if (tree->child1 != NULL) { - save_tree(obj, stream, tree->child1); - } - if (tree->child2 != NULL) { - save_tree(obj, stream, tree->child2); - } - } - - void load_tree(Derived &obj, FILE *stream, NodePtr &tree) { - tree = obj.pool.template allocate(); - load_value(stream, *tree); - if (tree->child1 != NULL) { - load_tree(obj, stream, tree->child1); - } - if (tree->child2 != NULL) { - load_tree(obj, stream, tree->child2); - } - } - - /** Stores the index in a binary file. - * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when - * loading the index object it must be constructed associated to the same - * source of data points used while building it. See the example: - * examples/saveload_example.cpp \sa loadIndex */ - void saveIndex_(Derived &obj, FILE *stream) { - save_value(stream, obj.m_size); - save_value(stream, obj.dim); - save_value(stream, obj.root_bbox); - save_value(stream, obj.m_leaf_max_size); - save_value(stream, obj.vind); - save_tree(obj, stream, obj.root_node); - } - - /** Loads a previous index from a binary file. - * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the - * index object must be constructed associated to the same source of data - * points used while building the index. See the example: - * examples/saveload_example.cpp \sa loadIndex */ - void loadIndex_(Derived &obj, FILE *stream) { - load_value(stream, obj.m_size); - load_value(stream, obj.dim); - load_value(stream, obj.root_bbox); - load_value(stream, obj.m_leaf_max_size); - load_value(stream, obj.vind); - load_tree(obj, stream, obj.root_node); - } + void planeSplit( + const Derived& obj, const Offset ind, const Size count, + const Dimension cutfeat, const DistanceType& cutval, Offset& lim1, + Offset& lim2) + { + /* First pass. + * Determine lim1 with all values less than cutval to the left. + */ + Offset left = 0; + Offset right = count - 1; + for (;;) + { + while (left <= right && + dataset_get(obj, vAcc_[ind + left], cutfeat) < cutval) + ++left; + while (right && left <= right && + dataset_get(obj, vAcc_[ind + right], cutfeat) >= cutval) + --right; + if (left > right || !right) + break; // "!right" was added to support unsigned Index types + std::swap(vAcc_[ind + left], vAcc_[ind + right]); + ++left; + --right; + } + /* Second pass + * Determine lim2 with all values greater than cutval to the right + * The middle is used for balancing the tree + */ + lim1 = left; + right = count - 1; + for (;;) + { + while (left <= right && + dataset_get(obj, vAcc_[ind + left], cutfeat) <= cutval) + ++left; + while (right && left <= right && + dataset_get(obj, vAcc_[ind + right], cutfeat) > cutval) + --right; + if (left > right || !right) + break; // "!right" was added to support unsigned Index types + std::swap(vAcc_[ind + left], vAcc_[ind + right]); + ++left; + --right; + } + lim2 = left; + } + + DistanceType computeInitialDistances( + const Derived& obj, const ElementType* vec, + distance_vector_t& dists) const + { + assert(vec); + DistanceType dist = DistanceType(); + + for (Dimension i = 0; i < (DIM > 0 ? DIM : obj.dim_); ++i) + { + if (vec[i] < obj.root_bbox_[i].low) + { + dists[i] = + obj.distance_.accum_dist(vec[i], obj.root_bbox_[i].low, i); + dist += dists[i]; + } + if (vec[i] > obj.root_bbox_[i].high) + { + dists[i] = + obj.distance_.accum_dist(vec[i], obj.root_bbox_[i].high, i); + dist += dists[i]; + } + } + return dist; + } + + static void save_tree( + const Derived& obj, std::ostream& stream, const NodeConstPtr tree) + { + save_value(stream, *tree); + if (tree->child1 != nullptr) { save_tree(obj, stream, tree->child1); } + if (tree->child2 != nullptr) { save_tree(obj, stream, tree->child2); } + } + + static void load_tree(Derived& obj, std::istream& stream, NodePtr& tree) + { + tree = obj.pool_.template allocate(); + load_value(stream, *tree); + if (tree->child1 != nullptr) { load_tree(obj, stream, tree->child1); } + if (tree->child2 != nullptr) { load_tree(obj, stream, tree->child2); } + } + + /** Stores the index in a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so + * when loading the index object it must be constructed associated to the + * same source of data points used while building it. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void saveIndex(const Derived& obj, std::ostream& stream) const + { + save_value(stream, obj.size_); + save_value(stream, obj.dim_); + save_value(stream, obj.root_bbox_); + save_value(stream, obj.leaf_max_size_); + save_value(stream, obj.vAcc_); + if (obj.root_node_) save_tree(obj, stream, obj.root_node_); + } + + /** Loads a previous index from a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so + * the index object must be constructed associated to the same source of + * data points used while building the index. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void loadIndex(Derived& obj, std::istream& stream) + { + load_value(stream, obj.size_); + load_value(stream, obj.dim_); + load_value(stream, obj.root_bbox_); + load_value(stream, obj.leaf_max_size_); + load_value(stream, obj.vAcc_); + load_tree(obj, stream, obj.root_node_); + } }; /** @addtogroup kdtrees_grp KD-tree classes and adaptors @@ -1085,11 +1511,11 @@ class KDTreeBaseClass { * * \code * // Must return the number of data poins - * inline size_t kdtree_get_point_count() const { ... } + * size_t kdtree_get_point_count() const { ... } * * * // Must return the dim'th component of the idx'th point in the class: - * inline T kdtree_get_pt(const size_t idx, const size_t dim) const { ... } + * T kdtree_get_pt(const size_t idx, const size_t dim) const { ... } * * // Optional bounding-box computation: return false to default to a standard * bbox computation loop. @@ -1106,340 +1532,467 @@ class KDTreeBaseClass { * * \endcode * - * \tparam DatasetAdaptor The user-provided adaptor (see comments above). + * \tparam DatasetAdaptor The user-provided adaptor, which must be ensured to + * have a lifetime equal or longer than the instance of this class. * \tparam Distance The distance metric to use: nanoflann::metric_L1, * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will * be typically size_t or int */ -template +template < + typename Distance, class DatasetAdaptor, int32_t DIM = -1, + typename index_t = uint32_t> class KDTreeSingleIndexAdaptor : public KDTreeBaseClass< - KDTreeSingleIndexAdaptor, - Distance, DatasetAdaptor, DIM, IndexType> { -public: - /** Deleted copy constructor*/ - KDTreeSingleIndexAdaptor( - const KDTreeSingleIndexAdaptor - &) = delete; - - /** - * The dataset used by this index - */ - const DatasetAdaptor &dataset; //!< The source of our data - - const KDTreeSingleIndexAdaptorParams index_params; - - Distance distance; - - typedef typename nanoflann::KDTreeBaseClass< - nanoflann::KDTreeSingleIndexAdaptor, - Distance, DatasetAdaptor, DIM, IndexType> - BaseClassRef; - - typedef typename BaseClassRef::ElementType ElementType; - typedef typename BaseClassRef::DistanceType DistanceType; - - typedef typename BaseClassRef::Node Node; - typedef Node *NodePtr; - - typedef typename BaseClassRef::Interval Interval; - /** Define "BoundingBox" as a fixed-size or variable-size container depending - * on "DIM" */ - typedef typename BaseClassRef::BoundingBox BoundingBox; - - /** Define "distance_vector_t" as a fixed-size or variable-size container - * depending on "DIM" */ - typedef typename BaseClassRef::distance_vector_t distance_vector_t; - - /** - * KDTree constructor - * - * Refer to docs in README.md or online in - * https://github.com/jlblancoc/nanoflann - * - * The KD-Tree point dimension (the length of each point in the datase, e.g. 3 - * for 3D points) is determined by means of: - * - The \a DIM template parameter if >0 (highest priority) - * - Otherwise, the \a dimensionality parameter of this constructor. - * - * @param inputData Dataset with the input features - * @param params Basically, the maximum leaf node size - */ - KDTreeSingleIndexAdaptor(const int dimensionality, - const DatasetAdaptor &inputData, - const KDTreeSingleIndexAdaptorParams ¶ms = - KDTreeSingleIndexAdaptorParams()) - : dataset(inputData), index_params(params), distance(inputData) { - BaseClassRef::root_node = NULL; - BaseClassRef::m_size = dataset.kdtree_get_point_count(); - BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; - BaseClassRef::dim = dimensionality; - if (DIM > 0) - BaseClassRef::dim = DIM; - BaseClassRef::m_leaf_max_size = params.leaf_max_size; - - // Create a permutable array of indices to the input vectors. - init_vind(); - } - - /** - * Builds the index - */ - void buildIndex() { - BaseClassRef::m_size = dataset.kdtree_get_point_count(); - BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; - init_vind(); - this->freeIndex(*this); - BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; - if (BaseClassRef::m_size == 0) - return; - computeBoundingBox(BaseClassRef::root_bbox); - BaseClassRef::root_node = - this->divideTree(*this, 0, BaseClassRef::m_size, - BaseClassRef::root_bbox); // construct the tree - } - - /** \name Query methods - * @{ */ - - /** - * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored - * inside the result object. - * - * Params: - * result = the result object in which the indices of the - * nearest-neighbors are stored vec = the vector for which to search the - * nearest neighbors - * - * \tparam RESULTSET Should be any ResultSet - * \return True if the requested neighbors could be found. - * \sa knnSearch, radiusSearch - */ - template - bool findNeighbors(RESULTSET &result, const ElementType *vec, - const SearchParams &searchParams) const { - assert(vec); - if (this->size(*this) == 0) - return false; - if (!BaseClassRef::root_node) - throw std::runtime_error( - "[nanoflann] findNeighbors() called before building the index."); - float epsError = 1 + searchParams.eps; - - distance_vector_t - dists; // fixed or variable-sized container (depending on DIM) - auto zero = static_cast(0); - assign(dists, (DIM > 0 ? DIM : BaseClassRef::dim), - zero); // Fill it with zeros. - DistanceType distsq = this->computeInitialDistances(*this, vec, dists); - searchLevel(result, vec, BaseClassRef::root_node, distsq, dists, - epsError); // "count_leaf" parameter removed since was neither - // used nor returned to the user. - return result.full(); - } - - /** - * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1]. - * Their indices are stored inside the result object. \sa radiusSearch, - * findNeighbors \note nChecks_IGNORED is ignored but kept for compatibility - * with the original FLANN interface. \return Number `N` of valid points in - * the result set. Only the first `N` entries in `out_indices` and - * `out_distances_sq` will be valid. Return may be less than `num_closest` - * only if the number of elements in the tree is less than `num_closest`. - */ - size_t knnSearch(const ElementType *query_point, const size_t num_closest, - IndexType *out_indices, DistanceType *out_distances_sq, - const int /* nChecks_IGNORED */ = 10) const { - nanoflann::KNNResultSet resultSet(num_closest); - resultSet.init(out_indices, out_distances_sq); - this->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); - return resultSet.size(); - } - - /** - * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius. - * The output is given as a vector of pairs, of which the first element is a - * point index and the second the corresponding distance. Previous contents of - * \a IndicesDists are cleared. - * - * If searchParams.sorted==true, the output list is sorted by ascending - * distances. - * - * For a better performance, it is advisable to do a .reserve() on the vector - * if you have any wild guess about the number of expected matches. - * - * \sa knnSearch, findNeighbors, radiusSearchCustomCallback - * \return The number of points within the given radius (i.e. indices.size() - * or dists.size() ) - */ - size_t - radiusSearch(const ElementType *query_point, const DistanceType &radius, - std::vector> &IndicesDists, - const SearchParams &searchParams) const { - RadiusResultSet resultSet(radius, IndicesDists); - const size_t nFound = - radiusSearchCustomCallback(query_point, resultSet, searchParams); - if (searchParams.sorted) - std::sort(IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter()); - return nFound; - } - - /** - * Just like radiusSearch() but with a custom callback class for each point - * found in the radius of the query. See the source of RadiusResultSet<> as a - * start point for your own classes. \sa radiusSearch - */ - template - size_t radiusSearchCustomCallback( - const ElementType *query_point, SEARCH_CALLBACK &resultSet, - const SearchParams &searchParams = SearchParams()) const { - this->findNeighbors(resultSet, query_point, searchParams); - return resultSet.size(); - } - - /** @} */ - -public: - /** Make sure the auxiliary list \a vind has the same size than the current - * dataset, and re-generate if size has changed. */ - void init_vind() { - // Create a permutable array of indices to the input vectors. - BaseClassRef::m_size = dataset.kdtree_get_point_count(); - if (BaseClassRef::vind.size() != BaseClassRef::m_size) - BaseClassRef::vind.resize(BaseClassRef::m_size); - for (size_t i = 0; i < BaseClassRef::m_size; i++) - BaseClassRef::vind[i] = i; - } - - void computeBoundingBox(BoundingBox &bbox) { - resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim)); - if (dataset.kdtree_get_bbox(bbox)) { - // Done! It was implemented in derived class - } else { - const size_t N = dataset.kdtree_get_point_count(); - if (!N) - throw std::runtime_error("[nanoflann] computeBoundingBox() called but " - "no data points found."); - for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { - bbox[i].low = bbox[i].high = this->dataset_get(*this, 0, i); - } - for (size_t k = 1; k < N; ++k) { - for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { - if (this->dataset_get(*this, k, i) < bbox[i].low) - bbox[i].low = this->dataset_get(*this, k, i); - if (this->dataset_get(*this, k, i) > bbox[i].high) - bbox[i].high = this->dataset_get(*this, k, i); + KDTreeSingleIndexAdaptor, + Distance, DatasetAdaptor, DIM, index_t> +{ + public: + /** Deleted copy constructor*/ + explicit KDTreeSingleIndexAdaptor( + const KDTreeSingleIndexAdaptor< + Distance, DatasetAdaptor, DIM, index_t>&) = delete; + + /** The data source used by this index */ + const DatasetAdaptor& dataset_; + + const KDTreeSingleIndexAdaptorParams indexParams; + + Distance distance_; + + using Base = typename nanoflann::KDTreeBaseClass< + nanoflann::KDTreeSingleIndexAdaptor< + Distance, DatasetAdaptor, DIM, index_t>, + Distance, DatasetAdaptor, DIM, index_t>; + + using Offset = typename Base::Offset; + using Size = typename Base::Size; + using Dimension = typename Base::Dimension; + + using ElementType = typename Base::ElementType; + using DistanceType = typename Base::DistanceType; + using IndexType = typename Base::IndexType; + + using Node = typename Base::Node; + using NodePtr = Node*; + + using Interval = typename Base::Interval; + + /** Define "BoundingBox" as a fixed-size or variable-size container + * depending on "DIM" */ + using BoundingBox = typename Base::BoundingBox; + + /** Define "distance_vector_t" as a fixed-size or variable-size container + * depending on "DIM" */ + using distance_vector_t = typename Base::distance_vector_t; + + /** + * KDTree constructor + * + * Refer to docs in README.md or online in + * https://github.com/jlblancoc/nanoflann + * + * The KD-Tree point dimension (the length of each point in the datase, e.g. + * 3 for 3D points) is determined by means of: + * - The \a DIM template parameter if >0 (highest priority) + * - Otherwise, the \a dimensionality parameter of this constructor. + * + * @param inputData Dataset with the input features. Its lifetime must be + * equal or longer than that of the instance of this class. + * @param params Basically, the maximum leaf node size + * + * Note that there is a variable number of optional additional parameters + * which will be forwarded to the metric class constructor. Refer to example + * `examples/pointcloud_custom_metric.cpp` for a use case. + * + */ + template + explicit KDTreeSingleIndexAdaptor( + const Dimension dimensionality, const DatasetAdaptor& inputData, + const KDTreeSingleIndexAdaptorParams& params, Args&&... args) + : dataset_(inputData), + indexParams(params), + distance_(inputData, std::forward(args)...) + { + init(dimensionality, params); + } + + explicit KDTreeSingleIndexAdaptor( + const Dimension dimensionality, const DatasetAdaptor& inputData, + const KDTreeSingleIndexAdaptorParams& params = {}) + : dataset_(inputData), indexParams(params), distance_(inputData) + { + init(dimensionality, params); + } + + private: + void init( + const Dimension dimensionality, + const KDTreeSingleIndexAdaptorParams& params) + { + Base::size_ = dataset_.kdtree_get_point_count(); + Base::size_at_index_build_ = Base::size_; + Base::dim_ = dimensionality; + if (DIM > 0) Base::dim_ = DIM; + Base::leaf_max_size_ = params.leaf_max_size; + if (params.n_thread_build > 0) + { + Base::n_thread_build_ = params.n_thread_build; + } + else + { + Base::n_thread_build_ = + std::max(std::thread::hardware_concurrency(), 1u); + } + + if (!(params.flags & + KDTreeSingleIndexAdaptorFlags::SkipInitialBuildIndex)) + { + // Build KD-tree: + buildIndex(); + } + } + + public: + /** + * Builds the index + */ + void buildIndex() + { + Base::size_ = dataset_.kdtree_get_point_count(); + Base::size_at_index_build_ = Base::size_; + init_vind(); + this->freeIndex(*this); + Base::size_at_index_build_ = Base::size_; + if (Base::size_ == 0) return; + computeBoundingBox(Base::root_bbox_); + // construct the tree + if (Base::n_thread_build_ == 1) + { + Base::root_node_ = + this->divideTree(*this, 0, Base::size_, Base::root_bbox_); + } + else + { +#ifndef NANOFLANN_NO_THREADS + std::atomic thread_count(0u); + std::mutex mutex; + Base::root_node_ = this->divideTreeConcurrent( + *this, 0, Base::size_, Base::root_bbox_, thread_count, mutex); +#else /* NANOFLANN_NO_THREADS */ + throw std::runtime_error("Multithreading is disabled"); +#endif /* NANOFLANN_NO_THREADS */ + } + } + + /** \name Query methods + * @{ */ + + /** + * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored + * inside the result object. + * + * Params: + * result = the result object in which the indices of the + * nearest-neighbors are stored vec = the vector for which to search the + * nearest neighbors + * + * \tparam RESULTSET Should be any ResultSet + * \return True if the requested neighbors could be found. + * \sa knnSearch, radiusSearch + * + * \note If L2 norms are used, all returned distances are actually squared + * distances. + */ + template + bool findNeighbors( + RESULTSET& result, const ElementType* vec, + const SearchParameters& searchParams = {}) const + { + assert(vec); + if (this->size(*this) == 0) return false; + if (!Base::root_node_) + throw std::runtime_error( + "[nanoflann] findNeighbors() called before building the " + "index."); + float epsError = 1 + searchParams.eps; + + // fixed or variable-sized container (depending on DIM) + distance_vector_t dists; + // Fill it with zeros. + auto zero = static_cast(0); + assign(dists, (DIM > 0 ? DIM : Base::dim_), zero); + DistanceType dist = this->computeInitialDistances(*this, vec, dists); + searchLevel(result, vec, Base::root_node_, dist, dists, epsError); + + if (searchParams.sorted) result.sort(); + + return result.full(); + } + + /** + * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1]. + * Their indices and distances are stored in the provided pointers to + * array/vector. + * + * \sa radiusSearch, findNeighbors + * \return Number `N` of valid points in the result set. + * + * \note If L2 norms are used, all returned distances are actually squared + * distances. + * + * \note Only the first `N` entries in `out_indices` and `out_distances` + * will be valid. Return is less than `num_closest` only if the + * number of elements in the tree is less than `num_closest`. + */ + Size knnSearch( + const ElementType* query_point, const Size num_closest, + IndexType* out_indices, DistanceType* out_distances) const + { + nanoflann::KNNResultSet resultSet(num_closest); + resultSet.init(out_indices, out_distances); + findNeighbors(resultSet, query_point); + return resultSet.size(); + } + + /** + * Find all the neighbors to \a query_point[0:dim-1] within a maximum + * radius. The output is given as a vector of pairs, of which the first + * element is a point index and the second the corresponding distance. + * Previous contents of \a IndicesDists are cleared. + * + * If searchParams.sorted==true, the output list is sorted by ascending + * distances. + * + * For a better performance, it is advisable to do a .reserve() on the + * vector if you have any wild guess about the number of expected matches. + * + * \sa knnSearch, findNeighbors, radiusSearchCustomCallback + * \return The number of points within the given radius (i.e. indices.size() + * or dists.size() ) + * + * \note If L2 norms are used, search radius and all returned distances + * are actually squared distances. + */ + Size radiusSearch( + const ElementType* query_point, const DistanceType& radius, + std::vector>& IndicesDists, + const SearchParameters& searchParams = {}) const + { + RadiusResultSet resultSet( + radius, IndicesDists); + const Size nFound = + radiusSearchCustomCallback(query_point, resultSet, searchParams); + return nFound; + } + + /** + * Just like radiusSearch() but with a custom callback class for each point + * found in the radius of the query. See the source of RadiusResultSet<> as + * a start point for your own classes. \sa radiusSearch + */ + template + Size radiusSearchCustomCallback( + const ElementType* query_point, SEARCH_CALLBACK& resultSet, + const SearchParameters& searchParams = {}) const + { + findNeighbors(resultSet, query_point, searchParams); + return resultSet.size(); + } + + /** + * Find the first N neighbors to \a query_point[0:dim-1] within a maximum + * radius. The output is given as a vector of pairs, of which the first + * element is a point index and the second the corresponding distance. + * Previous contents of \a IndicesDists are cleared. + * + * \sa radiusSearch, findNeighbors + * \return Number `N` of valid points in the result set. + * + * \note If L2 norms are used, all returned distances are actually squared + * distances. + * + * \note Only the first `N` entries in `out_indices` and `out_distances` + * will be valid. Return is less than `num_closest` only if the + * number of elements in the tree is less than `num_closest`. + */ + Size rknnSearch( + const ElementType* query_point, const Size num_closest, + IndexType* out_indices, DistanceType* out_distances, + const DistanceType& radius) const + { + nanoflann::RKNNResultSet resultSet( + num_closest, radius); + resultSet.init(out_indices, out_distances); + findNeighbors(resultSet, query_point); + return resultSet.size(); + } + + /** @} */ + + public: + /** Make sure the auxiliary list \a vind has the same size than the current + * dataset, and re-generate if size has changed. */ + void init_vind() + { + // Create a permutable array of indices to the input vectors. + Base::size_ = dataset_.kdtree_get_point_count(); + if (Base::vAcc_.size() != Base::size_) Base::vAcc_.resize(Base::size_); + for (IndexType i = 0; i < static_cast(Base::size_); i++) + Base::vAcc_[i] = i; + } + + void computeBoundingBox(BoundingBox& bbox) + { + const auto dims = (DIM > 0 ? DIM : Base::dim_); + resize(bbox, dims); + if (dataset_.kdtree_get_bbox(bbox)) + { + // Done! It was implemented in derived class + } + else + { + const Size N = dataset_.kdtree_get_point_count(); + if (!N) + throw std::runtime_error( + "[nanoflann] computeBoundingBox() called but " + "no data points found."); + for (Dimension i = 0; i < dims; ++i) + { + bbox[i].low = bbox[i].high = + this->dataset_get(*this, Base::vAcc_[0], i); + } + for (Offset k = 1; k < N; ++k) + { + for (Dimension i = 0; i < dims; ++i) + { + const auto val = + this->dataset_get(*this, Base::vAcc_[k], i); + if (val < bbox[i].low) bbox[i].low = val; + if (val > bbox[i].high) bbox[i].high = val; + } + } } - } - } - } - - /** - * Performs an exact search in the tree starting from a node. - * \tparam RESULTSET Should be any ResultSet - * \return true if the search should be continued, false if the results are - * sufficient - */ - template - bool searchLevel(RESULTSET &result_set, const ElementType *vec, - const NodePtr node, DistanceType mindistsq, - distance_vector_t &dists, const float epsError) const { - /* If this is a leaf node, then do check and return. */ - if ((node->child1 == NULL) && (node->child2 == NULL)) { - // count_leaf += (node->lr.right-node->lr.left); // Removed since was - // neither used nor returned to the user. - DistanceType worst_dist = result_set.worstDist(); - for (IndexType i = node->node_type.lr.left; i < node->node_type.lr.right; - ++i) { - const IndexType index = BaseClassRef::vind[i]; // reorder... : i; - DistanceType dist = distance.evalMetric( - vec, index, (DIM > 0 ? DIM : BaseClassRef::dim)); - if (dist < worst_dist) { - if (!result_set.addPoint(dist, BaseClassRef::vind[i])) { + } + + /** + * Performs an exact search in the tree starting from a node. + * \tparam RESULTSET Should be any ResultSet + * \return true if the search should be continued, false if the results are + * sufficient + */ + template + bool searchLevel( + RESULTSET& result_set, const ElementType* vec, const NodePtr node, + DistanceType mindist, distance_vector_t& dists, + const float epsError) const + { + /* If this is a leaf node, then do check and return. */ + if ((node->child1 == nullptr) && (node->child2 == nullptr)) + { + DistanceType worst_dist = result_set.worstDist(); + for (Offset i = node->node_type.lr.left; + i < node->node_type.lr.right; ++i) + { + const IndexType accessor = Base::vAcc_[i]; // reorder... : i; + DistanceType dist = distance_.evalMetric( + vec, accessor, (DIM > 0 ? DIM : Base::dim_)); + if (dist < worst_dist) + { + if (!result_set.addPoint(dist, Base::vAcc_[i])) + { + // the resultset doesn't want to receive any more + // points, we're done searching! + return false; + } + } + } + return true; + } + + /* Which child branch should be taken first? */ + Dimension idx = node->node_type.sub.divfeat; + ElementType val = vec[idx]; + DistanceType diff1 = val - node->node_type.sub.divlow; + DistanceType diff2 = val - node->node_type.sub.divhigh; + + NodePtr bestChild; + NodePtr otherChild; + DistanceType cut_dist; + if ((diff1 + diff2) < 0) + { + bestChild = node->child1; + otherChild = node->child2; + cut_dist = + distance_.accum_dist(val, node->node_type.sub.divhigh, idx); + } + else + { + bestChild = node->child2; + otherChild = node->child1; + cut_dist = + distance_.accum_dist(val, node->node_type.sub.divlow, idx); + } + + /* Call recursively to search next level down. */ + if (!searchLevel(result_set, vec, bestChild, mindist, dists, epsError)) + { // the resultset doesn't want to receive any more points, we're done // searching! return false; - } } - } - return true; - } - - /* Which child branch should be taken first? */ - int idx = node->node_type.sub.divfeat; - ElementType val = vec[idx]; - DistanceType diff1 = val - node->node_type.sub.divlow; - DistanceType diff2 = val - node->node_type.sub.divhigh; - - NodePtr bestChild; - NodePtr otherChild; - DistanceType cut_dist; - if ((diff1 + diff2) < 0) { - bestChild = node->child1; - otherChild = node->child2; - cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx); - } else { - bestChild = node->child2; - otherChild = node->child1; - cut_dist = distance.accum_dist(val, node->node_type.sub.divlow, idx); - } - - /* Call recursively to search next level down. */ - if (!searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError)) { - // the resultset doesn't want to receive any more points, we're done - // searching! - return false; - } - - DistanceType dst = dists[idx]; - mindistsq = mindistsq + cut_dist - dst; - dists[idx] = cut_dist; - if (mindistsq * epsError <= result_set.worstDist()) { - if (!searchLevel(result_set, vec, otherChild, mindistsq, dists, - epsError)) { - // the resultset doesn't want to receive any more points, we're done - // searching! - return false; - } + + DistanceType dst = dists[idx]; + mindist = mindist + cut_dist - dst; + dists[idx] = cut_dist; + if (mindist * epsError <= result_set.worstDist()) + { + if (!searchLevel( + result_set, vec, otherChild, mindist, dists, epsError)) + { + // the resultset doesn't want to receive any more points, we're + // done searching! + return false; + } + } + dists[idx] = dst; + return true; } - dists[idx] = dst; - return true; - } -public: - /** Stores the index in a binary file. - * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when - * loading the index object it must be constructed associated to the same - * source of data points used while building it. See the example: - * examples/saveload_example.cpp \sa loadIndex */ - void saveIndex(FILE *stream) { this->saveIndex_(*this, stream); } + public: + /** Stores the index in a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so + * when loading the index object it must be constructed associated to the + * same source of data points used while building it. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void saveIndex(std::ostream& stream) const + { + Base::saveIndex(*this, stream); + } - /** Loads a previous index from a binary file. - * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the - * index object must be constructed associated to the same source of data - * points used while building the index. See the example: - * examples/saveload_example.cpp \sa loadIndex */ - void loadIndex(FILE *stream) { this->loadIndex_(*this, stream); } + /** Loads a previous index from a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so + * the index object must be constructed associated to the same source of + * data points used while building the index. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void loadIndex(std::istream& stream) { Base::loadIndex(*this, stream); } -}; // class KDTree +}; // class KDTree /** kd-tree dynamic index * * Contains the k-d trees and other information for indexing a set of points * for nearest-neighbor matching. * - * The class "DatasetAdaptor" must provide the following interface (can be + * The class "DatasetAdaptor" must provide the following interface (can be * non-virtual, inlined methods): * * \code * // Must return the number of data poins - * inline size_t kdtree_get_point_count() const { ... } + * size_t kdtree_get_point_count() const { ... } * * // Must return the dim'th component of the idx'th point in the class: - * inline T kdtree_get_pt(const size_t idx, const size_t dim) const { ... } + * T kdtree_get_pt(const size_t idx, const size_t dim) const { ... } * * // Optional bounding-box computation: return false to default to a standard * bbox computation loop. @@ -1458,315 +2011,387 @@ class KDTreeSingleIndexAdaptor * * \tparam DatasetAdaptor The user-provided adaptor (see comments above). * \tparam Distance The distance metric to use: nanoflann::metric_L1, - * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM - * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will - * be typically size_t or int + * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. + * \tparam DIM Dimensionality of data points (e.g. 3 for 3D points) + * \tparam IndexType Type of the arguments with which the data can be + * accessed (e.g. float, double, int64_t, T*) */ -template +template < + typename Distance, class DatasetAdaptor, int32_t DIM = -1, + typename IndexType = uint32_t> class KDTreeSingleIndexDynamicAdaptor_ - : public KDTreeBaseClass, - Distance, DatasetAdaptor, DIM, IndexType> { -public: - /** - * The dataset used by this index - */ - const DatasetAdaptor &dataset; //!< The source of our data - - KDTreeSingleIndexAdaptorParams index_params; - - std::vector &treeIndex; - - Distance distance; - - typedef typename nanoflann::KDTreeBaseClass< - nanoflann::KDTreeSingleIndexDynamicAdaptor_, - Distance, DatasetAdaptor, DIM, IndexType> - BaseClassRef; - - typedef typename BaseClassRef::ElementType ElementType; - typedef typename BaseClassRef::DistanceType DistanceType; - - typedef typename BaseClassRef::Node Node; - typedef Node *NodePtr; - - typedef typename BaseClassRef::Interval Interval; - /** Define "BoundingBox" as a fixed-size or variable-size container depending - * on "DIM" */ - typedef typename BaseClassRef::BoundingBox BoundingBox; - - /** Define "distance_vector_t" as a fixed-size or variable-size container - * depending on "DIM" */ - typedef typename BaseClassRef::distance_vector_t distance_vector_t; - - /** - * KDTree constructor - * - * Refer to docs in README.md or online in - * https://github.com/jlblancoc/nanoflann - * - * The KD-Tree point dimension (the length of each point in the datase, e.g. 3 - * for 3D points) is determined by means of: - * - The \a DIM template parameter if >0 (highest priority) - * - Otherwise, the \a dimensionality parameter of this constructor. - * - * @param inputData Dataset with the input features - * @param params Basically, the maximum leaf node size - */ - KDTreeSingleIndexDynamicAdaptor_( - const int dimensionality, const DatasetAdaptor &inputData, - std::vector &treeIndex_, - const KDTreeSingleIndexAdaptorParams ¶ms = - KDTreeSingleIndexAdaptorParams()) - : dataset(inputData), index_params(params), treeIndex(treeIndex_), - distance(inputData) { - BaseClassRef::root_node = NULL; - BaseClassRef::m_size = 0; - BaseClassRef::m_size_at_index_build = 0; - BaseClassRef::dim = dimensionality; - if (DIM > 0) - BaseClassRef::dim = DIM; - BaseClassRef::m_leaf_max_size = params.leaf_max_size; - } - - /** Assignment operator definiton */ - KDTreeSingleIndexDynamicAdaptor_ - operator=(const KDTreeSingleIndexDynamicAdaptor_ &rhs) { - KDTreeSingleIndexDynamicAdaptor_ tmp(rhs); - std::swap(BaseClassRef::vind, tmp.BaseClassRef::vind); - std::swap(BaseClassRef::m_leaf_max_size, tmp.BaseClassRef::m_leaf_max_size); - std::swap(index_params, tmp.index_params); - std::swap(treeIndex, tmp.treeIndex); - std::swap(BaseClassRef::m_size, tmp.BaseClassRef::m_size); - std::swap(BaseClassRef::m_size_at_index_build, - tmp.BaseClassRef::m_size_at_index_build); - std::swap(BaseClassRef::root_node, tmp.BaseClassRef::root_node); - std::swap(BaseClassRef::root_bbox, tmp.BaseClassRef::root_bbox); - std::swap(BaseClassRef::pool, tmp.BaseClassRef::pool); - return *this; - } - - /** - * Builds the index - */ - void buildIndex() { - BaseClassRef::m_size = BaseClassRef::vind.size(); - this->freeIndex(*this); - BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; - if (BaseClassRef::m_size == 0) - return; - computeBoundingBox(BaseClassRef::root_bbox); - BaseClassRef::root_node = - this->divideTree(*this, 0, BaseClassRef::m_size, - BaseClassRef::root_bbox); // construct the tree - } - - /** \name Query methods - * @{ */ - - /** - * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored - * inside the result object. - * - * Params: - * result = the result object in which the indices of the - * nearest-neighbors are stored vec = the vector for which to search the - * nearest neighbors - * - * \tparam RESULTSET Should be any ResultSet - * \return True if the requested neighbors could be found. - * \sa knnSearch, radiusSearch - */ - template - bool findNeighbors(RESULTSET &result, const ElementType *vec, - const SearchParams &searchParams) const { - assert(vec); - if (this->size(*this) == 0) - return false; - if (!BaseClassRef::root_node) - return false; - float epsError = 1 + searchParams.eps; - - // fixed or variable-sized container (depending on DIM) - distance_vector_t dists; - // Fill it with zeros. - assign(dists, (DIM > 0 ? DIM : BaseClassRef::dim), - static_cast(0)); - DistanceType distsq = this->computeInitialDistances(*this, vec, dists); - searchLevel(result, vec, BaseClassRef::root_node, distsq, dists, - epsError); // "count_leaf" parameter removed since was neither - // used nor returned to the user. - return result.full(); - } - - /** - * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1]. - * Their indices are stored inside the result object. \sa radiusSearch, - * findNeighbors \note nChecks_IGNORED is ignored but kept for compatibility - * with the original FLANN interface. \return Number `N` of valid points in - * the result set. Only the first `N` entries in `out_indices` and - * `out_distances_sq` will be valid. Return may be less than `num_closest` - * only if the number of elements in the tree is less than `num_closest`. - */ - size_t knnSearch(const ElementType *query_point, const size_t num_closest, - IndexType *out_indices, DistanceType *out_distances_sq, - const int /* nChecks_IGNORED */ = 10) const { - nanoflann::KNNResultSet resultSet(num_closest); - resultSet.init(out_indices, out_distances_sq); - this->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); - return resultSet.size(); - } - - /** - * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius. - * The output is given as a vector of pairs, of which the first element is a - * point index and the second the corresponding distance. Previous contents of - * \a IndicesDists are cleared. - * - * If searchParams.sorted==true, the output list is sorted by ascending - * distances. - * - * For a better performance, it is advisable to do a .reserve() on the vector - * if you have any wild guess about the number of expected matches. - * - * \sa knnSearch, findNeighbors, radiusSearchCustomCallback - * \return The number of points within the given radius (i.e. indices.size() - * or dists.size() ) - */ - size_t - radiusSearch(const ElementType *query_point, const DistanceType &radius, - std::vector> &IndicesDists, - const SearchParams &searchParams) const { - RadiusResultSet resultSet(radius, IndicesDists); - const size_t nFound = - radiusSearchCustomCallback(query_point, resultSet, searchParams); - if (searchParams.sorted) - std::sort(IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter()); - return nFound; - } - - /** - * Just like radiusSearch() but with a custom callback class for each point - * found in the radius of the query. See the source of RadiusResultSet<> as a - * start point for your own classes. \sa radiusSearch - */ - template - size_t radiusSearchCustomCallback( - const ElementType *query_point, SEARCH_CALLBACK &resultSet, - const SearchParams &searchParams = SearchParams()) const { - this->findNeighbors(resultSet, query_point, searchParams); - return resultSet.size(); - } - - /** @} */ - -public: - void computeBoundingBox(BoundingBox &bbox) { - resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim)); - - if (dataset.kdtree_get_bbox(bbox)) { - // Done! It was implemented in derived class - } else { - const size_t N = BaseClassRef::m_size; - if (!N) - throw std::runtime_error("[nanoflann] computeBoundingBox() called but " - "no data points found."); - for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { - bbox[i].low = bbox[i].high = - this->dataset_get(*this, BaseClassRef::vind[0], i); - } - for (size_t k = 1; k < N; ++k) { - for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { - if (this->dataset_get(*this, BaseClassRef::vind[k], i) < bbox[i].low) - bbox[i].low = this->dataset_get(*this, BaseClassRef::vind[k], i); - if (this->dataset_get(*this, BaseClassRef::vind[k], i) > bbox[i].high) - bbox[i].high = this->dataset_get(*this, BaseClassRef::vind[k], i); + : public KDTreeBaseClass< + KDTreeSingleIndexDynamicAdaptor_< + Distance, DatasetAdaptor, DIM, IndexType>, + Distance, DatasetAdaptor, DIM, IndexType> +{ + public: + /** + * The dataset used by this index + */ + const DatasetAdaptor& dataset_; //!< The source of our data + + KDTreeSingleIndexAdaptorParams index_params_; + + std::vector& treeIndex_; + + Distance distance_; + + using Base = typename nanoflann::KDTreeBaseClass< + nanoflann::KDTreeSingleIndexDynamicAdaptor_< + Distance, DatasetAdaptor, DIM, IndexType>, + Distance, DatasetAdaptor, DIM, IndexType>; + + using ElementType = typename Base::ElementType; + using DistanceType = typename Base::DistanceType; + + using Offset = typename Base::Offset; + using Size = typename Base::Size; + using Dimension = typename Base::Dimension; + + using Node = typename Base::Node; + using NodePtr = Node*; + + using Interval = typename Base::Interval; + /** Define "BoundingBox" as a fixed-size or variable-size container + * depending on "DIM" */ + using BoundingBox = typename Base::BoundingBox; + + /** Define "distance_vector_t" as a fixed-size or variable-size container + * depending on "DIM" */ + using distance_vector_t = typename Base::distance_vector_t; + + /** + * KDTree constructor + * + * Refer to docs in README.md or online in + * https://github.com/jlblancoc/nanoflann + * + * The KD-Tree point dimension (the length of each point in the datase, e.g. + * 3 for 3D points) is determined by means of: + * - The \a DIM template parameter if >0 (highest priority) + * - Otherwise, the \a dimensionality parameter of this constructor. + * + * @param inputData Dataset with the input features. Its lifetime must be + * equal or longer than that of the instance of this class. + * @param params Basically, the maximum leaf node size + */ + KDTreeSingleIndexDynamicAdaptor_( + const Dimension dimensionality, const DatasetAdaptor& inputData, + std::vector& treeIndex, + const KDTreeSingleIndexAdaptorParams& params = + KDTreeSingleIndexAdaptorParams()) + : dataset_(inputData), + index_params_(params), + treeIndex_(treeIndex), + distance_(inputData) + { + Base::size_ = 0; + Base::size_at_index_build_ = 0; + for (auto& v : Base::root_bbox_) v = {}; + Base::dim_ = dimensionality; + if (DIM > 0) Base::dim_ = DIM; + Base::leaf_max_size_ = params.leaf_max_size; + if (params.n_thread_build > 0) + { + Base::n_thread_build_ = params.n_thread_build; } - } - } - } - - /** - * Performs an exact search in the tree starting from a node. - * \tparam RESULTSET Should be any ResultSet - */ - template - void searchLevel(RESULTSET &result_set, const ElementType *vec, - const NodePtr node, DistanceType mindistsq, - distance_vector_t &dists, const float epsError) const { - /* If this is a leaf node, then do check and return. */ - if ((node->child1 == NULL) && (node->child2 == NULL)) { - // count_leaf += (node->lr.right-node->lr.left); // Removed since was - // neither used nor returned to the user. - DistanceType worst_dist = result_set.worstDist(); - for (IndexType i = node->node_type.lr.left; i < node->node_type.lr.right; - ++i) { - const IndexType index = BaseClassRef::vind[i]; // reorder... : i; - if (treeIndex[index] == -1) - continue; - DistanceType dist = distance.evalMetric( - vec, index, (DIM > 0 ? DIM : BaseClassRef::dim)); - if (dist < worst_dist) { - if (!result_set.addPoint( - static_cast(dist), - static_cast( - BaseClassRef::vind[i]))) { - // the resultset doesn't want to receive any more points, we're done - // searching! - return; // false; - } + else + { + Base::n_thread_build_ = + std::max(std::thread::hardware_concurrency(), 1u); + } + } + + /** Explicitly default the copy constructor */ + KDTreeSingleIndexDynamicAdaptor_( + const KDTreeSingleIndexDynamicAdaptor_& rhs) = default; + + /** Assignment operator definiton */ + KDTreeSingleIndexDynamicAdaptor_ operator=( + const KDTreeSingleIndexDynamicAdaptor_& rhs) + { + KDTreeSingleIndexDynamicAdaptor_ tmp(rhs); + std::swap(Base::vAcc_, tmp.Base::vAcc_); + std::swap(Base::leaf_max_size_, tmp.Base::leaf_max_size_); + std::swap(index_params_, tmp.index_params_); + std::swap(treeIndex_, tmp.treeIndex_); + std::swap(Base::size_, tmp.Base::size_); + std::swap(Base::size_at_index_build_, tmp.Base::size_at_index_build_); + std::swap(Base::root_node_, tmp.Base::root_node_); + std::swap(Base::root_bbox_, tmp.Base::root_bbox_); + std::swap(Base::pool_, tmp.Base::pool_); + return *this; + } + + /** + * Builds the index + */ + void buildIndex() + { + Base::size_ = Base::vAcc_.size(); + this->freeIndex(*this); + Base::size_at_index_build_ = Base::size_; + if (Base::size_ == 0) return; + computeBoundingBox(Base::root_bbox_); + // construct the tree + if (Base::n_thread_build_ == 1) + { + Base::root_node_ = + this->divideTree(*this, 0, Base::size_, Base::root_bbox_); + } + else + { +#ifndef NANOFLANN_NO_THREADS + std::atomic thread_count(0u); + std::mutex mutex; + Base::root_node_ = this->divideTreeConcurrent( + *this, 0, Base::size_, Base::root_bbox_, thread_count, mutex); +#else /* NANOFLANN_NO_THREADS */ + throw std::runtime_error("Multithreading is disabled"); +#endif /* NANOFLANN_NO_THREADS */ + } + } + + /** \name Query methods + * @{ */ + + /** + * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored + * inside the result object. + * This is the core search function, all others are wrappers around this + * one. + * + * \param result The result object in which the indices of the + * nearest-neighbors are stored. + * \param vec The vector of the query point for which to search the + * nearest neighbors. + * \param searchParams Optional parameters for the search. + * + * \tparam RESULTSET Should be any ResultSet + * \return True if the requested neighbors could be found. + * + * \sa knnSearch(), radiusSearch(), radiusSearchCustomCallback() + * + * \note If L2 norms are used, all returned distances are actually squared + * distances. + */ + template + bool findNeighbors( + RESULTSET& result, const ElementType* vec, + const SearchParameters& searchParams = {}) const + { + assert(vec); + if (this->size(*this) == 0) return false; + if (!Base::root_node_) return false; + float epsError = 1 + searchParams.eps; + + // fixed or variable-sized container (depending on DIM) + distance_vector_t dists; + // Fill it with zeros. + assign( + dists, (DIM > 0 ? DIM : Base::dim_), + static_cast(0)); + DistanceType dist = this->computeInitialDistances(*this, vec, dists); + searchLevel(result, vec, Base::root_node_, dist, dists, epsError); + return result.full(); + } + + /** + * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1]. + * Their indices are stored inside the result object. \sa radiusSearch, + * findNeighbors + * \return Number `N` of valid points in + * the result set. + * + * \note If L2 norms are used, all returned distances are actually squared + * distances. + * + * \note Only the first `N` entries in `out_indices` and `out_distances` + * will be valid. Return may be less than `num_closest` only if the + * number of elements in the tree is less than `num_closest`. + */ + Size knnSearch( + const ElementType* query_point, const Size num_closest, + IndexType* out_indices, DistanceType* out_distances, + const SearchParameters& searchParams = {}) const + { + nanoflann::KNNResultSet resultSet(num_closest); + resultSet.init(out_indices, out_distances); + findNeighbors(resultSet, query_point, searchParams); + return resultSet.size(); + } + + /** + * Find all the neighbors to \a query_point[0:dim-1] within a maximum + * radius. The output is given as a vector of pairs, of which the first + * element is a point index and the second the corresponding distance. + * Previous contents of \a IndicesDists are cleared. + * + * If searchParams.sorted==true, the output list is sorted by ascending + * distances. + * + * For a better performance, it is advisable to do a .reserve() on the + * vector if you have any wild guess about the number of expected matches. + * + * \sa knnSearch, findNeighbors, radiusSearchCustomCallback + * \return The number of points within the given radius (i.e. indices.size() + * or dists.size() ) + * + * \note If L2 norms are used, search radius and all returned distances + * are actually squared distances. + */ + Size radiusSearch( + const ElementType* query_point, const DistanceType& radius, + std::vector>& IndicesDists, + const SearchParameters& searchParams = {}) const + { + RadiusResultSet resultSet( + radius, IndicesDists); + const size_t nFound = + radiusSearchCustomCallback(query_point, resultSet, searchParams); + return nFound; + } + + /** + * Just like radiusSearch() but with a custom callback class for each point + * found in the radius of the query. See the source of RadiusResultSet<> as + * a start point for your own classes. \sa radiusSearch + */ + template + Size radiusSearchCustomCallback( + const ElementType* query_point, SEARCH_CALLBACK& resultSet, + const SearchParameters& searchParams = {}) const + { + findNeighbors(resultSet, query_point, searchParams); + return resultSet.size(); + } + + /** @} */ + + public: + void computeBoundingBox(BoundingBox& bbox) + { + const auto dims = (DIM > 0 ? DIM : Base::dim_); + resize(bbox, dims); + + if (dataset_.kdtree_get_bbox(bbox)) + { + // Done! It was implemented in derived class + } + else + { + const Size N = Base::size_; + if (!N) + throw std::runtime_error( + "[nanoflann] computeBoundingBox() called but " + "no data points found."); + for (Dimension i = 0; i < dims; ++i) + { + bbox[i].low = bbox[i].high = + this->dataset_get(*this, Base::vAcc_[0], i); + } + for (Offset k = 1; k < N; ++k) + { + for (Dimension i = 0; i < dims; ++i) + { + const auto val = + this->dataset_get(*this, Base::vAcc_[k], i); + if (val < bbox[i].low) bbox[i].low = val; + if (val > bbox[i].high) bbox[i].high = val; + } + } + } + } + + /** + * Performs an exact search in the tree starting from a node. + * \tparam RESULTSET Should be any ResultSet + */ + template + void searchLevel( + RESULTSET& result_set, const ElementType* vec, const NodePtr node, + DistanceType mindist, distance_vector_t& dists, + const float epsError) const + { + /* If this is a leaf node, then do check and return. */ + if ((node->child1 == nullptr) && (node->child2 == nullptr)) + { + DistanceType worst_dist = result_set.worstDist(); + for (Offset i = node->node_type.lr.left; + i < node->node_type.lr.right; ++i) + { + const IndexType index = Base::vAcc_[i]; // reorder... : i; + if (treeIndex_[index] == -1) continue; + DistanceType dist = distance_.evalMetric( + vec, index, (DIM > 0 ? DIM : Base::dim_)); + if (dist < worst_dist) + { + if (!result_set.addPoint( + static_cast(dist), + static_cast( + Base::vAcc_[i]))) + { + // the resultset doesn't want to receive any more + // points, we're done searching! + return; // false; + } + } + } + return; + } + + /* Which child branch should be taken first? */ + Dimension idx = node->node_type.sub.divfeat; + ElementType val = vec[idx]; + DistanceType diff1 = val - node->node_type.sub.divlow; + DistanceType diff2 = val - node->node_type.sub.divhigh; + + NodePtr bestChild; + NodePtr otherChild; + DistanceType cut_dist; + if ((diff1 + diff2) < 0) + { + bestChild = node->child1; + otherChild = node->child2; + cut_dist = + distance_.accum_dist(val, node->node_type.sub.divhigh, idx); + } + else + { + bestChild = node->child2; + otherChild = node->child1; + cut_dist = + distance_.accum_dist(val, node->node_type.sub.divlow, idx); + } + + /* Call recursively to search next level down. */ + searchLevel(result_set, vec, bestChild, mindist, dists, epsError); + + DistanceType dst = dists[idx]; + mindist = mindist + cut_dist - dst; + dists[idx] = cut_dist; + if (mindist * epsError <= result_set.worstDist()) + { + searchLevel(result_set, vec, otherChild, mindist, dists, epsError); } - } - return; - } - - /* Which child branch should be taken first? */ - int idx = node->node_type.sub.divfeat; - ElementType val = vec[idx]; - DistanceType diff1 = val - node->node_type.sub.divlow; - DistanceType diff2 = val - node->node_type.sub.divhigh; - - NodePtr bestChild; - NodePtr otherChild; - DistanceType cut_dist; - if ((diff1 + diff2) < 0) { - bestChild = node->child1; - otherChild = node->child2; - cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx); - } else { - bestChild = node->child2; - otherChild = node->child1; - cut_dist = distance.accum_dist(val, node->node_type.sub.divlow, idx); - } - - /* Call recursively to search next level down. */ - searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError); - - DistanceType dst = dists[idx]; - mindistsq = mindistsq + cut_dist - dst; - dists[idx] = cut_dist; - if (mindistsq * epsError <= result_set.worstDist()) { - searchLevel(result_set, vec, otherChild, mindistsq, dists, epsError); - } - dists[idx] = dst; - } - -public: - /** Stores the index in a binary file. - * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when - * loading the index object it must be constructed associated to the same - * source of data points used while building it. See the example: - * examples/saveload_example.cpp \sa loadIndex */ - void saveIndex(FILE *stream) { this->saveIndex_(*this, stream); } - - /** Loads a previous index from a binary file. - * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the - * index object must be constructed associated to the same source of data - * points used while building the index. See the example: - * examples/saveload_example.cpp \sa loadIndex */ - void loadIndex(FILE *stream) { this->loadIndex_(*this, stream); } + dists[idx] = dst; + } + + public: + /** Stores the index in a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so + * when loading the index object it must be constructed associated to the + * same source of data points used while building it. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void saveIndex(std::ostream& stream) { saveIndex(*this, stream); } + + /** Loads a previous index from a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so + * the index object must be constructed associated to the same source of + * data points used while building the index. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void loadIndex(std::istream& stream) { loadIndex(*this, stream); } }; /** kd-tree dynaimic index @@ -1780,261 +2405,336 @@ class KDTreeSingleIndexDynamicAdaptor_ * \tparam DatasetAdaptor The user-provided adaptor (see comments above). * \tparam Distance The distance metric to use: nanoflann::metric_L1, * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM - * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will - * be typically size_t or int + * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType + * Will be typically size_t or int */ -template -class KDTreeSingleIndexDynamicAdaptor { -public: - typedef typename Distance::ElementType ElementType; - typedef typename Distance::DistanceType DistanceType; - -protected: - size_t m_leaf_max_size; - size_t treeCount; - size_t pointCount; - - /** - * The dataset used by this index - */ - const DatasetAdaptor &dataset; //!< The source of our data - - std::vector treeIndex; //!< treeIndex[idx] is the index of tree in which - //!< point at idx is stored. treeIndex[idx]=-1 - //!< means that point has been removed. - - KDTreeSingleIndexAdaptorParams index_params; - - int dim; //!< Dimensionality of each data point - - typedef KDTreeSingleIndexDynamicAdaptor_ - index_container_t; - std::vector index; - -public: - /** Get a const ref to the internal list of indices; the number of indices is - * adapted dynamically as the dataset grows in size. */ - const std::vector &getAllIndices() const { return index; } - -private: - /** finds position of least significant unset bit */ - int First0Bit(IndexType num) { - int pos = 0; - while (num & 1) { - num = num >> 1; - pos++; - } - return pos; - } - - /** Creates multiple empty trees to handle dynamic support */ - void init() { - typedef KDTreeSingleIndexDynamicAdaptor_ - my_kd_tree_t; - std::vector index_( - treeCount, my_kd_tree_t(dim /*dim*/, dataset, treeIndex, index_params)); - index = index_; - } - -public: - Distance distance; - - /** - * KDTree constructor - * - * Refer to docs in README.md or online in - * https://github.com/jlblancoc/nanoflann - * - * The KD-Tree point dimension (the length of each point in the datase, e.g. 3 - * for 3D points) is determined by means of: - * - The \a DIM template parameter if >0 (highest priority) - * - Otherwise, the \a dimensionality parameter of this constructor. - * - * @param inputData Dataset with the input features - * @param params Basically, the maximum leaf node size - */ - KDTreeSingleIndexDynamicAdaptor(const int dimensionality, - const DatasetAdaptor &inputData, - const KDTreeSingleIndexAdaptorParams ¶ms = - KDTreeSingleIndexAdaptorParams(), - const size_t maximumPointCount = 1000000000U) - : dataset(inputData), index_params(params), distance(inputData) { - treeCount = static_cast(std::log2(maximumPointCount)); - pointCount = 0U; - dim = dimensionality; - treeIndex.clear(); - if (DIM > 0) - dim = DIM; - m_leaf_max_size = params.leaf_max_size; - init(); - const size_t num_initial_points = dataset.kdtree_get_point_count(); - if (num_initial_points > 0) { - addPoints(0, num_initial_points - 1); - } - } - - /** Deleted copy constructor*/ - KDTreeSingleIndexDynamicAdaptor( - const KDTreeSingleIndexDynamicAdaptor &) = delete; - - /** Add points to the set, Inserts all points from [start, end] */ - void addPoints(IndexType start, IndexType end) { - size_t count = end - start + 1; - treeIndex.resize(treeIndex.size() + count); - for (IndexType idx = start; idx <= end; idx++) { - int pos = First0Bit(pointCount); - index[pos].vind.clear(); - treeIndex[pointCount] = pos; - for (int i = 0; i < pos; i++) { - for (int j = 0; j < static_cast(index[i].vind.size()); j++) { - index[pos].vind.push_back(index[i].vind[j]); - if (treeIndex[index[i].vind[j]] != -1) - treeIndex[index[i].vind[j]] = pos; +template < + typename Distance, class DatasetAdaptor, int32_t DIM = -1, + typename IndexType = uint32_t> +class KDTreeSingleIndexDynamicAdaptor +{ + public: + using ElementType = typename Distance::ElementType; + using DistanceType = typename Distance::DistanceType; + + using Offset = typename KDTreeSingleIndexDynamicAdaptor_< + Distance, DatasetAdaptor, DIM>::Offset; + using Size = typename KDTreeSingleIndexDynamicAdaptor_< + Distance, DatasetAdaptor, DIM>::Size; + using Dimension = typename KDTreeSingleIndexDynamicAdaptor_< + Distance, DatasetAdaptor, DIM>::Dimension; + + protected: + Size leaf_max_size_; + Size treeCount_; + Size pointCount_; + + /** + * The dataset used by this index + */ + const DatasetAdaptor& dataset_; //!< The source of our data + + /** treeIndex[idx] is the index of tree in which point at idx is stored. + * treeIndex[idx]=-1 means that point has been removed. */ + std::vector treeIndex_; + std::unordered_set removedPoints_; + + KDTreeSingleIndexAdaptorParams index_params_; + + Dimension dim_; //!< Dimensionality of each data point + + using index_container_t = KDTreeSingleIndexDynamicAdaptor_< + Distance, DatasetAdaptor, DIM, IndexType>; + std::vector index_; + + public: + /** Get a const ref to the internal list of indices; the number of indices + * is adapted dynamically as the dataset grows in size. */ + const std::vector& getAllIndices() const + { + return index_; + } + + private: + /** finds position of least significant unset bit */ + int First0Bit(IndexType num) + { + int pos = 0; + while (num & 1) + { + num = num >> 1; + pos++; + } + return pos; + } + + /** Creates multiple empty trees to handle dynamic support */ + void init() + { + using my_kd_tree_t = KDTreeSingleIndexDynamicAdaptor_< + Distance, DatasetAdaptor, DIM, IndexType>; + std::vector index( + treeCount_, + my_kd_tree_t(dim_ /*dim*/, dataset_, treeIndex_, index_params_)); + index_ = index; + } + + public: + Distance distance_; + + /** + * KDTree constructor + * + * Refer to docs in README.md or online in + * https://github.com/jlblancoc/nanoflann + * + * The KD-Tree point dimension (the length of each point in the datase, e.g. + * 3 for 3D points) is determined by means of: + * - The \a DIM template parameter if >0 (highest priority) + * - Otherwise, the \a dimensionality parameter of this constructor. + * + * @param inputData Dataset with the input features. Its lifetime must be + * equal or longer than that of the instance of this class. + * @param params Basically, the maximum leaf node size + */ + explicit KDTreeSingleIndexDynamicAdaptor( + const int dimensionality, const DatasetAdaptor& inputData, + const KDTreeSingleIndexAdaptorParams& params = + KDTreeSingleIndexAdaptorParams(), + const size_t maximumPointCount = 1000000000U) + : dataset_(inputData), index_params_(params), distance_(inputData) + { + treeCount_ = static_cast(std::log2(maximumPointCount)) + 1; + pointCount_ = 0U; + dim_ = dimensionality; + treeIndex_.clear(); + if (DIM > 0) dim_ = DIM; + leaf_max_size_ = params.leaf_max_size; + init(); + const size_t num_initial_points = dataset_.kdtree_get_point_count(); + if (num_initial_points > 0) { addPoints(0, num_initial_points - 1); } + } + + /** Deleted copy constructor*/ + explicit KDTreeSingleIndexDynamicAdaptor( + const KDTreeSingleIndexDynamicAdaptor< + Distance, DatasetAdaptor, DIM, IndexType>&) = delete; + + /** Add points to the set, Inserts all points from [start, end] */ + void addPoints(IndexType start, IndexType end) + { + const Size count = end - start + 1; + int maxIndex = 0; + treeIndex_.resize(treeIndex_.size() + count); + for (IndexType idx = start; idx <= end; idx++) + { + const int pos = First0Bit(pointCount_); + maxIndex = std::max(pos, maxIndex); + treeIndex_[pointCount_] = pos; + + const auto it = removedPoints_.find(idx); + if (it != removedPoints_.end()) + { + removedPoints_.erase(it); + treeIndex_[idx] = pos; + } + + for (int i = 0; i < pos; i++) + { + for (int j = 0; j < static_cast(index_[i].vAcc_.size()); + j++) + { + index_[pos].vAcc_.push_back(index_[i].vAcc_[j]); + if (treeIndex_[index_[i].vAcc_[j]] != -1) + treeIndex_[index_[i].vAcc_[j]] = pos; + } + index_[i].vAcc_.clear(); + } + index_[pos].vAcc_.push_back(idx); + pointCount_++; + } + + for (int i = 0; i <= maxIndex; ++i) + { + index_[i].freeIndex(index_[i]); + if (!index_[i].vAcc_.empty()) index_[i].buildIndex(); } - index[i].vind.clear(); - index[i].freeIndex(index[i]); - } - index[pos].vind.push_back(idx); - index[pos].buildIndex(); - pointCount++; - } - } - - /** Remove a point from the set (Lazy Deletion) */ - void removePoint(size_t idx) { - if (idx >= pointCount) - return; - treeIndex[idx] = -1; - } - - /** - * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored - * inside the result object. - * - * Params: - * result = the result object in which the indices of the - * nearest-neighbors are stored vec = the vector for which to search the - * nearest neighbors - * - * \tparam RESULTSET Should be any ResultSet - * \return True if the requested neighbors could be found. - * \sa knnSearch, radiusSearch - */ - template - bool findNeighbors(RESULTSET &result, const ElementType *vec, - const SearchParams &searchParams) const { - for (size_t i = 0; i < treeCount; i++) { - index[i].findNeighbors(result, &vec[0], searchParams); - } - return result.full(); - } + } + + /** Remove a point from the set (Lazy Deletion) */ + void removePoint(size_t idx) + { + if (idx >= pointCount_) return; + removedPoints_.insert(idx); + treeIndex_[idx] = -1; + } + + /** + * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored + * inside the result object. + * + * Params: + * result = the result object in which the indices of the + * nearest-neighbors are stored vec = the vector for which to search the + * nearest neighbors + * + * \tparam RESULTSET Should be any ResultSet + * \return True if the requested neighbors could be found. + * \sa knnSearch, radiusSearch + * + * \note If L2 norms are used, all returned distances are actually squared + * distances. + */ + template + bool findNeighbors( + RESULTSET& result, const ElementType* vec, + const SearchParameters& searchParams = {}) const + { + for (size_t i = 0; i < treeCount_; i++) + { + index_[i].findNeighbors(result, &vec[0], searchParams); + } + return result.full(); + } }; /** An L2-metric KD-tree adaptor for working with data directly stored in an - * Eigen Matrix, without duplicating the data storage. Each row in the matrix - * represents a point in the state space. + * Eigen Matrix, without duplicating the data storage. You can select whether a + * row or column in the matrix represents a point in the state space. * - * Example of usage: + * Example of usage: * \code - * Eigen::Matrix mat; - * // Fill out "mat"... + * Eigen::Matrix mat; + * + * // Fill out "mat"... + * using my_kd_tree_t = nanoflann::KDTreeEigenMatrixAdaptor< + * Eigen::Matrix>; * - * typedef KDTreeEigenMatrixAdaptor< Eigen::Matrix > - * my_kd_tree_t; const int max_leaf = 10; my_kd_tree_t mat_index(mat, max_leaf - * ); mat_index.index->buildIndex(); mat_index.index->... \endcode + * const int max_leaf = 10; + * my_kd_tree_t mat_index(mat, max_leaf); + * mat_index.index->... + * \endcode * * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality - * for the points in the data set, allowing more compiler optimizations. \tparam - * Distance The distance metric to use: nanoflann::metric_L1, + * for the points in the data set, allowing more compiler optimizations. + * \tparam Distance The distance metric to use: nanoflann::metric_L1, * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. + * \tparam row_major If set to true the rows of the matrix are used as the + * points, if set to false the columns of the matrix are used as the + * points. */ -template -struct KDTreeEigenMatrixAdaptor { - typedef KDTreeEigenMatrixAdaptor self_t; - typedef typename MatrixType::Scalar num_t; - typedef typename MatrixType::Index IndexType; - typedef - typename Distance::template traits::distance_t metric_t; - typedef KDTreeSingleIndexAdaptor - index_t; - - index_t *index; //! The kd-tree index for the user to call its methods as - //! usual with any other FLANN index. - - /// Constructor: takes a const ref to the matrix object with the data points - KDTreeEigenMatrixAdaptor(const size_t dimensionality, - const std::reference_wrapper &mat, - const int leaf_max_size = 10) - : m_data_matrix(mat) { - const auto dims = mat.get().cols(); - if (size_t(dims) != dimensionality) - throw std::runtime_error( - "Error: 'dimensionality' must match column count in data matrix"); - if (DIM > 0 && int(dims) != DIM) - throw std::runtime_error( - "Data set dimensionality does not match the 'DIM' template argument"); - index = - new index_t(static_cast(dims), *this /* adaptor */, - nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size)); - index->buildIndex(); - } - -public: - /** Deleted copy constructor */ - KDTreeEigenMatrixAdaptor(const self_t &) = delete; - - ~KDTreeEigenMatrixAdaptor() { delete index; } - - const std::reference_wrapper m_data_matrix; - - /** Query for the \a num_closest closest points to a given point (entered as - * query_point[0:dim-1]). Note that this is a short-cut method for - * index->findNeighbors(). The user can also call index->... methods as - * desired. \note nChecks_IGNORED is ignored but kept for compatibility with - * the original FLANN interface. - */ - inline void query(const num_t *query_point, const size_t num_closest, - IndexType *out_indices, num_t *out_distances_sq, - const int /* nChecks_IGNORED */ = 10) const { - nanoflann::KNNResultSet resultSet(num_closest); - resultSet.init(out_indices, out_distances_sq); - index->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); - } - - /** @name Interface expected by KDTreeSingleIndexAdaptor - * @{ */ - - const self_t &derived() const { return *this; } - self_t &derived() { return *this; } - - // Must return the number of data points - inline size_t kdtree_get_point_count() const { - return m_data_matrix.get().rows(); - } - - // Returns the dim'th component of the idx'th point in the class: - inline num_t kdtree_get_pt(const IndexType idx, size_t dim) const { - return m_data_matrix.get().coeff(idx, IndexType(dim)); - } - - // Optional bounding-box computation: return false to default to a standard - // bbox computation loop. - // Return true if the BBOX was already computed by the class and returned in - // "bb" so it can be avoided to redo it again. Look at bb.size() to find out - // the expected dimensionality (e.g. 2 or 3 for point clouds) - template bool kdtree_get_bbox(BBOX & /*bb*/) const { - return false; - } - - /** @} */ - -}; // end of KDTreeEigenMatrixAdaptor - /** @} */ - -/** @} */ // end of grouping -} // namespace nanoflann - -#endif /* NANOFLANN_HPP_ */ +template < + class MatrixType, int32_t DIM = -1, class Distance = nanoflann::metric_L2, + bool row_major = true> +struct KDTreeEigenMatrixAdaptor +{ + using self_t = + KDTreeEigenMatrixAdaptor; + using num_t = typename MatrixType::Scalar; + using IndexType = typename MatrixType::Index; + using metric_t = typename Distance::template traits< + num_t, self_t, IndexType>::distance_t; + + using index_t = KDTreeSingleIndexAdaptor< + metric_t, self_t, + row_major ? MatrixType::ColsAtCompileTime + : MatrixType::RowsAtCompileTime, + IndexType>; + + index_t* index_; //! The kd-tree index for the user to call its methods as + //! usual with any other FLANN index. + + using Offset = typename index_t::Offset; + using Size = typename index_t::Size; + using Dimension = typename index_t::Dimension; + + /// Constructor: takes a const ref to the matrix object with the data points + explicit KDTreeEigenMatrixAdaptor( + const Dimension dimensionality, + const std::reference_wrapper& mat, + const int leaf_max_size = 10, const unsigned int n_thread_build = 1) + : m_data_matrix(mat) + { + const auto dims = row_major ? mat.get().cols() : mat.get().rows(); + if (static_cast(dims) != dimensionality) + throw std::runtime_error( + "Error: 'dimensionality' must match column count in data " + "matrix"); + if (DIM > 0 && static_cast(dims) != DIM) + throw std::runtime_error( + "Data set dimensionality does not match the 'DIM' template " + "argument"); + index_ = new index_t( + dims, *this /* adaptor */, + nanoflann::KDTreeSingleIndexAdaptorParams( + leaf_max_size, nanoflann::KDTreeSingleIndexAdaptorFlags::None, + n_thread_build)); + } + + public: + /** Deleted copy constructor */ + KDTreeEigenMatrixAdaptor(const self_t&) = delete; + + ~KDTreeEigenMatrixAdaptor() { delete index_; } + + const std::reference_wrapper m_data_matrix; + + /** Query for the \a num_closest closest points to a given point (entered as + * query_point[0:dim-1]). Note that this is a short-cut method for + * index->findNeighbors(). The user can also call index->... methods as + * desired. + * + * \note If L2 norms are used, all returned distances are actually squared + * distances. + */ + void query( + const num_t* query_point, const Size num_closest, + IndexType* out_indices, num_t* out_distances) const + { + nanoflann::KNNResultSet resultSet(num_closest); + resultSet.init(out_indices, out_distances); + index_->findNeighbors(resultSet, query_point); + } + + /** @name Interface expected by KDTreeSingleIndexAdaptor + * @{ */ + + const self_t& derived() const { return *this; } + self_t& derived() { return *this; } + + // Must return the number of data points + Size kdtree_get_point_count() const + { + if (row_major) + return m_data_matrix.get().rows(); + else + return m_data_matrix.get().cols(); + } + + // Returns the dim'th component of the idx'th point in the class: + num_t kdtree_get_pt(const IndexType idx, size_t dim) const + { + if (row_major) + return m_data_matrix.get().coeff(idx, IndexType(dim)); + else + return m_data_matrix.get().coeff(IndexType(dim), idx); + } + + // Optional bounding-box computation: return false to default to a standard + // bbox computation loop. + // Return true if the BBOX was already computed by the class and returned + // in "bb" so it can be avoided to redo it again. Look at bb.size() to + // find out the expected dimensionality (e.g. 2 or 3 for point clouds) + template + bool kdtree_get_bbox(BBOX& /*bb*/) const + { + return false; + } + + /** @} */ + +}; // end of KDTreeEigenMatrixAdaptor +/** @} */ + +/** @} */ // end of grouping +} // namespace nanoflann diff --git a/modules/alphamat/src/cm.cpp b/modules/alphamat/src/cm.cpp index 8d7f682d14e..cfd6ccab6c3 100644 --- a/modules/alphamat/src/cm.cpp +++ b/modules/alphamat/src/cm.cpp @@ -61,7 +61,7 @@ void kdtree_CM(Mat& img, my_vector_of_vectors_t& indm, my_vector_of_vectors_t& s for (std::unordered_set::iterator it = unk.begin(); it != unk.end(); it++) { resultSet.init(&ret_indexes[0], &out_dists_sqr[0]); - mat_index.index->findNeighbors(resultSet, &samples[*it][0], nanoflann::SearchParams(10)); + mat_index.index->findNeighbors(resultSet, &samples[*it][0], nanoflann::SearchParameters(10)); indm[i].resize(num_results - 1); for (std::size_t j = 1; j < num_results; j++) diff --git a/modules/alphamat/src/intraU.cpp b/modules/alphamat/src/intraU.cpp index 167458fcfc1..0bce9c38d6b 100644 --- a/modules/alphamat/src/intraU.cpp +++ b/modules/alphamat/src/intraU.cpp @@ -78,7 +78,7 @@ void kdtree_intraU(Mat& img, Mat& tmap, my_vector_of_vectors_t& indm, my_vector_ for (int i = 0; i < N; i++) { resultSet.init(&ret_indexes[0], &out_dists_sqr[0]); - mat_index.index->findNeighbors(resultSet, &samples[i][0], nanoflann::SearchParams(10)); + mat_index.index->findNeighbors(resultSet, &samples[i][0], nanoflann::SearchParameters(10)); indm[i].resize(num_results - 1); for (std::size_t j = 1; j < num_results; j++)