Point Cloud Library (PCL)  1.9.1-dev
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 <cstdio>
43 
44 #include <flann/flann.hpp>
45 
46 #include <pcl/kdtree/kdtree_flann.h>
47 #include <pcl/console/print.h>
48 
49 ///////////////////////////////////////////////////////////////////////////////////////////
50 template <typename PointT, typename Dist>
52  : pcl::KdTree<PointT> (sorted)
53  , flann_index_ ()
54  , identity_mapping_ (false)
55  , dim_ (0), total_nr_points_ (0)
56  , param_k_ (::flann::SearchParams (-1 , epsilon_))
57  , param_radius_ (::flann::SearchParams (-1, epsilon_, sorted))
58 {
59 }
60 
61 ///////////////////////////////////////////////////////////////////////////////////////////
62 template <typename PointT, typename Dist>
64  : pcl::KdTree<PointT> (false)
65  , flann_index_ ()
66  , identity_mapping_ (false)
67  , dim_ (0), total_nr_points_ (0)
68  , param_k_ (::flann::SearchParams (-1 , epsilon_))
69  , param_radius_ (::flann::SearchParams (-1, epsilon_, false))
70 {
71  *this = k;
72 }
73 
74 ///////////////////////////////////////////////////////////////////////////////////////////
75 template <typename PointT, typename Dist> void
77 {
78  epsilon_ = eps;
79  param_k_ = ::flann::SearchParams (-1 , epsilon_);
80  param_radius_ = ::flann::SearchParams (-1 , epsilon_, sorted_);
81 }
82 
83 ///////////////////////////////////////////////////////////////////////////////////////////
84 template <typename PointT, typename Dist> void
86 {
87  sorted_ = sorted;
88  param_k_ = ::flann::SearchParams (-1, epsilon_);
89  param_radius_ = ::flann::SearchParams (-1, epsilon_, sorted_);
90 }
91 
92 ///////////////////////////////////////////////////////////////////////////////////////////
93 template <typename PointT, typename Dist> void
95 {
96  cleanup (); // Perform an automatic cleanup of structures
97 
98  epsilon_ = 0.0f; // default error bound value
99  dim_ = point_representation_->getNumberOfDimensions (); // Number of dimensions - default is 3 = xyz
100 
101  input_ = cloud;
102  indices_ = indices;
103 
104  // Allocate enough data
105  if (!input_)
106  {
107  PCL_ERROR ("[pcl::KdTreeFLANN::setInputCloud] Invalid input!\n");
108  return;
109  }
110  if (indices != nullptr)
111  {
112  convertCloudToArray (*input_, *indices_);
113  }
114  else
115  {
116  convertCloudToArray (*input_);
117  }
118  total_nr_points_ = static_cast<int> (index_mapping_.size ());
119  if (total_nr_points_ == 0)
120  {
121  PCL_ERROR ("[pcl::KdTreeFLANN::setInputCloud] Cannot create a KDTree with an empty input cloud!\n");
122  return;
123  }
124 
125  flann_index_.reset (new FLANNIndex (::flann::Matrix<float> (cloud_.get (),
126  index_mapping_.size (),
127  dim_),
128  ::flann::KDTreeSingleIndexParams (15))); // max 15 points/leaf
129  flann_index_->buildIndex ();
130 }
131 
132 ///////////////////////////////////////////////////////////////////////////////////////////
133 template <typename PointT, typename Dist> int
135  std::vector<int> &k_indices,
136  std::vector<float> &k_distances) const
137 {
138  assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
139 
140  if (k > total_nr_points_)
141  k = total_nr_points_;
142 
143  k_indices.resize (k);
144  k_distances.resize (k);
145 
146  std::vector<float> query (dim_);
147  point_representation_->vectorize (static_cast<PointT> (point), query);
148 
149  ::flann::Matrix<int> k_indices_mat (&k_indices[0], 1, k);
150  ::flann::Matrix<float> k_distances_mat (&k_distances[0], 1, k);
151  // Wrap the k_indices and k_distances vectors (no data copy)
152  flann_index_->knnSearch (::flann::Matrix<float> (&query[0], 1, dim_),
153  k_indices_mat, k_distances_mat,
154  k, param_k_);
155 
156  // Do mapping to original point cloud
157  if (!identity_mapping_)
158  {
159  for (size_t i = 0; i < static_cast<size_t> (k); ++i)
160  {
161  int& neighbor_index = k_indices[i];
162  neighbor_index = index_mapping_[neighbor_index];
163  }
164  }
165 
166  return (k);
167 }
168 
169 ///////////////////////////////////////////////////////////////////////////////////////////
170 template <typename PointT, typename Dist> int
171 pcl::KdTreeFLANN<PointT, Dist>::radiusSearch (const PointT &point, double radius, std::vector<int> &k_indices,
172  std::vector<float> &k_sqr_dists, unsigned int max_nn) const
173 {
174  assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!");
175 
176  std::vector<float> query (dim_);
177  point_representation_->vectorize (static_cast<PointT> (point), query);
178 
179  // Has max_nn been set properly?
180  if (max_nn == 0 || max_nn > static_cast<unsigned int> (total_nr_points_))
181  max_nn = total_nr_points_;
182 
183  std::vector<std::vector<int> > indices(1);
184  std::vector<std::vector<float> > dists(1);
185 
186  ::flann::SearchParams params (param_radius_);
187  if (max_nn == static_cast<unsigned int>(total_nr_points_))
188  params.max_neighbors = -1; // return all neighbors in radius
189  else
190  params.max_neighbors = max_nn;
191 
192  int neighbors_in_radius = flann_index_->radiusSearch (::flann::Matrix<float> (&query[0], 1, dim_),
193  indices,
194  dists,
195  static_cast<float> (radius * radius),
196  params);
197 
198  k_indices = indices[0];
199  k_sqr_dists = dists[0];
200 
201  // Do mapping to original point cloud
202  if (!identity_mapping_)
203  {
204  for (int i = 0; i < neighbors_in_radius; ++i)
205  {
206  int& neighbor_index = k_indices[i];
207  neighbor_index = index_mapping_[neighbor_index];
208  }
209  }
210 
211  return (neighbors_in_radius);
212 }
213 
214 ///////////////////////////////////////////////////////////////////////////////////////////
215 template <typename PointT, typename Dist> void
217 {
218  // Data array cleanup
219  index_mapping_.clear ();
220 
221  if (indices_)
222  indices_.reset ();
223 }
224 
225 ///////////////////////////////////////////////////////////////////////////////////////////
226 template <typename PointT, typename Dist> void
228 {
229  // No point in doing anything if the array is empty
230  if (cloud.points.empty ())
231  {
232  cloud_.reset ();
233  return;
234  }
235 
236  int original_no_of_points = static_cast<int> (cloud.points.size ());
237 
238  cloud_.reset (new float[original_no_of_points * dim_]);
239  float* cloud_ptr = cloud_.get ();
240  index_mapping_.reserve (original_no_of_points);
241  identity_mapping_ = true;
242 
243  for (int cloud_index = 0; cloud_index < original_no_of_points; ++cloud_index)
244  {
245  // Check if the point is invalid
246  if (!point_representation_->isValid (cloud.points[cloud_index]))
247  {
248  identity_mapping_ = false;
249  continue;
250  }
251 
252  index_mapping_.push_back (cloud_index);
253 
254  point_representation_->vectorize (cloud.points[cloud_index], cloud_ptr);
255  cloud_ptr += dim_;
256  }
257 }
258 
259 ///////////////////////////////////////////////////////////////////////////////////////////
260 template <typename PointT, typename Dist> void
261 pcl::KdTreeFLANN<PointT, Dist>::convertCloudToArray (const PointCloud &cloud, const std::vector<int> &indices)
262 {
263  // No point in doing anything if the array is empty
264  if (cloud.points.empty ())
265  {
266  cloud_.reset ();
267  return;
268  }
269 
270  int original_no_of_points = static_cast<int> (indices.size ());
271 
272  cloud_.reset (new float[original_no_of_points * dim_]);
273  float* cloud_ptr = cloud_.get ();
274  index_mapping_.reserve (original_no_of_points);
275  // its a subcloud -> false
276  // true only identity:
277  // - indices size equals cloud size
278  // - indices only contain values between 0 and cloud.size - 1
279  // - no index is multiple times in the list
280  // => index is complete
281  // But we can not guarantee that => identity_mapping_ = false
282  identity_mapping_ = false;
283 
284  for (const int &index : indices)
285  {
286  // Check if the point is invalid
287  if (!point_representation_->isValid (cloud.points[index]))
288  continue;
289 
290  // map from 0 - N -> indices [0] - indices [N]
291  index_mapping_.push_back (index); // If the returned index should be for the indices vector
292 
293  point_representation_->vectorize (cloud.points[index], cloud_ptr);
294  cloud_ptr += dim_;
295  }
296 }
297 
298 #define PCL_INSTANTIATE_KdTreeFLANN(T) template class PCL_EXPORTS pcl::KdTreeFLANN<T>;
299 
300 #endif //#ifndef _PCL_KDTREE_KDTREE_IMPL_FLANN_H_
301 
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
Definition: kdtree_flann.h:67
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:423
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
void setEpsilon(float eps) override
Set the search epsilon precision (error bound) for nearest neighbors searches.
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.
int radiusSearch(const PointT &point, double radius, std::vector< int > &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.
boost::shared_ptr< const PointCloud > PointCloudConstPtr
Definition: kdtree.h:63
boost::shared_ptr< const std::vector< int > > IndicesConstPtr
Definition: kdtree.h:59
int nearestKSearch(const PointT &point, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const override
Search for k-nearest neighbors for the given query point.
A point structure representing Euclidean xyz coordinates, and the RGB color.
KdTree represents the base spatial locator class for kd-tree implementations.
Definition: kdtree.h:55