Point Cloud Library (PCL) 1.12.0
flann_search.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, Willow Garage, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of the copyright holder(s) nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 * $Id$
37 *
38 */
39
40#ifndef PCL_SEARCH_IMPL_FLANN_SEARCH_H_
41#define PCL_SEARCH_IMPL_FLANN_SEARCH_H_
42
43#include <flann/algorithms/kdtree_index.h>
44#include <flann/algorithms/kdtree_single_index.h>
45#include <flann/algorithms/kmeans_index.h>
46
47#include <pcl/search/flann_search.h>
48#include <pcl/kdtree/kdtree_flann.h> // for radius_search, knn_search
49// @TODO: remove once constexpr makes it easy to have the function in the header only
50#include <pcl/kdtree/impl/kdtree_flann.hpp>
51
52//////////////////////////////////////////////////////////////////////////////////////////////
53template <typename PointT, typename FlannDistance>
56{
57 return (IndexPtr (new flann::KDTreeSingleIndex<FlannDistance> (*data,flann::KDTreeSingleIndexParams (max_leaf_size_))));
58}
59
60//////////////////////////////////////////////////////////////////////////////////////////////
61template <typename PointT, typename FlannDistance>
64{
65 return (IndexPtr (new flann::KMeansIndex<FlannDistance> (*data,flann::KMeansIndexParams ())));
66}
67
68//////////////////////////////////////////////////////////////////////////////////////////////
69template <typename PointT, typename FlannDistance>
72{
73 return (IndexPtr (new flann::KDTreeIndex<FlannDistance> (*data, flann::KDTreeIndexParams (trees_))));
74}
75
76//////////////////////////////////////////////////////////////////////////////////////////////
77template <typename PointT, typename FlannDistance>
81{
82 dim_ = point_representation_->getNumberOfDimensions ();
83}
84
85//////////////////////////////////////////////////////////////////////////////////////////////
86template <typename PointT, typename FlannDistance>
88{
89 if (input_copied_for_flann_)
90 delete [] input_flann_->ptr();
91}
92
93//////////////////////////////////////////////////////////////////////////////////////////////
94template <typename PointT, typename FlannDistance> void
96{
97 input_ = cloud;
98 indices_ = indices;
99 convertInputToFlannMatrix ();
100 index_ = creator_->createIndex (input_flann_);
101 index_->buildIndex ();
102}
103
104//////////////////////////////////////////////////////////////////////////////////////////////
105template <typename PointT, typename FlannDistance> int
106pcl::search::FlannSearch<PointT, FlannDistance>::nearestKSearch (const PointT &point, int k, Indices &indices, std::vector<float> &dists) const
107{
108 assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); // remove this check as soon as FLANN does NaN checks internally
109 bool can_cast = point_representation_->isTrivial ();
110
111 float* data = nullptr;
112 if (!can_cast)
113 {
114 data = new float [point_representation_->getNumberOfDimensions ()];
115 point_representation_->vectorize (point,data);
116 }
117
118 float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&point)): data;
119 const flann::Matrix<float> m (cdata ,1, point_representation_->getNumberOfDimensions ());
120
121 flann::SearchParams p;
122 p.eps = eps_;
123 p.sorted = sorted_results_;
124 p.checks = checks_;
125 if (indices.size() != static_cast<unsigned int> (k))
126 indices.resize (k,-1);
127 if (dists.size() != static_cast<unsigned int> (k))
128 dists.resize (k);
129 flann::Matrix<float> d (&dists[0],1,k);
130 int result = knn_search(*index_, m, indices, d, k, p);
131
132 delete [] data;
133
134 if (!identity_mapping_)
135 {
136 for (std::size_t i = 0; i < static_cast<unsigned int> (k); ++i)
137 {
138 auto& neighbor_index = indices[i];
139 neighbor_index = index_mapping_[neighbor_index];
140 }
141 }
142 return result;
143}
144
145//////////////////////////////////////////////////////////////////////////////////////////////
146template <typename PointT, typename FlannDistance> void
148 const PointCloud& cloud, const Indices& indices, int k, std::vector<Indices>& k_indices,
149 std::vector< std::vector<float> >& k_sqr_distances) const
150{
151 if (indices.empty ())
152 {
153 k_indices.resize (cloud.size ());
154 k_sqr_distances.resize (cloud.size ());
155
156 if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
157 {
158 for (std::size_t i = 0; i < cloud.size(); i++)
159 {
160 assert (point_representation_->isValid (cloud[i]) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
161 }
162 }
163
164 bool can_cast = point_representation_->isTrivial ();
165
166 // full point cloud + trivial copy operation = no need to do any conversion/copying to the flann matrix!
167 float* data=nullptr;
168 if (!can_cast)
169 {
170 data = new float[dim_*cloud.size ()];
171 for (std::size_t i = 0; i < cloud.size (); ++i)
172 {
173 float* out = data+i*dim_;
174 point_representation_->vectorize (cloud[i],out);
175 }
176 }
177
178 // const cast is evil, but the matrix constructor won't change the data, and the
179 // search won't change the matrix
180 float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&cloud[0])): data;
181 const flann::Matrix<float> m (cdata ,cloud.size (), dim_, can_cast ? sizeof (PointT) : dim_ * sizeof (float) );
182
183 flann::SearchParams p;
184 p.sorted = sorted_results_;
185 p.eps = eps_;
186 p.checks = checks_;
187 knn_search(*index_, m, k_indices, k_sqr_distances, k, p);
188
189 delete [] data;
190 }
191 else // if indices are present, the cloud has to be copied anyway. Only copy the relevant parts of the points here.
192 {
193 k_indices.resize (indices.size ());
194 k_sqr_distances.resize (indices.size ());
195
196 if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
197 {
198 for (std::size_t i = 0; i < indices.size(); i++)
199 {
200 assert (point_representation_->isValid (cloud [indices[i]]) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
201 }
202 }
203
204 float* data=new float [dim_*indices.size ()];
205 for (std::size_t i = 0; i < indices.size (); ++i)
206 {
207 float* out = data+i*dim_;
208 point_representation_->vectorize (cloud[indices[i]],out);
209 }
210 const flann::Matrix<float> m (data ,indices.size (), point_representation_->getNumberOfDimensions ());
211
212 flann::SearchParams p;
213 p.sorted = sorted_results_;
214 p.eps = eps_;
215 p.checks = checks_;
216 knn_search(*index_, m, k_indices, k_sqr_distances, k, p);
217
218 delete[] data;
219 }
220 if (!identity_mapping_)
221 {
222 for (auto &k_index : k_indices)
223 {
224 for (auto &neighbor_index : k_index)
225 {
226 neighbor_index = index_mapping_[neighbor_index];
227 }
228 }
229 }
230}
231
232//////////////////////////////////////////////////////////////////////////////////////////////
233template <typename PointT, typename FlannDistance> int
235 Indices &indices, std::vector<float> &distances,
236 unsigned int max_nn) const
237{
238 assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!"); // remove this check as soon as FLANN does NaN checks internally
239 bool can_cast = point_representation_->isTrivial ();
240
241 float* data = nullptr;
242 if (!can_cast)
243 {
244 data = new float [point_representation_->getNumberOfDimensions ()];
245 point_representation_->vectorize (point,data);
246 }
247
248 float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&point)) : data;
249 const flann::Matrix<float> m (cdata ,1, point_representation_->getNumberOfDimensions ());
250
251 flann::SearchParams p;
252 p.sorted = sorted_results_;
253 p.eps = eps_;
254 p.max_neighbors = max_nn > 0 ? max_nn : -1;
255 p.checks = checks_;
256 std::vector<Indices> i (1);
257 std::vector<std::vector<float> > d (1);
258 int result = radius_search(*index_, m, i, d, static_cast<float>(radius * radius), p);
259
260 delete [] data;
261 indices = i [0];
262 distances = d [0];
263
264 if (!identity_mapping_)
265 {
266 for (auto &neighbor_index : indices)
267 {
268 neighbor_index = index_mapping_ [neighbor_index];
269 }
270 }
271 return result;
272}
273
274//////////////////////////////////////////////////////////////////////////////////////////////
275template <typename PointT, typename FlannDistance> void
277 const PointCloud& cloud, const Indices& indices, double radius, std::vector<Indices>& k_indices,
278 std::vector< std::vector<float> >& k_sqr_distances, unsigned int max_nn) const
279{
280 if (indices.empty ()) // full point cloud + trivial copy operation = no need to do any conversion/copying to the flann matrix!
281 {
282 k_indices.resize (cloud.size ());
283 k_sqr_distances.resize (cloud.size ());
284
285 if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
286 {
287 for (std::size_t i = 0; i < cloud.size(); i++)
288 {
289 assert (point_representation_->isValid (cloud[i]) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!");
290 }
291 }
292
293 bool can_cast = point_representation_->isTrivial ();
294
295 float* data = nullptr;
296 if (!can_cast)
297 {
298 data = new float[dim_*cloud.size ()];
299 for (std::size_t i = 0; i < cloud.size (); ++i)
300 {
301 float* out = data+i*dim_;
302 point_representation_->vectorize (cloud[i],out);
303 }
304 }
305
306 float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&cloud[0])) : data;
307 const flann::Matrix<float> m (cdata ,cloud.size (), dim_, can_cast ? sizeof (PointT) : dim_ * sizeof (float));
308
309 flann::SearchParams p;
310 p.sorted = sorted_results_;
311 p.eps = eps_;
312 p.checks = checks_;
313 // here: max_nn==0: take all neighbors. flann: max_nn==0: return no neighbors, only count them. max_nn==-1: return all neighbors
314 p.max_neighbors = max_nn != 0 ? max_nn : -1;
316 *index_, m, k_indices, k_sqr_distances, static_cast<float>(radius * radius), p);
317
318 delete [] data;
319 }
320 else // if indices are present, the cloud has to be copied anyway. Only copy the relevant parts of the points here.
321 {
322 k_indices.resize (indices.size ());
323 k_sqr_distances.resize (indices.size ());
324
325 if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
326 {
327 for (std::size_t i = 0; i < indices.size(); i++)
328 {
329 assert (point_representation_->isValid (cloud [indices[i]]) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!");
330 }
331 }
332
333 float* data = new float [dim_ * indices.size ()];
334 for (std::size_t i = 0; i < indices.size (); ++i)
335 {
336 float* out = data+i*dim_;
337 point_representation_->vectorize (cloud[indices[i]], out);
338 }
339 const flann::Matrix<float> m (data, cloud.size (), point_representation_->getNumberOfDimensions ());
340
341 flann::SearchParams p;
342 p.sorted = sorted_results_;
343 p.eps = eps_;
344 p.checks = checks_;
345 // here: max_nn==0: take all neighbors. flann: max_nn==0: return no neighbors, only count them. max_nn==-1: return all neighbors
346 p.max_neighbors = max_nn != 0 ? max_nn : -1;
348 *index_, m, k_indices, k_sqr_distances, static_cast<float>(radius * radius), p);
349
350 delete[] data;
351 }
352 if (!identity_mapping_)
353 {
354 for (auto &k_index : k_indices)
355 {
356 for (auto &neighbor_index : k_index)
357 {
358 neighbor_index = index_mapping_[neighbor_index];
359 }
360 }
361 }
362}
363
364//////////////////////////////////////////////////////////////////////////////////////////////
365template <typename PointT, typename FlannDistance> void
367{
368 std::size_t original_no_of_points = indices_ && !indices_->empty () ? indices_->size () : input_->size ();
369
370 if (input_copied_for_flann_)
371 delete input_flann_->ptr();
372 input_copied_for_flann_ = true;
373 index_mapping_.clear();
374 identity_mapping_ = true;
375
376 //cloud_ = (float*)malloc (original_no_of_points * dim_ * sizeof (float));
377 //index_mapping_.reserve(original_no_of_points);
378 //identity_mapping_ = true;
379
380 if (!indices_ || indices_->empty ())
381 {
382 // best case: all points can be passed to flann without any conversions
383 if (input_->is_dense && point_representation_->isTrivial ())
384 {
385 // const cast is evil, but flann won't change the data
386 input_flann_ = MatrixPtr (new flann::Matrix<float> (const_cast<float*>(reinterpret_cast<const float*>(&(*input_) [0])), original_no_of_points, point_representation_->getNumberOfDimensions (),sizeof (PointT)));
387 input_copied_for_flann_ = false;
388 }
389 else
390 {
391 input_flann_ = MatrixPtr (new flann::Matrix<float> (new float[original_no_of_points*point_representation_->getNumberOfDimensions ()], original_no_of_points, point_representation_->getNumberOfDimensions ()));
392 float* cloud_ptr = input_flann_->ptr();
393 for (std::size_t i = 0; i < original_no_of_points; ++i)
394 {
395 const PointT& point = (*input_)[i];
396 // Check if the point is invalid
397 if (!point_representation_->isValid (point))
398 {
399 identity_mapping_ = false;
400 continue;
401 }
402
403 index_mapping_.push_back (static_cast<index_t> (i)); // If the returned index should be for the indices vector
404
405 point_representation_->vectorize (point, cloud_ptr);
406 cloud_ptr += dim_;
407 }
408 }
409
410 }
411 else
412 {
413 input_flann_ = MatrixPtr (new flann::Matrix<float> (new float[original_no_of_points*point_representation_->getNumberOfDimensions ()], original_no_of_points, point_representation_->getNumberOfDimensions ()));
414 float* cloud_ptr = input_flann_->ptr();
415 for (std::size_t indices_index = 0; indices_index < original_no_of_points; ++indices_index)
416 {
417 index_t cloud_index = (*indices_)[indices_index];
418 const PointT& point = (*input_)[cloud_index];
419 // Check if the point is invalid
420 if (!point_representation_->isValid (point))
421 {
422 identity_mapping_ = false;
423 continue;
424 }
425
426 index_mapping_.push_back (static_cast<index_t> (indices_index)); // If the returned index should be for the indices vector
427
428 point_representation_->vectorize (point, cloud_ptr);
429 cloud_ptr += dim_;
430 }
431 }
432 if (input_copied_for_flann_)
433 input_flann_->rows = index_mapping_.size ();
434}
435
436#define PCL_INSTANTIATE_FlannSearch(T) template class PCL_EXPORTS pcl::search::FlannSearch<T>;
437
438#endif
DefaultPointRepresentation extends PointRepresentation to define default behavior for common point ty...
virtual IndexPtr createIndex(MatrixConstPtr data)
Create a FLANN Index from the input data.
IndexPtr createIndex(MatrixConstPtr data) override
Create a FLANN Index from the input data.
virtual IndexPtr createIndex(MatrixConstPtr data)
Create a FLANN Index from the input data.
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
void convertInputToFlannMatrix()
converts the input data to a format usable by FLANN
int radiusSearch(const PointT &point, double radius, Indices &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const override
Search for all the nearest neighbors of the query point in a given radius.
shared_ptr< flann::NNIndex< FlannDistance > > IndexPtr
Definition: flann_search.h:117
int checks_
Number of checks to perform for approximate NN search using the multiple randomized tree index.
Definition: flann_search.h:355
shared_ptr< const flann::Matrix< float > > MatrixConstPtr
Definition: flann_search.h:114
FlannSearch(bool sorted=true, FlannIndexCreatorPtr creator=FlannIndexCreatorPtr(new KdTreeIndexCreator()))
PointRepresentationConstPtr point_representation_
Definition: flann_search.h:359
shared_ptr< FlannIndexCreator > FlannIndexCreatorPtr
Definition: flann_search.h:141
int nearestKSearch(const PointT &point, int k, Indices &k_indices, std::vector< float > &k_sqr_distances) const override
Search for the k-nearest neighbors for the given query point.
typename Search< PointT >::PointCloudConstPtr PointCloudConstPtr
Definition: flann_search.h:111
typename Search< PointT >::PointCloud PointCloud
Definition: flann_search.h:110
FlannIndexCreatorPtr creator_
The index creator, used to (re-) create the index when the search data is passed.
Definition: flann_search.h:343
IndexPtr index_
The FLANN index.
Definition: flann_search.h:339
float eps_
Epsilon for approximate NN search.
Definition: flann_search.h:351
shared_ptr< flann::Matrix< float > > MatrixPtr
Definition: flann_search.h:113
~FlannSearch()
Destructor for FlannSearch.
Generic search class.
Definition: search.h:75
pcl::IndicesConstPtr IndicesConstPtr
Definition: search.h:85
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition: types.h:112
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
int radius_search(const FlannIndex &index, const Query &query, Indices &indices, Distances &dists, float radius, const SearchParams &params)
Comaptibility template function to allow use of various types of indices with FLANN.
int knn_search(const FlannIndex &index, const Query &query, Indices &indices, Distances &dists, unsigned int k, const SearchParams &params)
Comaptibility template function to allow use of various types of indices with FLANN.
A point structure representing Euclidean xyz coordinates, and the RGB color.