37 #ifndef OMPL_DATASTRUCTURES_NEAREST_NEIGHBORS_FLANN_
38 #define OMPL_DATASTRUCTURES_NEAREST_NEIGHBORS_FLANN_
40 #include "ompl/config.h"
41 #if OMPL_HAVE_FLANN == 0
42 # error FLANN is not available. Please use a different NearestNeighbors data structure.
45 #include "ompl/datastructures/NearestNeighbors.h"
46 #include "ompl/base/StateSpace.h"
48 #include <flann/flann.hpp>
59 typedef _T ElementType;
60 typedef double ResultType;
67 template <
typename Iterator1,
typename Iterator2>
68 ResultType operator()(Iterator1 a, Iterator2 b,
69 size_t , ResultType = -1)
const
71 return distFun_(*a, *b);
86 template<
typename _T,
typename _Dist = FLANNDistance<_T> >
118 virtual void add(
const _T &data)
125 data_.push_back(data);
129 index_->addPoints(mat, std::numeric_limits<float>::max()/
size());
133 virtual void add(
const std::vector<_T> &data)
135 unsigned int oldSize =
data_.size();
136 unsigned int newSize = oldSize + data.size();
137 bool rebuild =
index_ && (newSize >
data_.capacity());
144 std::copy(data.begin(), data.end(),
data_.begin() + oldSize);
146 index_->addPoints(mat, std::numeric_limits<float>::max()/
size());
155 virtual bool remove(
const _T& data)
157 if (!
index_)
return false;
158 _T& elt =
const_cast<_T&
>(data);
159 const flann::Matrix<_T> query(&elt, 1,
dimension_);
160 std::vector<std::vector<size_t> > indices(1);
161 std::vector<std::vector<double> > dists(1);
163 if (*
index_->getPoint(indices[0][0]) == data)
165 index_->removePoint(indices[0][0]);
175 _T& elt =
const_cast<_T&
>(data);
176 const flann::Matrix<_T> query(&elt, 1,
dimension_);
177 std::vector<std::vector<size_t> > indices(1);
178 std::vector<std::vector<double> > dists(1);
180 return *
index_->getPoint(indices[0][0]);
182 throw Exception(
"No elements found in nearest neighbors data structure");
186 virtual void nearestK(
const _T &data, std::size_t k, std::vector<_T> &nbh)
const
188 _T& elt =
const_cast<_T&
>(data);
189 const flann::Matrix<_T> query(&elt, 1,
dimension_);
190 std::vector<std::vector<size_t> > indices;
191 std::vector<std::vector<double> > dists;
194 for (std::size_t i = 0 ; i < k ; ++i)
195 nbh[i] = *
index_->getPoint(indices[0][i]);
199 virtual void nearestR(
const _T &data,
double radius, std::vector<_T> &nbh)
const
201 _T& elt =
const_cast<_T&
>(data);
203 std::vector<std::vector<size_t> > indices;
204 std::vector<std::vector<double> > dists;
207 for (
int i = 0 ; i < k ; ++i)
208 nbh[i] = *
index_->getPoint(indices[0][i]);
211 virtual std::size_t
size(
void)
const
216 virtual void list(std::vector<_T> &data)
const
218 std::size_t sz =
size();
224 const _T& dummy = *
index_->getPoint(0);
268 unsigned int getContainerSize(
void)
const
289 std::vector<_T> data;
293 data_.reserve(capacity);
307 boost::shared_ptr<flann::IndexParams>
params_;
320 const flann::Matrix<double>& mat)
322 index_ =
new flann::Index<flann::L2<double> >(mat, *params_);
323 index_->buildIndex();
326 template<
typename _T,
typename _Dist = FLANNDistance<_T> >
332 boost::shared_ptr<flann::LinearIndexParams>(
333 new flann::LinearIndexParams()))
338 template<
typename _T,
typename _Dist = FLANNDistance<_T> >
344 boost::shared_ptr<flann::HierarchicalClusteringIndexParams>(
345 new flann::HierarchicalClusteringIndexParams()))
flann::SearchParams & getSearchParams(void)
Get the FLANN parameters used during nearest neighbor searches.
virtual std::size_t size(void) const
Get the number of elements in the datastructure.
virtual void clear(void)
Clear the datastructure.
unsigned int dimension_
If each element has an array-like structure that is exposed to FLANN, then the dimension_ needs to be...
boost::function< double(const _T &, const _T &)> DistanceFunction
The definition of a distance function.
Wrapper class to allow FLANN access to the NearestNeighbors::distFun_ callback function.
virtual _T nearest(const _T &data) const
Get the nearest neighbor of a point.
boost::shared_ptr< flann::IndexParams > params_
The FLANN index parameters. This contains both the type of index and the parameters for that type...
virtual void add(const std::vector< _T > &data)
Add a vector of points.
virtual void nearestR(const _T &data, double radius, std::vector< _T > &nbh) const
Return the nearest neighbors within distance radius in sorted order if searchParams_.sorted==true (the default)
std::vector< _T > data_
vector of data stored in FLANN's index. FLANN only indexes references, so we need store the original ...
virtual void list(std::vector< _T > &data) const
Get all the elements in the datastructure.
Wrapper class for nearest neighbor data structures in the FLANN library.
virtual void setDistanceFunction(const DistanceFunction &distFun)
Set the distance function to use.
flann::Index< _Dist > * index_
The FLANN index (the actual index type depends on params_).
flann::SearchParams searchParams_
The parameters used to seach for nearest neighbors.
Abstract representation of a container that can perform nearest neighbors queries.
void rebuildIndex(unsigned int capacity=0)
Rebuild the nearest neighbor data structure (necessary when changing the distance function or index p...
void createIndex(const flann::Matrix< _T > &mat)
Internal function to construct nearest neighbor data structure with initial elements stored in mat...
The exception type for ompl.
virtual void setIndexParams(const boost::shared_ptr< flann::IndexParams > ¶ms)
Set the FLANN index parameters.
const flann::SearchParams & getSearchParams(void) const
Get the FLANN parameters used during nearest neighbor searches.
virtual const boost::shared_ptr< flann::IndexParams > & getIndexParams(void) const
Get the FLANN parameters used to build the current index.
virtual void add(const _T &data)
Add an element to the datastructure.
virtual void nearestK(const _T &data, std::size_t k, std::vector< _T > &nbh) const
Return the k nearest neighbors in sorted order if searchParams_.sorted==true (the default) ...
virtual void setSearchParams(const flann::SearchParams &searchParams)
Set the FLANN parameters to be used during nearest neighbor searches.