Point Cloud Library (PCL) 1.15.0
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 ,
53 param_k_ (::flann::SearchParams (-1 , epsilon_))
54 , param_radius_ (::flann::SearchParams (-1, epsilon_, sorted))
55{
56 if (!std::is_same<std::size_t, pcl::index_t>::value) {
57 const auto message = "FLANN is not optimized for current index type. Will incur "
58 "extra allocations and copy\n";
59 if (std::is_same<int, pcl::index_t>::value) {
60 PCL_DEBUG(message); // since this has been the default behavior till PCL 1.12
61 }
62 else {
63 PCL_WARN(message);
64 }
65 }
66}
67
68///////////////////////////////////////////////////////////////////////////////////////////
69template <typename PointT, typename Dist>
71 : pcl::KdTree<PointT> (false)
72 , flann_index_ ()
73 ,
74 param_k_ (::flann::SearchParams (-1 , epsilon_))
75 , param_radius_ (::flann::SearchParams (-1, epsilon_, false))
76{
77 *this = k;
78}
79
80///////////////////////////////////////////////////////////////////////////////////////////
81template <typename PointT, typename Dist> void
83{
84 epsilon_ = eps;
85 param_k_ = ::flann::SearchParams (-1 , epsilon_);
86 param_radius_ = ::flann::SearchParams (-1 , epsilon_, sorted_);
87}
88
89///////////////////////////////////////////////////////////////////////////////////////////
90template <typename PointT, typename Dist> void
92{
93 sorted_ = sorted;
94 param_k_ = ::flann::SearchParams (-1, epsilon_);
95 param_radius_ = ::flann::SearchParams (-1, epsilon_, sorted_);
96}
97
98///////////////////////////////////////////////////////////////////////////////////////////
99template <typename PointT, typename Dist> void
101{
102 cleanup (); // Perform an automatic cleanup of structures
103
104 epsilon_ = 0.0f; // default error bound value
105 dim_ = point_representation_->getNumberOfDimensions (); // Number of dimensions - default is 3 = xyz
106
107 input_ = cloud;
108 indices_ = indices;
109
110 // Allocate enough data
111 if (!input_)
112 {
113 PCL_ERROR ("[pcl::KdTreeFLANN::setInputCloud] Invalid input!\n");
114 return;
115 }
116 if (indices != nullptr)
117 {
118 convertCloudToArray (*input_, *indices_);
119 }
120 else
121 {
122 convertCloudToArray (*input_);
123 }
124 total_nr_points_ = static_cast<uindex_t> (index_mapping_.size ());
125 if (total_nr_points_ == 0)
126 {
127 PCL_ERROR ("[pcl::KdTreeFLANN::setInputCloud] Cannot create a KDTree with an empty input cloud!\n");
128 return;
129 }
130
131 flann_index_.reset (new FLANNIndex (::flann::Matrix<float> (cloud_.get (),
132 index_mapping_.size (),
133 dim_),
134 ::flann::KDTreeSingleIndexParams (15))); // max 15 points/leaf
135 flann_index_->buildIndex ();
136}
137
138///////////////////////////////////////////////////////////////////////////////////////////
139namespace pcl {
140namespace detail {
141// Replace using constexpr in C++17
142template <class IndexT,
143 class A,
144 class B,
145 class C,
146 class D,
147 class F,
148 CompatWithFlann<IndexT> = true>
149int
150knn_search(A& index, B& query, C& k_indices, D& dists, unsigned int k, F& params)
151{
152 // Wrap k_indices vector (no data allocation)
153 ::flann::Matrix<index_t> k_indices_mat(&k_indices[0], 1, k);
154 return index.knnSearch(query, k_indices_mat, dists, k, params);
155}
156
157template <class IndexT,
158 class A,
159 class B,
160 class C,
161 class D,
162 class F,
163 NotCompatWithFlann<IndexT> = true>
164int
165knn_search(A& index, B& query, C& k_indices, D& dists, unsigned int k, F& params)
166{
167 std::vector<std::size_t> indices(k);
168 k_indices.resize(k);
169 // Wrap indices vector (no data allocation)
170 ::flann::Matrix<std::size_t> indices_mat(indices.data(), 1, k);
171 auto ret = index.knnSearch(query, indices_mat, dists, k, params);
172 // cast appropriately
173 std::transform(indices.cbegin(),
174 indices.cend(),
175 k_indices.begin(),
176 [](const auto& x) { return static_cast<pcl::index_t>(x); });
177 return ret;
178}
179
180template <class IndexT, class A, class B, class F, CompatWithFlann<IndexT> = true>
181int
182knn_search(A& index,
183 B& query,
184 std::vector<Indices>& k_indices,
185 std::vector<std::vector<float>>& dists,
186 unsigned int k,
187 F& params)
188{
189 return index.knnSearch(query, k_indices, dists, k, params);
190}
191
192template <class IndexT, class A, class B, class F, NotCompatWithFlann<IndexT> = true>
193int
194knn_search(A& index,
195 B& query,
196 std::vector<Indices>& k_indices,
197 std::vector<std::vector<float>>& dists,
198 unsigned int k,
199 F& params)
200{
201 std::vector<std::vector<std::size_t>> indices;
202 // flann will resize accordingly
203 auto ret = index.knnSearch(query, indices, dists, k, params);
204
205 k_indices.resize(indices.size());
206 {
207 auto it = indices.cbegin();
208 auto jt = k_indices.begin();
209 for (; it != indices.cend(); ++it, ++jt) {
210 jt->resize(it->size());
211 std::copy(it->cbegin(), it->cend(), jt->begin());
212 }
213 }
214 return ret;
215}
216} // namespace detail
217template <class FlannIndex,
218 class Query,
219 class Indices,
220 class Distances,
221 class SearchParams>
222int
223knn_search(const FlannIndex& index,
224 const Query& query,
225 Indices& indices,
226 Distances& dists,
227 unsigned int k,
228 const SearchParams& params)
229{
230 return detail::knn_search<pcl::index_t>(index, query, indices, dists, k, params);
231}
232} // namespace pcl
234template <typename PointT, typename Dist> int
236 Indices &k_indices,
237 std::vector<float> &k_distances) const
238{
239 assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
240
241 if (k > total_nr_points_)
242 k = total_nr_points_;
243
244 k_indices.resize (k);
245 k_distances.resize (k);
246
247 if (k==0)
248 return 0;
249
250 std::vector<float> query (dim_);
251 point_representation_->vectorize (static_cast<PointT> (point), query);
252
253 // Wrap the k_distances vector (no data copy)
254 ::flann::Matrix<float> k_distances_mat (k_distances.data(), 1, k);
255
256 knn_search(*flann_index_,
257 ::flann::Matrix<float>(query.data(), 1, dim_),
258 k_indices,
259 k_distances_mat,
260 k,
261 param_k_);
262
263 // Do mapping to original point cloud
264 if (!identity_mapping_)
265 {
266 for (std::size_t i = 0; i < static_cast<std::size_t> (k); ++i)
267 {
268 auto& neighbor_index = k_indices[i];
269 neighbor_index = index_mapping_[neighbor_index];
270 }
271 }
272
273 return (k);
274}
275
276///////////////////////////////////////////////////////////////////////////////////////////
277namespace pcl {
278namespace detail {
279// Replace using constexpr in C++17
280template <class IndexT,
281 class A,
282 class B,
283 class C,
284 class D,
285 class F,
286 CompatWithFlann<IndexT> = true>
287int
288radius_search(A& index, B& query, C& k_indices, D& dists, float radius, F& params)
289{
290 std::vector<pcl::Indices> indices(1);
291 int neighbors_in_radius = index.radiusSearch(query, indices, dists, radius, params);
292 k_indices = std::move(indices[0]);
293 return neighbors_in_radius;
294}
295
296template <class IndexT,
297 class A,
298 class B,
299 class C,
300 class D,
301 class F,
302 NotCompatWithFlann<IndexT> = true>
303int
304radius_search(A& index, B& query, C& k_indices, D& dists, float radius, F& params)
305{
306 std::vector<std::vector<std::size_t>> indices(1);
307 int neighbors_in_radius = index.radiusSearch(query, indices, dists, radius, params);
308 k_indices.resize(indices[0].size());
309 // cast appropriately
310 std::transform(indices[0].cbegin(),
311 indices[0].cend(),
312 k_indices.begin(),
313 [](const auto& x) { return static_cast<pcl::index_t>(x); });
314 return neighbors_in_radius;
315}
316
317template <class IndexT, class A, class B, class F, CompatWithFlann<IndexT> = true>
318int
320 B& query,
321 std::vector<Indices>& k_indices,
322 std::vector<std::vector<float>>& dists,
323 float radius,
324 F& params)
325{
326 return index.radiusSearch(query, k_indices, dists, radius, params);
327}
328
329template <class IndexT, class A, class B, class F, NotCompatWithFlann<IndexT> = true>
330int
331radius_search(A& index,
332 B& query,
333 std::vector<Indices>& k_indices,
334 std::vector<std::vector<float>>& dists,
335 float radius,
336 F& params)
337{
338 std::vector<std::vector<std::size_t>> indices;
339 // flann will resize accordingly
340 auto ret = index.radiusSearch(query, indices, dists, radius, params);
341
342 k_indices.resize(indices.size());
343 {
344 auto it = indices.cbegin();
345 auto jt = k_indices.begin();
346 for (; it != indices.cend(); ++it, ++jt) {
347 jt->resize(it->size());
348 std::copy(it->cbegin(), it->cend(), jt->begin());
349 }
350 }
351 return ret;
352}
353} // namespace detail
354template <class FlannIndex,
355 class Query,
356 class Indices,
357 class Distances,
358 class SearchParams>
359int
360radius_search(const FlannIndex& index,
361 const Query& query,
362 Indices& indices,
363 Distances& dists,
364 float radius,
365 const SearchParams& params)
366{
367 return detail::radius_search<pcl::index_t>(
368 index, query, indices, dists, radius, params);
369}
370} // namespace pcl
371
372template <typename PointT, typename Dist> int
373pcl::KdTreeFLANN<PointT, Dist>::radiusSearch (const PointT &point, double radius, Indices &k_indices,
374 std::vector<float> &k_sqr_dists, unsigned int max_nn) const
375{
376 assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!");
377
378 std::vector<float> query (dim_);
379 point_representation_->vectorize (static_cast<PointT> (point), query);
380
381 // Has max_nn been set properly?
382 if (max_nn == 0 || max_nn > total_nr_points_)
383 max_nn = total_nr_points_;
384
385 std::vector<std::vector<float> > dists(1);
386
387 ::flann::SearchParams params (param_radius_);
388 if (max_nn == total_nr_points_)
389 params.max_neighbors = -1; // return all neighbors in radius
390 else
391 params.max_neighbors = max_nn;
392
393 auto query_mat = ::flann::Matrix<float>(query.data(), 1, dim_);
394 int neighbors_in_radius = radius_search(*flann_index_,
395 query_mat,
396 k_indices,
397 dists,
398 static_cast<float>(radius * radius),
399 params);
400
401 k_sqr_dists = dists[0];
402
403 // Do mapping to original point cloud
404 if (!identity_mapping_)
405 {
406 for (int i = 0; i < neighbors_in_radius; ++i)
407 {
408 auto& neighbor_index = k_indices[i];
409 neighbor_index = index_mapping_[neighbor_index];
410 }
411 }
412
413 return (neighbors_in_radius);
414}
415
416///////////////////////////////////////////////////////////////////////////////////////////
417template <typename PointT, typename Dist> void
419{
420 // Data array cleanup
421 index_mapping_.clear ();
422
423 if (indices_)
424 indices_.reset ();
425}
426
427///////////////////////////////////////////////////////////////////////////////////////////
428template <typename PointT, typename Dist> void
430{
431 // No point in doing anything if the array is empty
432 if (cloud.empty ())
433 {
434 cloud_.reset ();
435 return;
436 }
437
438 const auto original_no_of_points = cloud.size ();
439
440 cloud_.reset (new float[original_no_of_points * dim_], std::default_delete<float[]> ());
441 float* cloud_ptr = cloud_.get ();
442 index_mapping_.reserve (original_no_of_points);
443 identity_mapping_ = true;
444
445 for (std::size_t cloud_index = 0; cloud_index < original_no_of_points; ++cloud_index)
446 {
447 // Check if the point is invalid
448 if (!point_representation_->isValid (cloud[cloud_index]))
449 {
450 identity_mapping_ = false;
451 continue;
452 }
453
454 index_mapping_.push_back (cloud_index);
455
456 point_representation_->vectorize (cloud[cloud_index], cloud_ptr);
457 cloud_ptr += dim_;
458 }
459}
460
461///////////////////////////////////////////////////////////////////////////////////////////
462template <typename PointT, typename Dist> void
464{
465 // No point in doing anything if the array is empty
466 if (cloud.empty ())
467 {
468 cloud_.reset ();
469 return;
470 }
471
472 int original_no_of_points = static_cast<int> (indices.size ());
473
474 cloud_.reset (new float[original_no_of_points * dim_], std::default_delete<float[]> ());
475 float* cloud_ptr = cloud_.get ();
476 index_mapping_.reserve (original_no_of_points);
477 // its a subcloud -> false
478 // true only identity:
479 // - indices size equals cloud size
480 // - indices only contain values between 0 and cloud.size - 1
481 // - no index is multiple times in the list
482 // => index is complete
483 // But we can not guarantee that => identity_mapping_ = false
484 identity_mapping_ = false;
485
486 for (const auto &index : indices)
487 {
488 // Check if the point is invalid
489 if (!point_representation_->isValid (cloud[index]))
490 continue;
491
492 // map from 0 - N -> indices [0] - indices [N]
493 index_mapping_.push_back (index); // If the returned index should be for the indices vector
494
495 point_representation_->vectorize (cloud[index], cloud_ptr);
496 cloud_ptr += dim_;
497 }
498}
499
500#define PCL_INSTANTIATE_KdTreeFLANN(T) template class PCL_EXPORTS pcl::KdTreeFLANN<T>;
501
502#endif //#ifndef _PCL_KDTREE_KDTREE_IMPL_FLANN_H_
503
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:56
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)
Compatibility 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)
Compatibility template function to allow use of various types of indices with FLANN.
A point structure representing Euclidean xyz coordinates, and the RGB color.