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