You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 
 
 
 

131 lines
4.7 KiB

#ifndef MEDUSA_BITS_SPATIAL_SEARCH_KDTREEMUTABLE_FWD_HPP_
#define MEDUSA_BITS_SPATIAL_SEARCH_KDTREEMUTABLE_FWD_HPP_
/**
* @file
* Declaration of KDTreeMutable.
*
* @example test/spatial_search/KDTreeMutable_test.cpp
*/
#include <medusa/Config.hpp>
#include <medusa/bits/types/Range_fwd.hpp>
#include <nanoflann/nanoflann.hpp>
#include <array>
#include <iosfwd>
#include "PointCloud.hpp"
namespace mm
{
/**
* A [k-d tree](https://en.wikipedia.org/wiki/K-d_tree) data structure that supports dynamic
* insertions and lazy-removal.
*
* This class is a wrapper around [nanoflann](https://github.com/jlblancoc/nanoflann)
* k-d tree implementation. For specific behavior please refer to
* [nanoflann docs](http://jlblancoc.github.io/nanoflann/classnanoflann_1_1KDTreeSingleIndexDynamicAdaptor.html).
*
* If dynamic structure is not needed, a faster static KDTree structure can be used.
*
* Usage example:
* @snippet spatial_search/KDTreeMutable_test.cpp KDTreeMutable usage example
* @ingroup utils
* @sa KDTree, KDGrid, GeneralFill, FindSupport
*/
template <class vec_t>
class KDTreeMutable
{
public:
typedef vec_t vector_t; ///< Vector type used.
typedef typename vector_t::scalar_t scalar_t; ///< Scalar type used.
/// Store dimension of the space.
enum
{ /** Dimensionality of the space. */ dim = vec_t::dim };
static_assert(std::is_same<double, scalar_t>::value,
"Not Implemented Error: only implemented for double type!");
private:
typedef nanoflann::KDTreeSingleIndexDynamicAdaptor<
nanoflann::L2_Simple_Adaptor<scalar_t, kdtree_internal::PointCloud<vec_t>>,
kdtree_internal::PointCloud<vec_t>, dim>
kd_tree_t; ///< Tree type.
kdtree_internal::PointCloud<vec_t> points_; ///< Points, contained in the tree.
int size_; ///< Number of points in the tree.
kd_tree_t tree; ///< Actual tree build over points.
public:
/**
* Constructor that builds the search tree for the given points
* @param points A collection of points.
*/
explicit KDTreeMutable(const Range<vec_t> &points) : points_(points), size_(points.size()),
tree(dim, points_, nanoflann::KDTreeSingleIndexAdaptorParams(20)) {}
/// Creates an empty k-d tree.
KDTreeMutable() : points_(), size_(0),
tree(dim, points_, nanoflann::KDTreeSingleIndexAdaptorParams(20)) {}
/**
* Grows a new tree with new points.
* This function deletes the old points and the old tree and builds a new one with the given
* points.
*
* @param points A new container of points.
*/
void reset(const Range<vec_t> &points)
{
points_.setPts(points);
size_ = points.size();
tree.reset();
}
/**
* Insert a point into the tree.
* @param point Point to be inserted into the tree.
*/
void insert(const vec_t &point);
/// Inserts a sequence of points.
void insert(const Range<vec_t> &points);
/// Check if any point exists in sphere centered at `p` with radius `r`.
bool existsPointInSphere(const vec_t &p, scalar_t r)
{
if (size_ == 0)
return false;
return query(p).second[0] <= r * r;
}
/**
* Removes a point with given index from the tree. The indexes of the points are given
* sequentially at insertion and do not change. The removal is lazy and point still takes
* up memory.
*/
void remove(int index)
{
size_ -= tree.removePoint(index);
}
/// Returns number of points in the tree.
int size() const { return size_; }
/**
* Find `k` nearest neighbors to given point.
* @param point Find closest points to this point.
* @param k How many nearest points to find.
*
* @return A pair of two vectors of size `k` containing
* indices of nearest neighbors and squared distances to said neighbors.
* @throw Assertion fails if there are not enough points in the tree.
*/
std::pair<mm::Range<int>, mm::Range<double>> query(const vec_t &point, int k = 1);
/// Output basic info about given tree.
template <class V>
friend std::ostream &operator<<(std::ostream &os, const KDTreeMutable<V> &tree);
};
} // namespace mm
#endif // MEDUSA_BITS_SPATIAL_SEARCH_KDTREEMUTABLE_FWD_HPP_