NearestNeighborsFLANN.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Mark Moll */
36 
37 #ifndef OMPL_DATASTRUCTURES_NEAREST_NEIGHBORS_FLANN_
38 #define OMPL_DATASTRUCTURES_NEAREST_NEIGHBORS_FLANN_
39 
40 #include "ompl/config.h"
41 #if OMPL_HAVE_FLANN == 0
42 #error FLANN is not available. Please use a different NearestNeighbors data structure.
43 #else
44 
45 #include "ompl/base/StateSpace.h"
46 #include "ompl/datastructures/NearestNeighbors.h"
47 #include "ompl/util/Exception.h"
48 
49 #include <flann/flann.hpp>
50 #include <utility>
51 
52 namespace ompl
53 {
57  template <typename _T>
58  class FLANNDistance
59  {
60  public:
61  using ElementType = ompl::base::State *;
62  using ResultType = double;
63 
64  FLANNDistance(const typename NearestNeighbors<_T>::DistanceFunction &distFun) : distFun_(distFun)
65  {
66  }
67 
68  template <typename Iterator1, typename Iterator2>
69  ResultType operator()(Iterator1 a, Iterator2 b, size_t /*size*/, ResultType /*worst_dist*/ = -1) const
70  {
71  return distFun_(*a, *b);
72  }
73 
74  protected:
75  const typename NearestNeighbors<_T>::DistanceFunction &distFun_;
76  };
77 
87  template <typename _T, typename _Dist = FLANNDistance<_T>>
88  class NearestNeighborsFLANN : public NearestNeighbors<_T>
89  {
90  public:
91  NearestNeighborsFLANN(std::shared_ptr<flann::IndexParams> params)
92  : index_(nullptr), params_(std::move(params)), searchParams_(32, 0., true), dimension_(1)
93  {
94  }
95 
96  ~NearestNeighborsFLANN() override
97  {
98  if (index_)
99  delete index_;
100  }
101 
102  void clear() override
103  {
104  if (index_)
105  {
106  delete index_;
107  index_ = nullptr;
108  }
109  data_.clear();
110  }
111 
112  bool reportsSortedResults() const override
113  {
114  return searchParams_.sorted;
115  }
116 
117  void setDistanceFunction(const typename NearestNeighbors<_T>::DistanceFunction &distFun) override
118  {
121  }
122 
123  void add(const _T &data) override
124  {
125  bool rebuild = index_ && (data_.size() + 1 > data_.capacity());
126 
127  if (rebuild)
128  rebuildIndex(2 * data_.capacity());
129 
130  data_.push_back(data);
131  const flann::Matrix<_T> mat(&data_.back(), 1, dimension_);
132 
133  if (index_)
134  index_->addPoints(mat, std::numeric_limits<float>::max() / size());
135  else
136  createIndex(mat);
137  }
138  void add(const std::vector<_T> &data) override
139  {
140  if (data.empty())
141  {
142  return;
143  }
144  unsigned int oldSize = data_.size();
145  unsigned int newSize = oldSize + data.size();
146  bool rebuild = index_ && (newSize > data_.capacity());
147 
148  if (rebuild)
149  rebuildIndex(std::max(2 * oldSize, newSize));
150 
151  if (index_)
152  {
153  std::copy(data.begin(), data.end(), data_.begin() + oldSize);
154  const flann::Matrix<_T> mat(&data_[oldSize], data.size(), dimension_);
155  index_->addPoints(mat, std::numeric_limits<float>::max() / size());
156  }
157  else
158  {
159  data_ = data;
160  const flann::Matrix<_T> mat(&data_[0], data_.size(), dimension_);
161  createIndex(mat);
162  }
163  }
164  bool remove(const _T &data) override
165  {
166  if (!index_)
167  return false;
168  auto &elt = const_cast<_T &>(data);
169  const flann::Matrix<_T> query(&elt, 1, dimension_);
170  std::vector<std::vector<size_t>> indices(1);
171  std::vector<std::vector<double>> dists(1);
172  index_->knnSearch(query, indices, dists, 1, searchParams_);
173  if (*index_->getPoint(indices[0][0]) == data)
174  {
175  index_->removePoint(indices[0][0]);
176  rebuildIndex();
177  return true;
178  }
179  return false;
180  }
181  _T nearest(const _T &data) const override
182  {
183  if (size())
184  {
185  auto &elt = const_cast<_T &>(data);
186  const flann::Matrix<_T> query(&elt, 1, dimension_);
187  std::vector<std::vector<size_t>> indices(1);
188  std::vector<std::vector<double>> dists(1);
189  index_->knnSearch(query, indices, dists, 1, searchParams_);
190  return *index_->getPoint(indices[0][0]);
191  }
192  throw Exception("No elements found in nearest neighbors data structure");
193  }
196  void nearestK(const _T &data, std::size_t k, std::vector<_T> &nbh) const override
197  {
198  auto &elt = const_cast<_T &>(data);
199  const flann::Matrix<_T> query(&elt, 1, dimension_);
200  std::vector<std::vector<size_t>> indices;
201  std::vector<std::vector<double>> dists;
202  k = index_ ? index_->knnSearch(query, indices, dists, k, searchParams_) : 0;
203  nbh.resize(k);
204  for (std::size_t i = 0; i < k; ++i)
205  nbh[i] = *index_->getPoint(indices[0][i]);
206  }
209  void nearestR(const _T &data, double radius, std::vector<_T> &nbh) const override
210  {
211  auto &elt = const_cast<_T &>(data);
212  flann::Matrix<_T> query(&elt, 1, dimension_);
213  std::vector<std::vector<size_t>> indices;
214  std::vector<std::vector<double>> dists;
215  int k = index_ ? index_->radiusSearch(query, indices, dists, radius, searchParams_) : 0;
216  nbh.resize(k);
217  for (int i = 0; i < k; ++i)
218  nbh[i] = *index_->getPoint(indices[0][i]);
219  }
220 
221  std::size_t size() const override
222  {
223  return index_ ? index_->size() : 0;
224  }
225 
226  void list(std::vector<_T> &data) const override
227  {
228  std::size_t sz = size();
229  if (sz == 0)
230  {
231  data.resize(0);
232  return;
233  }
234  const _T &dummy = *index_->getPoint(0);
235  int checks = searchParams_.checks;
236  searchParams_.checks = size();
237  nearestK(dummy, sz, data);
238  searchParams_.checks = checks;
239  }
240 
245  virtual void setIndexParams(const std::shared_ptr<flann::IndexParams> &params)
246  {
247  params_ = params;
248  rebuildIndex();
249  }
250 
252  virtual const std::shared_ptr<flann::IndexParams> &getIndexParams() const
253  {
254  return params_;
255  }
256 
259  virtual void setSearchParams(const flann::SearchParams &searchParams)
260  {
261  searchParams_ = searchParams;
262  }
263 
266  flann::SearchParams &getSearchParams()
267  {
268  return searchParams_;
269  }
270 
273  const flann::SearchParams &getSearchParams() const
274  {
275  return searchParams_;
276  }
277 
278  unsigned int getContainerSize() const
279  {
280  return dimension_;
281  }
282 
283  protected:
286  void createIndex(const flann::Matrix<_T> &mat)
287  {
288  index_ = new flann::Index<_Dist>(mat, *params_, _Dist(NearestNeighbors<_T>::distFun_));
289  index_->buildIndex();
290  }
291 
294  void rebuildIndex(unsigned int capacity = 0)
295  {
296  if (index_)
297  {
298  std::vector<_T> data;
299  list(data);
300  clear();
301  if (capacity != 0u)
302  data_.reserve(capacity);
303  add(data);
304  }
305  }
306 
309  std::vector<_T> data_;
310 
312  flann::Index<_Dist> *index_;
313 
316  std::shared_ptr<flann::IndexParams> params_;
317 
319  mutable flann::SearchParams searchParams_;
320 
324  unsigned int dimension_;
325  };
326 
327  template <>
328  inline void NearestNeighborsFLANN<double, flann::L2<double>>::createIndex(
329  const flann::Matrix<double> &mat)
330  {
331  index_ = new flann::Index<flann::L2<double>>(mat, *params_);
332  index_->buildIndex();
333  }
334 
335  template <typename _T, typename _Dist = FLANNDistance<_T>>
336  class NearestNeighborsFLANNLinear : public NearestNeighborsFLANN<_T, _Dist>
337  {
338  public:
340  : NearestNeighborsFLANN<_T, _Dist>(std::shared_ptr<flann::LinearIndexParams>(new flann::LinearIndexParams()))
341  {
342  }
343  };
344 
345  template <typename _T, typename _Dist = FLANNDistance<_T>>
347  {
348  public:
350  : NearestNeighborsFLANN<_T, _Dist>(std::shared_ptr<flann::HierarchicalClusteringIndexParams>(
351  new flann::HierarchicalClusteringIndexParams()))
352  {
353  }
354  };
355 }
356 #endif
357 
358 #endif
void nearestR(const _T &data, double radius, std::vector< _T > &nbh) const override
Return the nearest neighbors within distance radius in sorted order if searchParams_....
virtual void setSearchParams(const flann::SearchParams &searchParams)
Set the FLANN parameters to be used during nearest neighbor searches.
void add(const _T &data) override
Add an element to the datastructure.
Definition of an abstract state.
Definition: State.h:113
flann::SearchParams searchParams_
The parameters used to seach for nearest neighbors.
bool remove(const _T &data) override
Remove an element from the datastructure.
std::function< double(const _T &, const _T &)> DistanceFunction
The definition of a distance function.
void rebuildIndex(unsigned int capacity=0)
Rebuild the nearest neighbor data structure (necessary when changing the distance function or index p...
flann::Index< _Dist > * index_
The FLANN index (the actual index type depends on params_).
void createIndex(const flann::Matrix< _T > &mat)
Internal function to construct nearest neighbor data structure with initial elements stored in mat.
_T nearest(const _T &data) const override
Get the nearest neighbor of a point.
Abstract representation of a container that can perform nearest neighbors queries.
unsigned int dimension_
If each element has an array-like structure that is exposed to FLANN, then the dimension_ needs to be...
bool reportsSortedResults() const override
Return true if the solutions reported by this data structure are sorted, when calling nearestK / near...
void list(std::vector< _T > &data) const override
Get all the elements in the datastructure.
void clear() override
Clear the datastructure.
Wrapper class for nearest neighbor data structures in the FLANN library.
flann::SearchParams & getSearchParams()
Get the FLANN parameters used during nearest neighbor searches.
virtual void setIndexParams(const std::shared_ptr< flann::IndexParams > &params)
Set the FLANN index parameters.
std::size_t size() const override
Get the number of elements in the datastructure.
std::vector< _T > data_
vector of data stored in FLANN's index. FLANN only indexes references, so we need store the original ...
std::shared_ptr< flann::IndexParams > params_
The FLANN index parameters. This contains both the type of index and the parameters for that type.
virtual const std::shared_ptr< flann::IndexParams > & getIndexParams() const
Get the FLANN parameters used to build the current index.
void nearestK(const _T &data, std::size_t k, std::vector< _T > &nbh) const override
Return the k nearest neighbors in sorted order if searchParams_.sorted==true (the default)
Main namespace. Contains everything in this library.
virtual void setDistanceFunction(const DistanceFunction &distFun)
Set the distance function to use.