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/datastructures/NearestNeighbors.h"
46 #include "ompl/base/StateSpace.h"
47 
48 #include <flann/flann.hpp>
49 #include <utility>
50 
51 namespace ompl
52 {
56  template <typename _T>
58  {
59  public:
61  using ResultType = double;
62 
63  FLANNDistance(const typename NearestNeighbors<_T>::DistanceFunction &distFun) : distFun_(distFun)
64  {
65  }
66 
67  template <typename Iterator1, typename Iterator2>
68  ResultType operator()(Iterator1 a, Iterator2 b, size_t /*size*/, ResultType /*worst_dist*/ = -1) const
69  {
70  return distFun_(*a, *b);
71  }
72 
73  protected:
74  const typename NearestNeighbors<_T>::DistanceFunction &distFun_;
75  };
76 
86  template <typename _T, typename _Dist = FLANNDistance<_T>>
88  {
89  public:
90  NearestNeighborsFLANN(std::shared_ptr<flann::IndexParams> params)
91  : index_(nullptr), params_(std::move(params)), searchParams_(32, 0., true), dimension_(1)
92  {
93  }
94 
95  ~NearestNeighborsFLANN() override
96  {
97  if (index_)
98  delete index_;
99  }
100 
101  void clear() override
102  {
103  if (index_)
104  {
105  delete index_;
106  index_ = nullptr;
107  }
108  data_.clear();
109  }
110 
111  bool reportsSortedResults() const override
112  {
113  return searchParams_.sorted;
114  }
115 
116  void setDistanceFunction(const typename NearestNeighbors<_T>::DistanceFunction &distFun) override
117  {
119  rebuildIndex();
120  }
121 
122  void add(const _T &data) override
123  {
124  bool rebuild = index_ && (data_.size() + 1 > data_.capacity());
125 
126  if (rebuild)
127  rebuildIndex(2 * data_.capacity());
128 
129  data_.push_back(data);
130  const flann::Matrix<_T> mat(&data_.back(), 1, dimension_);
131 
132  if (index_)
133  index_->addPoints(mat, std::numeric_limits<float>::max() / size());
134  else
135  createIndex(mat);
136  }
137  void add(const std::vector<_T> &data) override
138  {
139  unsigned int oldSize = data_.size();
140  unsigned int newSize = oldSize + data.size();
141  bool rebuild = index_ && (newSize > data_.capacity());
142 
143  if (rebuild)
144  rebuildIndex(std::max(2 * oldSize, newSize));
145 
146  if (index_)
147  {
148  std::copy(data.begin(), data.end(), data_.begin() + oldSize);
149  const flann::Matrix<_T> mat(&data_[oldSize], data.size(), dimension_);
150  index_->addPoints(mat, std::numeric_limits<float>::max() / size());
151  }
152  else
153  {
154  data_ = data;
155  const flann::Matrix<_T> mat(&data_[0], data_.size(), dimension_);
156  createIndex(mat);
157  }
158  }
159  bool remove(const _T &data) override
160  {
161  if (!index_)
162  return false;
163  _T &elt = const_cast<_T &>(data);
164  const flann::Matrix<_T> query(&elt, 1, dimension_);
165  std::vector<std::vector<size_t>> indices(1);
166  std::vector<std::vector<double>> dists(1);
167  index_->knnSearch(query, indices, dists, 1, searchParams_);
168  if (*index_->getPoint(indices[0][0]) == data)
169  {
170  index_->removePoint(indices[0][0]);
171  rebuildIndex();
172  return true;
173  }
174  return false;
175  }
176  _T nearest(const _T &data) const override
177  {
178  if (size())
179  {
180  _T &elt = const_cast<_T &>(data);
181  const flann::Matrix<_T> query(&elt, 1, dimension_);
182  std::vector<std::vector<size_t>> indices(1);
183  std::vector<std::vector<double>> dists(1);
184  index_->knnSearch(query, indices, dists, 1, searchParams_);
185  return *index_->getPoint(indices[0][0]);
186  }
187  throw Exception("No elements found in nearest neighbors data structure");
188  }
191  void nearestK(const _T &data, std::size_t k, std::vector<_T> &nbh) const override
192  {
193  _T &elt = const_cast<_T &>(data);
194  const flann::Matrix<_T> query(&elt, 1, dimension_);
195  std::vector<std::vector<size_t>> indices;
196  std::vector<std::vector<double>> dists;
197  k = index_ ? index_->knnSearch(query, indices, dists, k, searchParams_) : 0;
198  nbh.resize(k);
199  for (std::size_t i = 0; i < k; ++i)
200  nbh[i] = *index_->getPoint(indices[0][i]);
201  }
204  void nearestR(const _T &data, double radius, std::vector<_T> &nbh) const override
205  {
206  _T &elt = const_cast<_T &>(data);
207  flann::Matrix<_T> query(&elt, 1, dimension_);
208  std::vector<std::vector<size_t>> indices;
209  std::vector<std::vector<double>> dists;
210  int k = index_ ? index_->radiusSearch(query, indices, dists, radius, searchParams_) : 0;
211  nbh.resize(k);
212  for (int i = 0; i < k; ++i)
213  nbh[i] = *index_->getPoint(indices[0][i]);
214  }
215 
216  std::size_t size() const override
217  {
218  return index_ ? index_->size() : 0;
219  }
220 
221  void list(std::vector<_T> &data) const override
222  {
223  std::size_t sz = size();
224  if (sz == 0)
225  {
226  data.resize(0);
227  return;
228  }
229  const _T &dummy = *index_->getPoint(0);
230  int checks = searchParams_.checks;
231  searchParams_.checks = size();
232  nearestK(dummy, sz, data);
233  searchParams_.checks = checks;
234  }
235 
240  virtual void setIndexParams(const std::shared_ptr<flann::IndexParams> &params)
241  {
242  params_ = params;
243  rebuildIndex();
244  }
245 
247  virtual const std::shared_ptr<flann::IndexParams> &getIndexParams() const
248  {
249  return params_;
250  }
251 
254  virtual void setSearchParams(const flann::SearchParams &searchParams)
255  {
256  searchParams_ = searchParams;
257  }
258 
261  flann::SearchParams &getSearchParams()
262  {
263  return searchParams_;
264  }
265 
268  const flann::SearchParams &getSearchParams() const
269  {
270  return searchParams_;
271  }
272 
273  unsigned int getContainerSize() const
274  {
275  return dimension_;
276  }
277 
278  protected:
281  void createIndex(const flann::Matrix<_T> &mat)
282  {
283  index_ = new flann::Index<_Dist>(mat, *params_, _Dist(NearestNeighbors<_T>::distFun_));
284  index_->buildIndex();
285  }
286 
289  void rebuildIndex(unsigned int capacity = 0)
290  {
291  if (index_)
292  {
293  std::vector<_T> data;
294  list(data);
295  clear();
296  if (capacity)
297  data_.reserve(capacity);
298  add(data);
299  }
300  }
301 
304  std::vector<_T> data_;
305 
307  flann::Index<_Dist> *index_;
308 
311  std::shared_ptr<flann::IndexParams> params_;
312 
314  mutable flann::SearchParams searchParams_;
315 
319  unsigned int dimension_;
320  };
321 
322  template <>
323  void NearestNeighborsFLANN<double, flann::L2<double>>::createIndex(const flann::Matrix<double> &mat)
324  {
325  index_ = new flann::Index<flann::L2<double>>(mat, *params_);
326  index_->buildIndex();
327  }
328 
329  template <typename _T, typename _Dist = FLANNDistance<_T>>
331  {
332  public:
334  : NearestNeighborsFLANN<_T, _Dist>(std::shared_ptr<flann::LinearIndexParams>(new flann::LinearIndexParams()))
335  {
336  }
337  };
338 
339  template <typename _T, typename _Dist = FLANNDistance<_T>>
341  {
342  public:
344  : NearestNeighborsFLANN<_T, _Dist>(std::shared_ptr<flann::HierarchicalClusteringIndexParams>(
345  new flann::HierarchicalClusteringIndexParams()))
346  {
347  }
348  };
349 }
350 #endif
351 
352 #endif
void list(std::vector< _T > &data) const override
Get all the elements in the datastructure.
unsigned int dimension_
If each element has an array-like structure that is exposed to FLANN, then the dimension_ needs to be...
Wrapper class to allow FLANN access to the NearestNeighbors::distFun_ callback function.
bool reportsSortedResults() const override
Return true if the solutions reported by this data structure are sorted, when calling nearestK / near...
std::size_t size() const override
Get the number of elements in the datastructure.
flann::SearchParams & getSearchParams()
Get the FLANN parameters used during nearest neighbor searches.
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.
Definition: AppBase.h:21
void add(const _T &data) override
Add an element to the datastructure.
void add(const std::vector< _T > &data) override
Add a vector of points.
std::vector< _T > data_
vector of data stored in FLANN&#39;s index. FLANN only indexes references, so we need store the original ...
virtual void setIndexParams(const std::shared_ptr< flann::IndexParams > &params)
Set the FLANN index parameters.
virtual const std::shared_ptr< flann::IndexParams > & getIndexParams() const
Get the FLANN parameters used to build the current index.
Wrapper class for nearest neighbor data structures in the FLANN library.
virtual void setDistanceFunction(const DistanceFunction &distFun)
Set the distance function to use.
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_.sorted==true (the default)
Definition of an abstract state.
Definition: State.h:49
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.
Definition: Exception.h:46
std::shared_ptr< flann::IndexParams > params_
The FLANN index parameters. This contains both the type of index and the parameters for that type...
void clear() override
Clear the datastructure.
const flann::SearchParams & getSearchParams() const
Get the FLANN parameters used during nearest neighbor searches.
_T nearest(const _T &data) const override
Get the nearest neighbor of a point.
std::function< double(const _T &, const _T &)> DistanceFunction
The definition of a distance function.
virtual void setSearchParams(const flann::SearchParams &searchParams)
Set the FLANN parameters to be used during nearest neighbor searches.