All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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 
50 namespace ompl
51 {
55  template<typename _T>
56  class FLANNDistance
57  {
58  public:
59  typedef _T ElementType;
60  typedef double ResultType;
61 
62  FLANNDistance(const typename NearestNeighbors<_T>::DistanceFunction& distFun)
63  : distFun_(distFun)
64  {
65  }
66 
67  template <typename Iterator1, typename Iterator2>
68  ResultType operator()(Iterator1 a, Iterator2 b,
69  size_t /*size*/, ResultType /*worst_dist*/ = -1) const
70  {
71  return distFun_(*a, *b);
72  }
73  protected:
74  const typename NearestNeighbors<_T>::DistanceFunction& distFun_;
75  };
76 
86  template<typename _T, typename _Dist = FLANNDistance<_T> >
87  class NearestNeighborsFLANN : public NearestNeighbors<_T>
88  {
89  public:
90 
91  NearestNeighborsFLANN(const boost::shared_ptr<flann::IndexParams> &params)
92  : index_(0), params_(params), searchParams_(32, 0., true), dimension_(1)
93  {
94  }
95 
96  virtual ~NearestNeighborsFLANN(void)
97  {
98  if (index_)
99  delete index_;
100  }
101 
102  virtual void clear(void)
103  {
104  if (index_)
105  {
106  delete index_;
107  index_ = NULL;
108  }
109  data_.clear();
110  }
111 
112  virtual void setDistanceFunction(const typename NearestNeighbors<_T>::DistanceFunction &distFun)
113  {
115  rebuildIndex();
116  }
117 
118  virtual void add(const _T &data)
119  {
120  bool rebuild = index_ && (data_.size() + 1 > data_.capacity());
121 
122  if (rebuild)
123  rebuildIndex(2 * data_.capacity());
124 
125  data_.push_back(data);
126  const flann::Matrix<_T> mat(&data_.back(), 1, dimension_);
127 
128  if (index_)
129  index_->addPoints(mat, std::numeric_limits<float>::max()/size());
130  else
131  createIndex(mat);
132  }
133  virtual void add(const std::vector<_T> &data)
134  {
135  unsigned int oldSize = data_.size();
136  unsigned int newSize = oldSize + data.size();
137  bool rebuild = index_ && (newSize > data_.capacity());
138 
139  if (rebuild)
140  rebuildIndex(std::max(2 * oldSize, newSize));
141 
142  if (index_)
143  {
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());
147  }
148  else
149  {
150  data_ = data;
151  const flann::Matrix<_T> mat(&data_[0], data_.size(), dimension_);
152  createIndex(mat);
153  }
154  }
155  virtual bool remove(const _T& data)
156  {
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)
164  {
165  index_->removePoint(indices[0][0]);
166  rebuildIndex();
167  return true;
168  }
169  return false;
170  }
171  virtual _T nearest(const _T &data) const
172  {
173  if (size())
174  {
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]);
181  }
182  throw Exception("No elements found in nearest neighbors data structure");
183  }
184  virtual void nearestK(const _T &data, std::size_t k, std::vector<_T> &nbh) const
185  {
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;
191  nbh.resize(k);
192  for (std::size_t i = 0 ; i < k ; ++i)
193  nbh[i] = *index_->getPoint(indices[0][i]);
194  }
195 
196  virtual void nearestR(const _T &data, double radius, std::vector<_T> &nbh) const
197  {
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;
203  nbh.resize(k);
204  for (int i = 0 ; i < k ; ++i)
205  nbh[i] = *index_->getPoint(indices[0][i]);
206  }
207 
208  virtual std::size_t size(void) const
209  {
210  return index_ ? index_->size() : 0;
211  }
212 
213  virtual void list(std::vector<_T> &data) const
214  {
215  std::size_t sz = size();
216  if (sz == 0)
217  {
218  data.resize(0);
219  return;
220  }
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;
226  }
227 
232  virtual void setIndexParams(const boost::shared_ptr<flann::IndexParams> &params)
233  {
234  params_ = params;
235  rebuildIndex();
236  }
237 
239  virtual const boost::shared_ptr<flann::IndexParams>& getIndexParams(void) const
240  {
241  return params_;
242  }
243 
246  virtual void setSearchParams(const flann::SearchParams& searchParams)
247  {
248  searchParams_ = searchParams;
249  }
250 
253  flann::SearchParams& getSearchParams(void)
254  {
255  return searchParams_;
256  }
257 
260  const flann::SearchParams& getSearchParams(void) const
261  {
262  return searchParams_;
263  }
264 
265  unsigned int getContainerSize(void) const
266  {
267  return dimension_;
268  }
269 
270  protected:
271 
274  void createIndex(const flann::Matrix<_T>& mat)
275  {
276  index_ = new flann::Index<_Dist>(mat, *params_, _Dist(NearestNeighbors<_T>::distFun_));
277  index_->buildIndex();
278  }
279 
282  void rebuildIndex(unsigned int capacity = 0)
283  {
284  if (index_)
285  {
286  std::vector<_T> data;
287  list(data);
288  clear();
289  if (capacity)
290  data_.reserve(capacity);
291  add(data);
292  }
293  }
294 
297  std::vector<_T> data_;
298 
300  flann::Index<_Dist>* index_;
301 
304  boost::shared_ptr<flann::IndexParams> params_;
305 
307  mutable flann::SearchParams searchParams_;
308 
312  unsigned int dimension_;
313  };
314 
315  template<>
316  void NearestNeighborsFLANN<double, flann::L2<double> >::createIndex(
317  const flann::Matrix<double>& mat)
318  {
319  index_ = new flann::Index<flann::L2<double> >(mat, *params_);
320  index_->buildIndex();
321  }
322 
323  template<typename _T, typename _Dist = FLANNDistance<_T> >
324  class NearestNeighborsFLANNLinear : public NearestNeighborsFLANN<_T, _Dist>
325  {
326  public:
327  NearestNeighborsFLANNLinear()
328  : NearestNeighborsFLANN<_T, _Dist>(
329  boost::shared_ptr<flann::LinearIndexParams>(
330  new flann::LinearIndexParams()))
331  {
332  }
333  };
334 
335  template<typename _T, typename _Dist = FLANNDistance<_T> >
336  class NearestNeighborsFLANNHierarchicalClustering : public NearestNeighborsFLANN<_T, _Dist>
337  {
338  public:
339  NearestNeighborsFLANNHierarchicalClustering()
340  : NearestNeighborsFLANN<_T, _Dist>(
341  boost::shared_ptr<flann::HierarchicalClusteringIndexParams>(
342  new flann::HierarchicalClusteringIndexParams()))
343  {
344  }
345  };
346 
347 }
348 #endif
349 
350 #endif