Template Class KDTreeCapable

Nested Relationships

Nested Types

Class Documentation

template<class Derived, typename num_t = float, typename metric_t = nanoflann::L2_Simple_Adaptor<num_t, Derived>>
class KDTreeCapable

Adaptor class providing Nearest Neighbor (NN) searcg via nanoflann, making use of the CRTP design pattern.

Derived classes must call kdtree_mark_as_outdated() when data points change to mark the cached KD-tree (an “index”) as invalid, and they must also implement the following interface (note that these are not virtual functions due to the usage of CRTP):

  // Must return the number of data points
  size_t kdtree_get_point_count() const { ... }

  // Returns the distance between the vector "p1[0:size-1]" and the data
point with index "idx_p2" stored in the class:
  float kdtree_distance(
    const float *p1, const size_t idx_p2,size_t size) const { ... }

  // Returns the dim'th component of the idx'th point in the class:
  num_t kdtree_get_pt(const size_t idx, int dim) const { ... }

  // 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 <class BBOX>
  bool kdtree_get_bbox(BBOX &bb) const
  {
     bb[0].low = ...; bb[0].high = ...;  // "x" limits
     return true;
  }

The KD-tree index will be built on demand only upon call of any of the query methods provided by this class.

Notice that there is only ONE internal cached KD-tree, so if a method to query a 2D point is called, then another method for 3D points, then again the 2D method, three KD-trees will be built. So, try to group all the calls for a given dimensionality together or build different class instances for queries of each dimensionality.

See also

See some of the derived classes for example implementations. See also the documentation of nanoflann

Public utility methods to query the KD-tree

inline size_t kdTreeClosestPoint2D(float x0, float y0, float &out_x, float &out_y, float &out_dist_sqr) const

KD Tree-based search for the closest point (only ONE) to some given 2D coordinates. This method automatically build the “m_kdtree_data” structure when:

  • It is called for the first time

  • The map has changed

  • The KD-tree was build for 3D.

See also

kdTreeClosestPoint3D, kdTreeTwoClosestPoint2D

Parameters:

  • x0 – The X coordinate of the query.

  • y0 – The Y coordinate of the query.

  • out_x – The X coordinate of the found closest correspondence.

  • out_y – The Y coordinate of the found closest correspondence.

  • out_dist_sqr – The square distance between the query and the returned point.

Returns:

The index of the closest point in the map array.

inline size_t kdTreeClosestPoint2D(float x0, float y0, float &out_dist_sqr) const

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

inline size_t kdTreeClosestPoint2D(const TPoint2D &p0, TPoint2D &pOut, float &outDistSqr) const

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

inline float kdTreeClosestPoint2DsqrError(float x0, float y0) const

Like kdTreeClosestPoint2D, but just return the square error from some point to its closest neighbor.

inline float kdTreeClosestPoint2DsqrError(const TPoint2D &p0) const
inline void kdTreeTwoClosestPoint2D(float x0, float y0, float &out_x1, float &out_y1, float &out_x2, float &out_y2, float &out_dist_sqr1, float &out_dist_sqr2) const

KD Tree-based search for the TWO closest point to some given 2D coordinates. This method automatically build the “m_kdtree_data” structure when:

  • It is called for the first time

  • The map has changed

  • The KD-tree was build for 3D.

See also

kdTreeClosestPoint2D

Parameters:

  • x0 – The X coordinate of the query.

  • y0 – The Y coordinate of the query.

  • out_x1 – The X coordinate of the first correspondence.

  • out_y1 – The Y coordinate of the first correspondence.

  • out_x2 – The X coordinate of the second correspondence.

  • out_y2 – The Y coordinate of the second correspondence.

  • out_dist_sqr1 – The square distance between the query and the first returned point.

  • out_dist_sqr2 – The square distance between the query and the second returned point.

inline void kdTreeTwoClosestPoint2D(const TPoint2D &p0, TPoint2D &pOut1, TPoint2D &pOut2, float &outDistSqr1, float &outDistSqr2) const
inline std::vector<size_t> kdTreeNClosestPoint2D(float x0, float y0, size_t knn, std::vector<float> &out_x, std::vector<float> &out_y, std::vector<float> &out_dist_sqr, const std::optional<float> &maximumSearchDistanceSqr = std::nullopt) const

KD Tree-based search for the N closest point to some given 2D coordinates. This method automatically build the “m_kdtree_data” structure when:

  • It is called for the first time

  • The map has changed

  • The KD-tree was build for 3D.

See also

kdTreeClosestPoint2D

See also

kdTreeTwoClosestPoint2D

Parameters:

  • x0 – The X coordinate of the query.

  • y0 – The Y coordinate of the query.

  • N – The number of closest points to search.

  • out_x – The vector containing the X coordinates of the correspondences.

  • out_y – The vector containing the Y coordinates of the correspondences.

  • out_dist_sqr – The vector containing the square distance between the query and the returned points.

  • maximumSearchDistanceSqr – If provided, only NN up to that given squared distance will be returned.

Returns:

The list of indices

inline std::vector<size_t> kdTreeNClosestPoint2D(const TPoint2D &p0, size_t N, std::vector<TPoint2D> &pOut, std::vector<float> &outDistSqr, const std::optional<float> &maximumSearchDistanceSqr = std::nullopt) const
inline void kdTreeNClosestPoint2DIdx(float x0, float y0, size_t knn, std::vector<size_t> &out_idx, std::vector<float> &out_dist_sqr, const std::optional<float> &maximumSearchDistanceSqr = std::nullopt) const

KD Tree-based search for the N closest point to some given 2D coordinates and returns their indexes. This method automatically build the “m_kdtree_data” structure when:

  • It is called for the first time

  • The map has changed

  • The KD-tree was build for 3D.

See also

kdTreeClosestPoint2D

Parameters:

  • x0 – The X coordinate of the query.

  • y0 – The Y coordinate of the query.

  • N – The number of closest points to search.

  • out_idx – The indexes of the found closest correspondence.

  • out_dist_sqr – The square distance between the query and the returned point.

  • maximumSearchDistanceSqr – If provided, only NN up to that given squared distance will be returned.

inline void kdTreeNClosestPoint2DIdx(const TPoint2D &p0, size_t N, std::vector<size_t> &outIdx, std::vector<float> &outDistSqr) const
inline size_t kdTreeClosestPoint3D(float x0, float y0, float z0, float &out_x, float &out_y, float &out_z, float &out_dist_sqr) const

KD Tree-based search for the closest point (only ONE) to some given 3D coordinates. This method automatically build the “m_kdtree_data” structure when:

  • It is called for the first time

  • The map has changed

  • The KD-tree was build for 2D.

See also

kdTreeClosestPoint2D

Parameters:

  • x0 – The X coordinate of the query.

  • y0 – The Y coordinate of the query.

  • z0 – The Z coordinate of the query.

  • out_x – The X coordinate of the found closest correspondence.

  • out_y – The Y coordinate of the found closest correspondence.

  • out_z – The Z coordinate of the found closest correspondence.

  • out_dist_sqr – The square distance between the query and the returned point.

Returns:

The index of the closest point in the map array.

inline size_t kdTreeClosestPoint3D(float x0, float y0, float z0, float &out_dist_sqr) const

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

inline size_t kdTreeClosestPoint3D(const TPoint3D &p0, TPoint3D &pOut, float &outDistSqr) const

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

inline void kdTreeNClosestPoint3D(float x0, float y0, float z0, size_t knn, std::vector<float> &out_x, std::vector<float> &out_y, std::vector<float> &out_z, std::vector<float> &out_dist_sqr, const std::optional<float> &maximumSearchDistanceSqr = std::nullopt) const

KD Tree-based search for the N closest points to some given 3D coordinates. This method automatically build the “m_kdtree_data” structure when:

  • It is called for the first time

  • The map has changed

  • The KD-tree was build for 2D.

See also

kdTreeNClosestPoint2D

Parameters:

  • x0 – The X coordinate of the query.

  • y0 – The Y coordinate of the query.

  • z0 – The Z coordinate of the query.

  • N – The number of closest points to search.

  • out_x – The vector containing the X coordinates of the correspondences.

  • out_y – The vector containing the Y coordinates of the correspondences.

  • out_z – The vector containing the Z coordinates of the correspondences.

  • out_dist_sqr – The vector containing the square distance between the query and the returned points.

  • maximumSearchDistanceSqr – If provided, only NN up to that given squared distance will be returned.

inline void kdTreeNClosestPoint3DWithIdx(float x0, float y0, float z0, size_t knn, std::vector<float> &out_x, std::vector<float> &out_y, std::vector<float> &out_z, std::vector<size_t> &out_idx, std::vector<float> &out_dist_sqr, const std::optional<float> &maximumSearchDistanceSqr = std::nullopt) const

KD Tree-based search for the N closest points to some given 3D coordinates. This method automatically build the “m_kdtree_data” structure when:

  • It is called for the first time

  • The map has changed

  • The KD-tree was build for 2D.

See also

kdTreeNClosestPoint2D

Parameters:

  • x0 – The X coordinate of the query.

  • y0 – The Y coordinate of the query.

  • z0 – The Z coordinate of the query.

  • N – The number of closest points to search.

  • out_x – The vector containing the X coordinates of the correspondences.

  • out_y – The vector containing the Y coordinates of the correspondences.

  • out_z – The vector containing the Z coordinates of the correspondences.

  • out_idx – The vector containing the indexes of the correspondences.

  • out_dist_sqr – The vector containing the square distance between the query and the returned points.

inline void kdTreeNClosestPoint3D(const TPoint3D &p0, size_t N, std::vector<TPoint3D> &pOut, std::vector<float> &outDistSqr, const std::optional<float> &maximumSearchDistanceSqr = std::nullopt) const
inline size_t kdTreeRadiusSearch3D(const num_t x0, const num_t y0, const num_t z0, const num_t maxRadiusSqr, std::vector<nanoflann::ResultItem<size_t, num_t>> &out_indices_dist) const

KD Tree-based search for all the points within a given radius of some 3D point. This method automatically build the “m_kdtree_data” structure when:

  • It is called for the first time

  • The map has changed

  • The KD-tree was build for 2D.

See also

kdTreeRadiusSearch2D, kdTreeNClosestPoint3DIdx

Parameters:

  • x0 – The X coordinate of the query.

  • y0 – The Y coordinate of the query.

  • z0 – The Z coordinate of the query.

  • maxRadiusSqr – The square of the desired search radius.

  • out_indices_dist – The output list, with pairs of indeces/squared distances for the found correspondences.

Returns:

Number of found points.

inline size_t kdTreeRadiusSearch2D(const num_t x0, const num_t y0, const num_t maxRadiusSqr, std::vector<nanoflann::ResultItem<size_t, num_t>> &out_indices_dist) const

KD Tree-based search for all the points within a given radius of some 2D point. This method automatically build the “m_kdtree_data” structure when:

  • It is called for the first time

  • The map has changed

  • The KD-tree was build for 3D.

See also

kdTreeRadiusSearch3D, kdTreeNClosestPoint2DIdx

Parameters:

  • x0 – The X coordinate of the query.

  • y0 – The Y coordinate of the query.

  • maxRadiusSqr – The square of the desired search radius.

  • out_indices_dist – The output list, with pairs of indeces/squared distances for the found correspondences.

Returns:

Number of found points.

inline void kdTreeNClosestPoint3DIdx(float x0, float y0, float z0, size_t knn, std::vector<size_t> &out_idx, std::vector<float> &out_dist_sqr, const std::optional<float> &maximumSearchDistanceSqr = std::nullopt) const

KD Tree-based search for the N closest point to some given 3D coordinates and returns their indexes. This method automatically build the “m_kdtree_data” structure when:

  • It is called for the first time

  • The map has changed

  • The KD-tree was build for 2D.

See also

kdTreeClosestPoint2D, kdTreeRadiusSearch3D

Parameters:

  • x0 – The X coordinate of the query.

  • y0 – The Y coordinate of the query.

  • z0 – The Z coordinate of the query.

  • N – The number of closest points to search.

  • out_idx – The indexes of the found closest correspondence.

  • out_dist_sqr – The square distance between the query and the returned point.

inline void kdTreeNClosestPoint3DIdx(const TPoint3D &p0, size_t N, std::vector<size_t> &outIdx, std::vector<float> &outDistSqr, const std::optional<float> &maximumSearchDistanceSqr = std::nullopt) const
inline void kdTreeEnsureIndexBuilt3D()
inline void kdTreeEnsureIndexBuilt2D()
inline void kdtree_mark_as_outdated() const

To be called by child classes when KD tree data changes.

Public Types

using self_t = KDTreeCapable<Derived, num_t, metric_t>

Public Functions

KDTreeCapable() = default

Constructor.

inline KDTreeCapable(const KDTreeCapable&)
inline KDTreeCapable &operator=(const KDTreeCapable&)
inline const Derived &derived() const

CRTP helper method.

inline Derived &derived()

CRTP helper method.

Public Members

TKDTreeSearchParams kdtree_search_params

Parameters to tune KD-tree searches. Refer to nanoflann docs.

struct TKDTreeSearchParams

Public Functions

TKDTreeSearchParams() = default

Public Members

size_t leaf_max_size = 10

Max points per leaf