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/util/Exception.h"
47 
48 #include <flann/flann.hpp>
49 #include <utility>
50 
51 namespace ompl
52 {
56  template <typename _T>
58  {
59  public:
60  using ElementType = _T;
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  if (data.empty()) return;
140  unsigned int oldSize = data_.size();
141  unsigned int newSize = oldSize + data.size();
142  bool rebuild = index_ && (newSize > data_.capacity());
143 
144  if (rebuild)
145  rebuildIndex(std::max(2 * oldSize, newSize));
146 
147  if (index_)
148  {
149  std::copy(data.begin(), data.end(), data_.begin() + oldSize);
150  const flann::Matrix<_T> mat(&data_[oldSize], data.size(), dimension_);
151  index_->addPoints(mat, std::numeric_limits<float>::max() / size());
152  }
153  else
154  {
155  data_ = data;
156  const flann::Matrix<_T> mat(&data_[0], data_.size(), dimension_);
157  createIndex(mat);
158  }
159  }
160  bool remove(const _T &data) override
161  {
162  if (!index_)
163  return false;
164  auto &elt = const_cast<_T &>(data);
165  const flann::Matrix<_T> query(&elt, 1, dimension_);
166  std::vector<std::vector<size_t>> indices(1);
167  std::vector<std::vector<double>> dists(1);
168  index_->knnSearch(query, indices, dists, 1, searchParams_);
169  if (*index_->getPoint(indices[0][0]) == data)
170  {
171  index_->removePoint(indices[0][0]);
172  rebuildIndex();
173  return true;
174  }
175  return false;
176  }
177  _T nearest(const _T &data) const override
178  {
179  if (size())
180  {
181  auto &elt = const_cast<_T &>(data);
182  const flann::Matrix<_T> query(&elt, 1, dimension_);
183  std::vector<std::vector<size_t>> indices(1);
184  std::vector<std::vector<double>> dists(1);
185  index_->knnSearch(query, indices, dists, 1, searchParams_);
186  return *index_->getPoint(indices[0][0]);
187  }
188  throw Exception("No elements found in nearest neighbors data structure");
189  }
192  void nearestK(const _T &data, std::size_t k, std::vector<_T> &nbh) const override
193  {
194  auto &elt = const_cast<_T &>(data);
195  const flann::Matrix<_T> query(&elt, 1, dimension_);
196  std::vector<std::vector<size_t>> indices;
197  std::vector<std::vector<double>> dists;
198  k = index_ ? index_->knnSearch(query, indices, dists, k, searchParams_) : 0;
199  nbh.resize(k);
200  for (std::size_t i = 0; i < k; ++i)
201  nbh[i] = *index_->getPoint(indices[0][i]);
202  }
205  void nearestR(const _T &data, double radius, std::vector<_T> &nbh) const override
206  {
207  auto &elt = const_cast<_T &>(data);
208  flann::Matrix<_T> query(&elt, 1, dimension_);
209  std::vector<std::vector<size_t>> indices;
210  std::vector<std::vector<double>> dists;
211  int k = index_ ? index_->radiusSearch(query, indices, dists, radius, searchParams_) : 0;
212  nbh.resize(k);
213  for (int i = 0; i < k; ++i)
214  nbh[i] = *index_->getPoint(indices[0][i]);
215  }
216 
217  std::size_t size() const override
218  {
219  return index_ ? index_->size() : 0;
220  }
221 
222  void list(std::vector<_T> &data) const override
223  {
224  std::size_t sz = size();
225  if (sz == 0)
226  {
227  data.resize(0);
228  return;
229  }
230  const _T &dummy = *index_->getPoint(0);
231  int checks = searchParams_.checks;
232  searchParams_.checks = size();
233  nearestK(dummy, sz, data);
234  searchParams_.checks = checks;
235  }
236 
241  virtual void setIndexParams(const std::shared_ptr<flann::IndexParams> &params)
242  {
243  params_ = params;
244  rebuildIndex();
245  }
246 
248  virtual const std::shared_ptr<flann::IndexParams> &getIndexParams() const
249  {
250  return params_;
251  }
252 
255  virtual void setSearchParams(const flann::SearchParams &searchParams)
256  {
257  searchParams_ = searchParams;
258  }
259 
262  flann::SearchParams &getSearchParams()
263  {
264  return searchParams_;
265  }
266 
269  const flann::SearchParams &getSearchParams() const
270  {
271  return searchParams_;
272  }
273 
274  unsigned int getContainerSize() const
275  {
276  return dimension_;
277  }
278 
279  protected:
282  void createIndex(const flann::Matrix<_T> &mat)
283  {
284  index_ = new flann::Index<_Dist>(mat, *params_, _Dist(NearestNeighbors<_T>::distFun_));
285  index_->buildIndex();
286  }
287 
290  void rebuildIndex(unsigned int capacity = 0)
291  {
292  if (index_)
293  {
294  std::vector<_T> data;
295  list(data);
296  clear();
297  if (capacity != 0u)
298  data_.reserve(capacity);
299  add(data);
300  }
301  }
302 
305  std::vector<_T> data_;
306 
308  flann::Index<_Dist> *index_;
309 
312  std::shared_ptr<flann::IndexParams> params_;
313 
315  mutable flann::SearchParams searchParams_;
316 
320  unsigned int dimension_;
321  };
322 
323  template <>
324  inline void NearestNeighborsFLANN<double, flann::L2<double>>::createIndex(
325  const flann::Matrix<double> &mat)
326  {
327  index_ = new flann::Index<flann::L2<double>>(mat, *params_);
328  index_->buildIndex();
329  }
330 
331  template <typename _T, typename _Dist = FLANNDistance<_T>>
333  {
334  public:
336  : NearestNeighborsFLANN<_T, _Dist>(std::shared_ptr<flann::LinearIndexParams>(new flann::LinearIndexParams()))
337  {
338  }
339  };
340 
341  template <typename _T, typename _Dist = FLANNDistance<_T>>
343  {
344  public:
346  : NearestNeighborsFLANN<_T, _Dist>(std::shared_ptr<flann::HierarchicalClusteringIndexParams>(
347  new flann::HierarchicalClusteringIndexParams()))
348  {
349  }
350  };
351 }
352 #endif
353 
354 #endif
The exception type for ompl.
Definition: Exception.h:47
Wrapper class to allow FLANN access to the NearestNeighbors::distFun_ callback function.
Wrapper class for nearest neighbor data structures in the FLANN library.
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_....
std::shared_ptr< flann::IndexParams > params_
The FLANN index parameters. This contains both the type of index and the parameters for that type.
bool reportsSortedResults() const override
Return true if the solutions reported by this data structure are sorted, when calling nearestK / near...
virtual const std::shared_ptr< flann::IndexParams > & getIndexParams() const
Get the FLANN parameters used to build the current index.
void add(const _T &data) override
Add an element to the datastructure.
flann::SearchParams & getSearchParams()
Get the FLANN parameters used during nearest neighbor searches.
std::vector< _T > data_
vector of data stored in FLANN's index. FLANN only indexes references, so we need store the original ...
virtual void setSearchParams(const flann::SearchParams &searchParams)
Set the FLANN parameters to be used during nearest neighbor searches.
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...
const flann::SearchParams & getSearchParams() const
Get the FLANN parameters used during nearest neighbor searches.
void rebuildIndex(unsigned int capacity=0)
Rebuild the nearest neighbor data structure (necessary when changing the distance function or index p...
virtual void setIndexParams(const std::shared_ptr< flann::IndexParams > &params)
Set the FLANN index parameters.
_T nearest(const _T &data) const override
Get the nearest neighbor of a point.
bool remove(const _T &data) override
Remove an element from the datastructure.
void createIndex(const flann::Matrix< _T > &mat)
Internal function to construct nearest neighbor data structure with initial elements stored in mat.
flann::SearchParams searchParams_
The parameters used to seach for nearest neighbors.
flann::Index< _Dist > * index_
The FLANN index (the actual index type depends on params_).
std::size_t size() const override
Get the number of elements in the datastructure.
void add(const std::vector< _T > &data) override
Add a vector of points.
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)
void clear() override
Clear the datastructure.
Abstract representation of a container that can perform nearest neighbors queries.
std::function< double(const _T &, const _T &)> DistanceFunction
The definition of a distance function.
virtual void setDistanceFunction(const DistanceFunction &distFun)
Set the distance function to use.
Main namespace. Contains everything in this library.
Definition: AppBase.h:22