Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
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 (static_cast<IndexPtr> (new flann::KDTreeSingleIndex<FlannDistance> (*data,static_cast<flann::KDTreeSingleIndexParams> (max_leaf_size_))));
58}
59
60//////////////////////////////////////////////////////////////////////////////////////////////
61template <typename PointT, typename FlannDistance>
64{
65 return (static_cast<IndexPtr> (new flann::KMeansIndex<FlannDistance> (*data,flann::KMeansIndexParams ())));
66}
67
68//////////////////////////////////////////////////////////////////////////////////////////////
69template <typename PointT, typename FlannDistance>
72{
73 return (static_cast<IndexPtr> (new flann::KDTreeIndex<FlannDistance> (*data, static_cast<flann::KDTreeIndexParams> (trees_))));
74}
75
76//////////////////////////////////////////////////////////////////////////////////////////////
77template <typename PointT, typename FlannDistance>
80{
81 dim_ = point_representation_->getNumberOfDimensions ();
82}
83
84//////////////////////////////////////////////////////////////////////////////////////////////
85template <typename PointT, typename FlannDistance>
87{
88 if (input_copied_for_flann_)
89 delete [] input_flann_->ptr();
90}
91
92//////////////////////////////////////////////////////////////////////////////////////////////
93template <typename PointT, typename FlannDistance> bool
95{
96 input_ = cloud;
97 indices_ = indices;
98 convertInputToFlannMatrix ();
99 index_ = creator_->createIndex (input_flann_);
100 index_->buildIndex ();
101 return true;
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 if (static_cast<unsigned int>(k) > total_nr_points_)
122 k = total_nr_points_;
123
124 flann::SearchParams p;
125 p.eps = eps_;
126 p.sorted = sorted_results_;
127 p.checks = checks_;
128 if (indices.size() != static_cast<unsigned int> (k))
129 indices.resize (k,-1);
130 if (dists.size() != static_cast<unsigned int> (k))
131 dists.resize (k);
132 flann::Matrix<float> d (dists.data(),1,k);
133 int result = knn_search(*index_, m, indices, d, k, p);
134
135 delete [] data;
136
137 if (!identity_mapping_)
138 {
139 for (std::size_t i = 0; i < static_cast<unsigned int> (k); ++i)
140 {
141 auto& neighbor_index = indices[i];
142 neighbor_index = index_mapping_[neighbor_index];
143 }
144 }
145 return result;
146}
147
148//////////////////////////////////////////////////////////////////////////////////////////////
149template <typename PointT, typename FlannDistance> void
151 const PointCloud& cloud, const Indices& indices, int k, std::vector<Indices>& k_indices,
152 std::vector< std::vector<float> >& k_sqr_distances) const
153{
154 if (indices.empty ())
155 {
156 k_indices.resize (cloud.size ());
157 k_sqr_distances.resize (cloud.size ());
158
159 if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
160 {
161 for (std::size_t i = 0; i < cloud.size(); i++)
162 {
163 assert (point_representation_->isValid (cloud[i]) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
164 }
165 }
166
167 bool can_cast = point_representation_->isTrivial ();
168
169 // full point cloud + trivial copy operation = no need to do any conversion/copying to the flann matrix!
170 float* data=nullptr;
171 if (!can_cast)
172 {
173 data = new float[dim_*cloud.size ()];
174 for (std::size_t i = 0; i < cloud.size (); ++i)
175 {
176 float* out = data+i*dim_;
177 point_representation_->vectorize (cloud[i],out);
178 }
179 }
180
181 // const cast is evil, but the matrix constructor won't change the data, and the
182 // search won't change the matrix
183 float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&cloud[0])): data;
184 const flann::Matrix<float> m (cdata ,cloud.size (), dim_, can_cast ? sizeof (PointT) : dim_ * sizeof (float) );
185
186 if (static_cast<unsigned int>(k) > total_nr_points_)
187 k = total_nr_points_;
188
189 flann::SearchParams p;
190 p.sorted = sorted_results_;
191 p.eps = eps_;
192 p.checks = checks_;
193 knn_search(*index_, m, k_indices, k_sqr_distances, k, p);
194
195 delete [] data;
196 }
197 else // if indices are present, the cloud has to be copied anyway. Only copy the relevant parts of the points here.
198 {
199 k_indices.resize (indices.size ());
200 k_sqr_distances.resize (indices.size ());
201
202 if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
203 {
204 for (std::size_t i = 0; i < indices.size(); i++)
205 {
206 assert (point_representation_->isValid (cloud [indices[i]]) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
207 }
208 }
209
210 float* data=new float [dim_*indices.size ()];
211 for (std::size_t i = 0; i < indices.size (); ++i)
212 {
213 float* out = data+i*dim_;
214 point_representation_->vectorize (cloud[indices[i]],out);
215 }
216 const flann::Matrix<float> m (data ,indices.size (), point_representation_->getNumberOfDimensions ());
217
218 flann::SearchParams p;
219 p.sorted = sorted_results_;
220 p.eps = eps_;
221 p.checks = checks_;
222 knn_search(*index_, m, k_indices, k_sqr_distances, k, p);
223
224 delete[] data;
225 }
226 if (!identity_mapping_)
227 {
228 for (auto &k_index : k_indices)
229 {
230 for (auto &neighbor_index : k_index)
231 {
232 neighbor_index = index_mapping_[neighbor_index];
233 }
234 }
235 }
236}
237
238//////////////////////////////////////////////////////////////////////////////////////////////
239template <typename PointT, typename FlannDistance> int
241 Indices &indices, std::vector<float> &distances,
242 unsigned int max_nn) const
243{
244 assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!"); // remove this check as soon as FLANN does NaN checks internally
245 bool can_cast = point_representation_->isTrivial ();
246
247 float* data = nullptr;
248 if (!can_cast)
249 {
250 data = new float [point_representation_->getNumberOfDimensions ()];
251 point_representation_->vectorize (point,data);
252 }
253
254 float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&point)) : data;
255 const flann::Matrix<float> m (cdata ,1, point_representation_->getNumberOfDimensions ());
256
257 flann::SearchParams p;
258 p.sorted = sorted_results_;
259 p.eps = eps_;
260 p.max_neighbors = max_nn > 0 ? max_nn : -1;
261 p.checks = checks_;
262 std::vector<Indices> i (1);
263 std::vector<std::vector<float> > d (1);
264 int result = radius_search(*index_, m, i, d, static_cast<float>(radius * radius), p);
265
266 delete [] data;
267 indices = i [0];
268 distances = d [0];
269
270 if (!identity_mapping_)
271 {
272 for (auto &neighbor_index : indices)
273 {
274 neighbor_index = index_mapping_ [neighbor_index];
275 }
276 }
277 return result;
278}
279
280//////////////////////////////////////////////////////////////////////////////////////////////
281template <typename PointT, typename FlannDistance> void
283 const PointCloud& cloud, const Indices& indices, double radius, std::vector<Indices>& k_indices,
284 std::vector< std::vector<float> >& k_sqr_distances, unsigned int max_nn) const
285{
286 if (indices.empty ()) // full point cloud + trivial copy operation = no need to do any conversion/copying to the flann matrix!
287 {
288 k_indices.resize (cloud.size ());
289 k_sqr_distances.resize (cloud.size ());
290
291 if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
292 {
293 for (std::size_t i = 0; i < cloud.size(); i++)
294 {
295 assert (point_representation_->isValid (cloud[i]) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!");
296 }
297 }
298
299 bool can_cast = point_representation_->isTrivial ();
300
301 float* data = nullptr;
302 if (!can_cast)
303 {
304 data = new float[dim_*cloud.size ()];
305 for (std::size_t i = 0; i < cloud.size (); ++i)
306 {
307 float* out = data+i*dim_;
308 point_representation_->vectorize (cloud[i],out);
309 }
310 }
311
312 float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&cloud[0])) : data;
313 const flann::Matrix<float> m (cdata ,cloud.size (), dim_, can_cast ? sizeof (PointT) : dim_ * sizeof (float));
314
315 flann::SearchParams p;
316 p.sorted = sorted_results_;
317 p.eps = eps_;
318 p.checks = checks_;
319 // here: max_nn==0: take all neighbors. flann: max_nn==0: return no neighbors, only count them. max_nn==-1: return all neighbors
320 p.max_neighbors = max_nn != 0 ? max_nn : -1;
322 *index_, m, k_indices, k_sqr_distances, static_cast<float>(radius * radius), p);
323
324 delete [] data;
325 }
326 else // if indices are present, the cloud has to be copied anyway. Only copy the relevant parts of the points here.
327 {
328 k_indices.resize (indices.size ());
329 k_sqr_distances.resize (indices.size ());
330
331 if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
332 {
333 for (std::size_t i = 0; i < indices.size(); i++)
334 {
335 assert (point_representation_->isValid (cloud [indices[i]]) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!");
336 }
337 }
338
339 float* data = new float [dim_ * indices.size ()];
340 for (std::size_t i = 0; i < indices.size (); ++i)
341 {
342 float* out = data+i*dim_;
343 point_representation_->vectorize (cloud[indices[i]], out);
344 }
345 const flann::Matrix<float> m (data, cloud.size (), point_representation_->getNumberOfDimensions ());
346
347 flann::SearchParams p;
348 p.sorted = sorted_results_;
349 p.eps = eps_;
350 p.checks = checks_;
351 // here: max_nn==0: take all neighbors. flann: max_nn==0: return no neighbors, only count them. max_nn==-1: return all neighbors
352 p.max_neighbors = max_nn != 0 ? max_nn : -1;
354 *index_, m, k_indices, k_sqr_distances, static_cast<float>(radius * radius), p);
355
356 delete[] data;
357 }
358 if (!identity_mapping_)
359 {
360 for (auto &k_index : k_indices)
361 {
362 for (auto &neighbor_index : k_index)
363 {
364 neighbor_index = index_mapping_[neighbor_index];
365 }
366 }
367 }
368}
369
370//////////////////////////////////////////////////////////////////////////////////////////////
371template <typename PointT, typename FlannDistance> void
373{
374 std::size_t original_no_of_points = indices_ && !indices_->empty () ? indices_->size () : input_->size ();
375
376 if (input_copied_for_flann_)
377 delete input_flann_->ptr();
378 input_copied_for_flann_ = true;
379 index_mapping_.clear();
380 identity_mapping_ = true;
381
382 //cloud_ = (float*)malloc (original_no_of_points * dim_ * sizeof (float));
383 //index_mapping_.reserve(original_no_of_points);
384 //identity_mapping_ = true;
385
386 if (!indices_ || indices_->empty ())
387 {
388 // best case: all points can be passed to flann without any conversions
389 if (input_->is_dense && point_representation_->isTrivial ())
390 {
391 // const cast is evil, but flann won't change the data
392 input_flann_ = static_cast<MatrixPtr> (new flann::Matrix<float> (const_cast<float*>(reinterpret_cast<const float*>(&(*input_) [0])), original_no_of_points, point_representation_->getNumberOfDimensions (),sizeof (PointT)));
393 input_copied_for_flann_ = false;
394 total_nr_points_ = input_->points.size();
395 }
396 else
397 {
398 input_flann_ = static_cast<MatrixPtr> (new flann::Matrix<float> (new float[original_no_of_points*point_representation_->getNumberOfDimensions ()], original_no_of_points, point_representation_->getNumberOfDimensions ()));
399 float* cloud_ptr = input_flann_->ptr();
400 for (std::size_t i = 0; i < original_no_of_points; ++i)
401 {
402 const PointT& point = (*input_)[i];
403 // Check if the point is invalid
404 if (!point_representation_->isValid (point))
405 {
406 identity_mapping_ = false;
407 continue;
408 }
409
410 index_mapping_.push_back (static_cast<index_t> (i));
411
412 point_representation_->vectorize (point, cloud_ptr);
413 cloud_ptr += dim_;
414 }
415 total_nr_points_ = index_mapping_.size();
416 }
417
418 }
419 else
420 {
421 input_flann_ = static_cast<MatrixPtr> (new flann::Matrix<float> (new float[original_no_of_points*point_representation_->getNumberOfDimensions ()], original_no_of_points, point_representation_->getNumberOfDimensions ()));
422 float* cloud_ptr = input_flann_->ptr();
423 identity_mapping_ = false;
424 for (std::size_t indices_index = 0; indices_index < original_no_of_points; ++indices_index)
425 {
426 index_t cloud_index = (*indices_)[indices_index];
427 const PointT& point = (*input_)[cloud_index];
428 // Check if the point is invalid
429 if (!point_representation_->isValid (point))
430 {
431 continue;
432 }
433
434 index_mapping_.push_back (static_cast<index_t> (cloud_index));
435
436 point_representation_->vectorize (point, cloud_ptr);
437 cloud_ptr += dim_;
438 }
439 total_nr_points_ = index_mapping_.size();
440 }
441 if (input_copied_for_flann_)
442 input_flann_->rows = index_mapping_.size ();
443}
444
445#define PCL_INSTANTIATE_FlannSearch(T) template class PCL_EXPORTS pcl::search::FlannSearch<T>;
446
447#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 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
shared_ptr< const flann::Matrix< float > > MatrixConstPtr
FlannSearch(bool sorted=true, FlannIndexCreatorPtr creator=FlannIndexCreatorPtr(new KdTreeIndexCreator()))
~FlannSearch() override
Destructor for FlannSearch.
bool setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
PointRepresentationConstPtr point_representation_
shared_ptr< FlannIndexCreator > FlannIndexCreatorPtr
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
typename Search< PointT >::PointCloud PointCloud
FlannIndexCreatorPtr creator_
The index creator, used to (re-) create the index when the search data is passed.
IndexPtr index_
The FLANN index.
shared_ptr< flann::Matrix< float > > MatrixPtr
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.