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
51namespace 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.
const flann::SearchParams & getSearchParams() const
Get the FLANN parameters used during nearest neighbor searches.
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.
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.
flann::SearchParams & getSearchParams()
Get the FLANN parameters 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...
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.