Point Cloud Library (PCL) 1.13.1
Loading...
Searching...
No Matches
kdtree_flann.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2009-2011, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 *
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the copyright holder(s) nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 */
38
39#ifndef PCL_KDTREE_KDTREE_IMPL_FLANN_H_
40#define PCL_KDTREE_KDTREE_IMPL_FLANN_H_
41
42#include <flann/flann.hpp>
43
44#include <pcl/kdtree/kdtree_flann.h>
45#include <pcl/console/print.h>
46
47///////////////////////////////////////////////////////////////////////////////////////////
48template <typename PointT, typename Dist>
50 : pcl::KdTree<PointT> (sorted)
51 , flann_index_ ()
52 , identity_mapping_ (false)
53 , dim_ (0), total_nr_points_ (0)
54 , param_k_ (::flann::SearchParams (-1 , epsilon_))
55 , param_radius_ (::flann::SearchParams (-1, epsilon_, sorted))
56{
57 if (!std::is_same<std::size_t, pcl::index_t>::value) {
58 const auto message = "FLANN is not optimized for current index type. Will incur "
59 "extra allocations and copy\n";
60 if (std::is_same<int, pcl::index_t>::value) {
61 PCL_DEBUG(message); // since this has been the default behavior till PCL 1.12
62 }
63 else {
64 PCL_WARN(message);
65 }
66 }
67}
68
69///////////////////////////////////////////////////////////////////////////////////////////
70template <typename PointT, typename Dist>
72 : pcl::KdTree<PointT> (false)
73 , flann_index_ ()
74 , identity_mapping_ (false)
75 , dim_ (0), total_nr_points_ (0)
76 , param_k_ (::flann::SearchParams (-1 , epsilon_))
77 , param_radius_ (::flann::SearchParams (-1, epsilon_, false))
78{
79 *this = k;
80}
81
82///////////////////////////////////////////////////////////////////////////////////////////
83template <typename PointT, typename Dist> void
85{
86 epsilon_ = eps;
87 param_k_ = ::flann::SearchParams (-1 , epsilon_);
88 param_radius_ = ::flann::SearchParams (-1 , epsilon_, sorted_);
89}
90
91///////////////////////////////////////////////////////////////////////////////////////////
92template <typename PointT, typename Dist> void
94{
95 sorted_ = sorted;
96 param_k_ = ::flann::SearchParams (-1, epsilon_);
97 param_radius_ = ::flann::SearchParams (-1, epsilon_, sorted_);
98}
99
100///////////////////////////////////////////////////////////////////////////////////////////
101template <typename PointT, typename Dist> void
103{
104 cleanup (); // Perform an automatic cleanup of structures
105
106 epsilon_ = 0.0f; // default error bound value
107 dim_ = point_representation_->getNumberOfDimensions (); // Number of dimensions - default is 3 = xyz
108
109 input_ = cloud;
110 indices_ = indices;
111
112 // Allocate enough data
113 if (!input_)
114 {
115 PCL_ERROR ("[pcl::KdTreeFLANN::setInputCloud] Invalid input!\n");
116 return;
117 }
118 if (indices != nullptr)
119 {
120 convertCloudToArray (*input_, *indices_);
121 }
122 else
123 {
124 convertCloudToArray (*input_);
125 }
126 total_nr_points_ = static_cast<uindex_t> (index_mapping_.size ());
127 if (total_nr_points_ == 0)
128 {
129 PCL_ERROR ("[pcl::KdTreeFLANN::setInputCloud] Cannot create a KDTree with an empty input cloud!\n");
130 return;
131 }
132
133 flann_index_.reset (new FLANNIndex (::flann::Matrix<float> (cloud_.get (),
134 index_mapping_.size (),
135 dim_),
136 ::flann::KDTreeSingleIndexParams (15))); // max 15 points/leaf
137 flann_index_->buildIndex ();
138}
139
140///////////////////////////////////////////////////////////////////////////////////////////
141namespace pcl {
142namespace detail {
143// Replace using constexpr in C++17
144template <class IndexT,
145 class A,
146 class B,
147 class C,
148 class D,
149 class F,
150 CompatWithFlann<IndexT> = true>
151int
152knn_search(A& index, B& query, C& k_indices, D& dists, unsigned int k, F& params)
153{
154 // Wrap k_indices vector (no data allocation)
155 ::flann::Matrix<index_t> k_indices_mat(&k_indices[0], 1, k);
156 return index.knnSearch(query, k_indices_mat, dists, k, params);
157}
158
159template <class IndexT,
160 class A,
161 class B,
162 class C,
163 class D,
164 class F,
165 NotCompatWithFlann<IndexT> = true>
166int
167knn_search(A& index, B& query, C& k_indices, D& dists, unsigned int k, F& params)
168{
169 std::vector<std::size_t> indices(k);
170 k_indices.resize(k);
171 // Wrap indices vector (no data allocation)
172 ::flann::Matrix<std::size_t> indices_mat(indices.data(), 1, k);
173 auto ret = index.knnSearch(query, indices_mat, dists, k, params);
174 // cast appropriately
175 std::transform(indices.cbegin(),
176 indices.cend(),
177 k_indices.begin(),
178 [](const auto& x) { return static_cast<pcl::index_t>(x); });
179 return ret;
180}
181
182template <class IndexT, class A, class B, class F, CompatWithFlann<IndexT> = true>
183int
184knn_search(A& index,
185 B& query,
186 std::vector<Indices>& k_indices,
187 std::vector<std::vector<float>>& dists,
188 unsigned int k,
189 F& params)
190{
191 return index.knnSearch(query, k_indices, dists, k, params);
193
194template <class IndexT, class A, class B, class F, NotCompatWithFlann<IndexT> = true>
195int
196knn_search(A& index,
197 B& query,
198 std::vector<Indices>& k_indices,
199 std::vector<std::vector<float>>& dists,
200 unsigned int k,
201 F& params)
202{
203 std::vector<std::vector<std::size_t>> indices;
204 // flann will resize accordingly
205 auto ret = index.knnSearch(query, indices, dists, k, params);
206
207 k_indices.resize(indices.size());
208 {
209 auto it = indices.cbegin();
210 auto jt = k_indices.begin();
211 for (; it != indices.cend(); ++it, ++jt) {
212 jt->resize(it->size());
213 std::copy(it->cbegin(), it->cend(), jt->begin());
215 }
216 return ret;
217}
218} // namespace detail
219template <class FlannIndex,
220 class Query,
221 class Indices,
222 class Distances,
223 class SearchParams>
224int
225knn_search(const FlannIndex& index,
226 const Query& query,
227 Indices& indices,
228 Distances& dists,
229 unsigned int k,
230 const SearchParams& params)
231{
232 return detail::knn_search<pcl::index_t>(index, query, indices, dists, k, params);
234} // namespace pcl
235
236template <typename PointT, typename Dist> int
238 Indices &k_indices,
239 std::vector<float> &k_distances) const
240{
241 assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
242
243 if (k > total_nr_points_)
244 k = total_nr_points_;
245
246 k_indices.resize (k);
247 k_distances.resize (k);
248
249 if (k==0)
250 return 0;
251
252 std::vector<float> query (dim_);
253 point_representation_->vectorize (static_cast<PointT> (point), query);
254
255 // Wrap the k_distances vector (no data copy)
256 ::flann::Matrix<float> k_distances_mat (k_distances.data(), 1, k);
257
258 knn_search(*flann_index_,
259 ::flann::Matrix<float>(query.data(), 1, dim_),
260 k_indices,
261 k_distances_mat,
262 k,
263 param_k_);
264
265 // Do mapping to original point cloud
266 if (!identity_mapping_)
267 {
268 for (std::size_t i = 0; i < static_cast<std::size_t> (k); ++i)
269 {
270 auto& neighbor_index = k_indices[i];
271 neighbor_index = index_mapping_[neighbor_index];
272 }
273 }
274
275 return (k);
276}
277
278///////////////////////////////////////////////////////////////////////////////////////////
279namespace pcl {
280namespace detail {
281// Replace using constexpr in C++17
282template <class IndexT,
283 class A,
284 class B,
285 class C,
286 class D,
287 class F,
288 CompatWithFlann<IndexT> = true>
289int
290radius_search(A& index, B& query, C& k_indices, D& dists, float radius, F& params)
291{
292 std::vector<pcl::Indices> indices(1);
293 int neighbors_in_radius = index.radiusSearch(query, indices, dists, radius, params);
294 k_indices = std::move(indices[0]);
295 return neighbors_in_radius;
296}
297
298template <class IndexT,
299 class A,
300 class B,
301 class C,
302 class D,
303 class F,
304 NotCompatWithFlann<IndexT> = true>
305int
306radius_search(A& index, B& query, C& k_indices, D& dists, float radius, F& params)
307{
308 std::vector<std::vector<std::size_t>> indices(1);
309 int neighbors_in_radius = index.radiusSearch(query, indices, dists, radius, params);
310 k_indices.resize(indices[0].size());
311 // cast appropriately
312 std::transform(indices[0].cbegin(),
313 indices[0].cend(),
314 k_indices.begin(),
315 [](const auto& x) { return static_cast<pcl::index_t>(x); });
316 return neighbors_in_radius;
317}
318
319template <class IndexT, class A, class B, class F, CompatWithFlann<IndexT> = true>
320int
322 B& query,
323 std::vector<Indices>& k_indices,
324 std::vector<std::vector<float>>& dists,
325 float radius,
326 F& params)
327{
328 return index.radiusSearch(query, k_indices, dists, radius, params);
329}
330
331template <class IndexT, class A, class B, class F, NotCompatWithFlann<IndexT> = true>
332int
333radius_search(A& index,
334 B& query,
335 std::vector<Indices>& k_indices,
336 std::vector<std::vector<float>>& dists,
337 float radius,
338 F& params)
339{
340 std::vector<std::vector<std::size_t>> indices;
341 // flann will resize accordingly
342 auto ret = index.radiusSearch(query, indices, dists, radius, params);
343
344 k_indices.resize(indices.size());
345 {
346 auto it = indices.cbegin();
347 auto jt = k_indices.begin();
348 for (; it != indices.cend(); ++it, ++jt) {
349 jt->resize(it->size());
350 std::copy(it->cbegin(), it->cend(), jt->begin());
351 }
352 }
353 return ret;
354}
355} // namespace detail
356template <class FlannIndex,
357 class Query,
358 class Indices,
359 class Distances,
360 class SearchParams>
361int
362radius_search(const FlannIndex& index,
363 const Query& query,
364 Indices& indices,
365 Distances& dists,
366 float radius,
367 const SearchParams& params)
368{
369 return detail::radius_search<pcl::index_t>(
370 index, query, indices, dists, radius, params);
371}
372} // namespace pcl
373
374template <typename PointT, typename Dist> int
375pcl::KdTreeFLANN<PointT, Dist>::radiusSearch (const PointT &point, double radius, Indices &k_indices,
376 std::vector<float> &k_sqr_dists, unsigned int max_nn) const
377{
378 assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!");
379
380 std::vector<float> query (dim_);
381 point_representation_->vectorize (static_cast<PointT> (point), query);
382
383 // Has max_nn been set properly?
384 if (max_nn == 0 || max_nn > total_nr_points_)
385 max_nn = total_nr_points_;
386
387 std::vector<std::vector<float> > dists(1);
388
389 ::flann::SearchParams params (param_radius_);
390 if (max_nn == total_nr_points_)
391 params.max_neighbors = -1; // return all neighbors in radius
392 else
393 params.max_neighbors = max_nn;
394
395 auto query_mat = ::flann::Matrix<float>(query.data(), 1, dim_);
396 int neighbors_in_radius = radius_search(*flann_index_,
397 query_mat,
398 k_indices,
399 dists,
400 static_cast<float>(radius * radius),
401 params);
402
403 k_sqr_dists = dists[0];
404
405 // Do mapping to original point cloud
406 if (!identity_mapping_)
407 {
408 for (int i = 0; i < neighbors_in_radius; ++i)
409 {
410 auto& neighbor_index = k_indices[i];
411 neighbor_index = index_mapping_[neighbor_index];
412 }
413 }
414
415 return (neighbors_in_radius);
416}
417
418///////////////////////////////////////////////////////////////////////////////////////////
419template <typename PointT, typename Dist> void
421{
422 // Data array cleanup
423 index_mapping_.clear ();
424
425 if (indices_)
426 indices_.reset ();
427}
428
429///////////////////////////////////////////////////////////////////////////////////////////
430template <typename PointT, typename Dist> void
432{
433 // No point in doing anything if the array is empty
434 if (cloud.empty ())
435 {
436 cloud_.reset ();
437 return;
438 }
439
440 const auto original_no_of_points = cloud.size ();
441
442 cloud_.reset (new float[original_no_of_points * dim_], std::default_delete<float[]> ());
443 float* cloud_ptr = cloud_.get ();
444 index_mapping_.reserve (original_no_of_points);
445 identity_mapping_ = true;
446
447 for (std::size_t cloud_index = 0; cloud_index < original_no_of_points; ++cloud_index)
448 {
449 // Check if the point is invalid
450 if (!point_representation_->isValid (cloud[cloud_index]))
451 {
452 identity_mapping_ = false;
453 continue;
454 }
455
456 index_mapping_.push_back (cloud_index);
457
458 point_representation_->vectorize (cloud[cloud_index], cloud_ptr);
459 cloud_ptr += dim_;
460 }
461}
462
463///////////////////////////////////////////////////////////////////////////////////////////
464template <typename PointT, typename Dist> void
466{
467 // No point in doing anything if the array is empty
468 if (cloud.empty ())
469 {
470 cloud_.reset ();
471 return;
472 }
473
474 int original_no_of_points = static_cast<int> (indices.size ());
475
476 cloud_.reset (new float[original_no_of_points * dim_], std::default_delete<float[]> ());
477 float* cloud_ptr = cloud_.get ();
478 index_mapping_.reserve (original_no_of_points);
479 // its a subcloud -> false
480 // true only identity:
481 // - indices size equals cloud size
482 // - indices only contain values between 0 and cloud.size - 1
483 // - no index is multiple times in the list
484 // => index is complete
485 // But we can not guarantee that => identity_mapping_ = false
486 identity_mapping_ = false;
487
488 for (const auto &index : indices)
489 {
490 // Check if the point is invalid
491 if (!point_representation_->isValid (cloud[index]))
492 continue;
493
494 // map from 0 - N -> indices [0] - indices [N]
495 index_mapping_.push_back (index); // If the returned index should be for the indices vector
496
497 point_representation_->vectorize (cloud[index], cloud_ptr);
498 cloud_ptr += dim_;
499 }
500}
501
502#define PCL_INSTANTIATE_KdTreeFLANN(T) template class PCL_EXPORTS pcl::KdTreeFLANN<T>;
503
504#endif //#ifndef _PCL_KDTREE_KDTREE_IMPL_FLANN_H_
505
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
void setEpsilon(float eps) override
Set the search epsilon precision (error bound) for nearest neighbors searches.
shared_ptr< const Indices > IndicesConstPtr
int nearestKSearch(const PointT &point, unsigned int k, Indices &k_indices, std::vector< float > &k_sqr_distances) const override
Search for k-nearest neighbors for the given query point.
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.
typename KdTree< PointT >::PointCloudConstPtr PointCloudConstPtr
void setSortedResults(bool sorted)
KdTreeFLANN(bool sorted=true)
Default Constructor for KdTreeFLANN.
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
KdTree represents the base spatial locator class for kd-tree implementations.
Definition kdtree.h:55
PointCloud represents the base class in PCL for storing collections of 3D points.
bool empty() const
std::size_t size() const
@ B
Definition norms.h:54
int knn_search(A &index, B &query, C &k_indices, D &dists, unsigned int k, F &params)
int radius_search(A &index, B &query, C &k_indices, D &dists, float radius, F &params)
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
Definition types.h:120
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.