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;
62 FLANNDistance(
const typename NearestNeighbors<_T>::DistanceFunction& distFun)
67 template <
typename Iterator1,
typename Iterator2>
68 ResultType operator()(Iterator1 a, Iterator2 b,
69 size_t , ResultType = -1)
const
71 return distFun_(*a, *b);
74 const typename NearestNeighbors<_T>::DistanceFunction& distFun_;
86 template<
typename _T,
typename _Dist = FLANNDistance<_T> >
87 class NearestNeighborsFLANN :
public NearestNeighbors<_T>
91 NearestNeighborsFLANN(
const boost::shared_ptr<flann::IndexParams> ¶ms)
92 : index_(0), params_(params), searchParams_(32, 0., true), dimension_(1)
96 virtual ~NearestNeighborsFLANN(
void)
102 virtual void clear(
void)
112 virtual void setDistanceFunction(
const typename NearestNeighbors<_T>::DistanceFunction &distFun)
118 virtual void add(
const _T &data)
120 bool rebuild = index_ && (data_.size() + 1 > data_.capacity());
123 rebuildIndex(2 * data_.capacity());
125 data_.push_back(data);
126 const flann::Matrix<_T> mat(&data_.back(), 1, dimension_);
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());
140 rebuildIndex(std::max(2 * oldSize, newSize));
144 std::copy(data.begin(), data.end(), data_.begin() + oldSize);
145 const flann::Matrix<_T> mat(&data_[oldSize], data.size(), dimension_);
146 index_->addPoints(mat, std::numeric_limits<float>::max()/size());
151 const flann::Matrix<_T> mat(&data_[0], data_.size(), dimension_);
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);
162 index_->knnSearch(query, indices, dists, 1, searchParams_);
163 if (*index_->getPoint(indices[0][0]) == data)
165 index_->removePoint(indices[0][0]);
171 virtual _T nearest(
const _T &data)
const
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);
179 index_->knnSearch(query, indices, dists, 1, searchParams_);
180 return *index_->getPoint(indices[0][0]);
182 throw Exception(
"No elements found in nearest neighbors data structure");
184 virtual void nearestK(
const _T &data, std::size_t k, std::vector<_T> &nbh)
const
186 _T& elt =
const_cast<_T&
>(data);
187 const flann::Matrix<_T> query(&elt, 1, dimension_);
188 std::vector<std::vector<size_t> > indices;
189 std::vector<std::vector<double> > dists;
190 k = index_ ? index_->knnSearch(query, indices, dists, k, searchParams_) : 0;
192 for (std::size_t i = 0 ; i < k ; ++i)
193 nbh[i] = *index_->getPoint(indices[0][i]);
196 virtual void nearestR(
const _T &data,
double radius, std::vector<_T> &nbh)
const
198 _T& elt =
const_cast<_T&
>(data);
199 flann::Matrix<_T> query(&elt, 1, dimension_);
200 std::vector<std::vector<size_t> > indices;
201 std::vector<std::vector<double> > dists;
202 int k = index_ ? index_->radiusSearch(query, indices, dists, radius, searchParams_) : 0;
204 for (
int i = 0 ; i < k ; ++i)
205 nbh[i] = *index_->getPoint(indices[0][i]);
208 virtual std::size_t size(
void)
const
210 return index_ ? index_->size() : 0;
213 virtual void list(std::vector<_T> &data)
const
215 std::size_t sz = size();
221 const _T& dummy = *index_->getPoint(0);
222 int checks = searchParams_.checks;
223 searchParams_.checks = size();
224 nearestK(dummy, sz, data);
225 searchParams_.checks = checks;
232 virtual void setIndexParams(
const boost::shared_ptr<flann::IndexParams> ¶ms)
239 virtual const boost::shared_ptr<flann::IndexParams>& getIndexParams(
void)
const
246 virtual void setSearchParams(
const flann::SearchParams& searchParams)
248 searchParams_ = searchParams;
253 flann::SearchParams& getSearchParams(
void)
255 return searchParams_;
260 const flann::SearchParams& getSearchParams(
void)
const
262 return searchParams_;
265 unsigned int getContainerSize(
void)
const
274 void createIndex(
const flann::Matrix<_T>& mat)
276 index_ =
new flann::Index<_Dist>(mat, *params_, _Dist(NearestNeighbors<_T>::distFun_));
277 index_->buildIndex();
282 void rebuildIndex(
unsigned int capacity = 0)
286 std::vector<_T> data;
290 data_.reserve(capacity);
297 std::vector<_T> data_;
300 flann::Index<_Dist>* index_;
304 boost::shared_ptr<flann::IndexParams> params_;
307 mutable flann::SearchParams searchParams_;
312 unsigned int dimension_;
316 void NearestNeighborsFLANN<double, flann::L2<double> >::createIndex(
317 const flann::Matrix<double>& mat)
319 index_ =
new flann::Index<flann::L2<double> >(mat, *params_);
320 index_->buildIndex();
323 template<
typename _T,
typename _Dist = FLANNDistance<_T> >
324 class NearestNeighborsFLANNLinear :
public NearestNeighborsFLANN<_T, _Dist>
327 NearestNeighborsFLANNLinear()
328 : NearestNeighborsFLANN<_T, _Dist>(
329 boost::shared_ptr<flann::LinearIndexParams>(
330 new flann::LinearIndexParams()))
335 template<
typename _T,
typename _Dist = FLANNDistance<_T> >
336 class NearestNeighborsFLANNHierarchicalClustering :
public NearestNeighborsFLANN<_T, _Dist>
339 NearestNeighborsFLANNHierarchicalClustering()
340 : NearestNeighborsFLANN<_T, _Dist>(
341 boost::shared_ptr<flann::HierarchicalClusteringIndexParams>(
342 new flann::HierarchicalClusteringIndexParams()))