template class mrpt::math::KDTreeCapable

Overview

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

#include <mrpt/math/KDTreeCapable.h>

template <
    class Derived,
    typename num_t = float,
    typename metric_t = nanoflann::L2_Simple_Adaptor<num_t, Derived>
    >
class KDTreeCapable
{
public:
    // typedefs

    typedef KDTreeCapable<Derived, num_t, metric_t> self_t;

    // structs

    template <int _DIM = -1>
    struct TKDTreeDataHolder;

    struct TKDTreeSearchParams;

    // fields

    TKDTreeSearchParams kdtree_search_params;

    // construction

    KDTreeCapable();
    KDTreeCapable(const KDTreeCapable& o);

    // methods

    size_t kdTreeClosestPoint2D(float x0, float y0, float& out_x, float& out_y, float& out_dist_sqr) const;
    size_t kdTreeClosestPoint2D(float x0, float y0, float& out_dist_sqr) const;
    size_t kdTreeClosestPoint2D(const TPoint2D& p0, TPoint2D& pOut, float& outDistSqr) const;
    float kdTreeClosestPoint2DsqrError(float x0, float y0) const;
    float kdTreeClosestPoint2DsqrError(const TPoint2D& p0) const;

    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;

    void kdTreeTwoClosestPoint2D(
        const TPoint2D& p0,
        TPoint2D& pOut1,
        TPoint2D& pOut2,
        float& outDistSqr1,
        float& outDistSqr2
        ) const;

    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;

    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;

    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;

    void kdTreeNClosestPoint2DIdx(
        const TPoint2D& p0,
        size_t N,
        std::vector<size_t>& outIdx,
        std::vector<float>& outDistSqr
        ) const;

    size_t kdTreeClosestPoint3D(
        float x0,
        float y0,
        float z0,
        float& out_x,
        float& out_y,
        float& out_z,
        float& out_dist_sqr
        ) const;

    size_t kdTreeClosestPoint3D(float x0, float y0, float z0, float& out_dist_sqr) const;
    size_t kdTreeClosestPoint3D(const TPoint3D& p0, TPoint3D& pOut, float& outDistSqr) const;

    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;

    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;

    void kdTreeNClosestPoint3D(
        const TPoint3D& p0,
        size_t N,
        std::vector<TPoint3D>& pOut,
        std::vector<float>& outDistSqr,
        const std::optional<float>& maximumSearchDistanceSqr = std::nullopt
        ) const;

    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;

    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;

    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;

    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;

    void kdTreeEnsureIndexBuilt3D();
    void kdTreeEnsureIndexBuilt2D();
    bool kdtree_save_index_3D(] std::ostream& out) const;
    void kdtree_load_index_3D(] std::istream& in) const;
    bool kdtree_save_index_2D(] std::ostream& out) const;
    void kdtree_load_index_2D(] std::istream& in) const;
    KDTreeCapable& operator = (const KDTreeCapable& o);
    const Derived& derived() const;
    Derived& derived();
};

// direct descendants

class CPointsMap;

Fields

TKDTreeSearchParams kdtree_search_params

Parameters to tune KD-tree searches.

Refer to nanoflann docs.

Construction

KDTreeCapable()

Constructor.

KDTreeCapable(const KDTreeCapable& o)

Copy constructor: the cached index is NOT copied (it will be rebuilt on demand).

A kdtree_disable() opt-out is inherited from the source, but never undone: see operator=().

Methods

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.

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.

See also:

kdTreeClosestPoint3D, kdTreeTwoClosestPoint2D

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.

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.

float kdTreeClosestPoint2DsqrError(float x0, float y0) const

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

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.

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.

See also:

kdTreeClosestPoint2D

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.

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

See also:

kdTreeClosestPoint2D

kdTreeTwoClosestPoint2D

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.

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.

See also:

kdTreeClosestPoint2D

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.

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.

See also:

kdTreeClosestPoint2D

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.

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.

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.

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.

See also:

kdTreeNClosestPoint2D

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.

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.

See also:

kdTreeNClosestPoint2D

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.

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.

See also:

kdTreeRadiusSearch2D, kdTreeNClosestPoint3DIdx

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.

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.

See also:

kdTreeRadiusSearch3D, kdTreeNClosestPoint2DIdx

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.

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.

See also:

kdTreeClosestPoint2D, kdTreeRadiusSearch3D

bool kdtree_save_index_3D(] std::ostream& out) const

Serializes the current 3D KD-tree index into the given binary stream, building it first if needed.

Returns false (writing nothing) if the map has no points.

IMPORTANT: only the tree topology is written, NOT the point data. To restore it later, first load the points so that kdtree_get_point_count() and kdtree_get_pt() return the identical dataset, then call kdtree_load_index_3D(). A point-count header is written so that kdtree_load_index_3D() can detect a mismatched dataset.

The typical use case is caching the (costly to build) index alongside a serialized point cloud, so it does not have to be rebuilt on load.

Requires nanoflann >= v1.5.0; throws std::runtime_error otherwise.

See also:

kdtree_load_index_3D

void kdtree_load_index_3D(] std::istream& in) const

Restores a 3D KD-tree index previously written by kdtree_save_index_3D(), avoiding the (costly) rebuild that would otherwise happen on the next query.

The associated points MUST already be present and identical to those used when the index was saved; this is verified against the stored point-count header (throws std::runtime_error on mismatch). On success the index is marked up-to-date and will NOT be rebuilt on the next query, until the derived class calls kdtree_mark_as_outdated() (e.g. if points change).

Requires nanoflann >= v1.5.0; throws std::runtime_error otherwise.

See also:

kdtree_save_index_3D

bool kdtree_save_index_2D(] std::ostream& out) const

2D counterpart of kdtree_save_index_3D().

Requires nanoflann >= v1.5.0; throws std::runtime_error otherwise.

See also:

kdtree_load_index_2D

void kdtree_load_index_2D(] std::istream& in) const

2D counterpart of kdtree_load_index_3D().

Requires nanoflann >= v1.5.0; throws std::runtime_error otherwise.

See also:

kdtree_save_index_2D

KDTreeCapable& operator = (const KDTreeCapable& o)

Copy assignment: as the copy constructor, and note that the kdtree_disable() opt-out is sticky.

Assigning from an object whose index is enabled does NOT re-enable ours: an index a class declared unsupported for itself must never come back to life through an assignment.

See also:

kdtree_disable()

const Derived& derived() const

CRTP helper method.

Derived& derived()

CRTP helper method.