Point Cloud Library (PCL)  1.14.1-dev
brute_force.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  */
37 
38 #pragma once
39 
40 #include <pcl/common/point_tests.h> // for pcl::isFinite
41 #include <pcl/search/brute_force.h>
42 #include <queue>
43 
44 //////////////////////////////////////////////////////////////////////////////////////////////
45 template <typename PointT> float
47  const PointT& point1, const PointT& point2) const
48 {
49  return (point1.getVector3fMap () - point2.getVector3fMap ()).squaredNorm ();
50 }
51 
52 //////////////////////////////////////////////////////////////////////////////////////////////
53 template <typename PointT> int
55  const PointT& point, int k, Indices& k_indices, std::vector<float>& k_distances) const
56 {
57  assert (isFinite (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
58 
59  k_indices.clear ();
60  k_distances.clear ();
61  if (k < 1)
62  return 0;
63 
64  if (input_->is_dense)
65  return denseKSearch (point, k, k_indices, k_distances);
66  return sparseKSearch (point, k, k_indices, k_distances);
67 }
68 
69 //////////////////////////////////////////////////////////////////////////////////////////////
70 template <typename PointT> int
72  const PointT &point, int k, Indices &k_indices, std::vector<float> &k_distances) const
73 {
74  // container for first k elements -> O(1) for insertion, since order not required here
75  std::vector<Entry> result;
76  result.reserve (k);
77  std::priority_queue<Entry> queue;
78  if (indices_)
79  {
80  auto iIt = indices_->cbegin ();
81  auto iEnd = indices_->cbegin () + std::min (static_cast<unsigned> (k), static_cast<unsigned> (indices_->size ()));
82  for (; iIt != iEnd; ++iIt)
83  result.push_back (Entry (*iIt, getDistSqr ((*input_)[*iIt], point)));
84 
85  queue = std::priority_queue<Entry> (result.begin (), result.end ());
86 
87  // add the rest
88  Entry entry;
89  for (; iIt != indices_->cend (); ++iIt)
90  {
91  entry.distance = getDistSqr ((*input_)[*iIt], point);
92  if (queue.top ().distance > entry.distance)
93  {
94  entry.index = *iIt;
95  queue.pop ();
96  queue.push (entry);
97  }
98  }
99  }
100  else
101  {
102  Entry entry;
103  for (entry.index = 0; entry.index < std::min<pcl::index_t> (k, input_->size ()); ++entry.index)
104  {
105  entry.distance = getDistSqr ((*input_)[entry.index], point);
106  result.push_back (entry);
107  }
108 
109  queue = std::priority_queue<Entry> (result.begin (), result.end ());
110 
111  // add the rest
112  for (; entry.index < static_cast<pcl::index_t>(input_->size ()); ++entry.index)
113  {
114  entry.distance = getDistSqr ((*input_)[entry.index], point);
115  if (queue.top ().distance > entry.distance)
116  {
117  queue.pop ();
118  queue.push (entry);
119  }
120  }
121  }
122 
123  k_indices.resize (queue.size ());
124  k_distances.resize (queue.size ());
125  std::size_t idx = queue.size () - 1;
126  while (!queue.empty ())
127  {
128  k_indices [idx] = queue.top ().index;
129  k_distances [idx] = queue.top ().distance;
130  queue.pop ();
131  --idx;
132  }
133 
134  return (static_cast<int> (k_indices.size ()));
135 }
136 
137 //////////////////////////////////////////////////////////////////////////////////////////////
138 template <typename PointT> int
140  const PointT &point, int k, Indices &k_indices, std::vector<float> &k_distances) const
141 {
142  // result used to collect the first k neighbors -> unordered
143  std::vector<Entry> result;
144  result.reserve (k);
145 
146  std::priority_queue<Entry> queue;
147  if (indices_)
148  {
149  auto iIt =indices_->cbegin ();
150  for (; iIt != indices_->cend () && result.size () < static_cast<unsigned> (k); ++iIt)
151  {
152  if (std::isfinite ((*input_)[*iIt].x))
153  result.push_back (Entry (*iIt, getDistSqr ((*input_)[*iIt], point)));
154  }
155 
156  queue = std::priority_queue<Entry> (result.begin (), result.end ());
157 
158  // either we have k elements, or there are none left to iterate >in either case we're fine
159  // add the rest
160  Entry entry;
161  for (; iIt != indices_->cend (); ++iIt)
162  {
163  if (!std::isfinite ((*input_)[*iIt].x))
164  continue;
165 
166  entry.distance = getDistSqr ((*input_)[*iIt], point);
167  if (queue.top ().distance > entry.distance)
168  {
169  entry.index = *iIt;
170  queue.pop ();
171  queue.push (entry);
172  }
173  }
174  }
175  else
176  {
177  Entry entry;
178  for (entry.index = 0; (entry.index < static_cast<pcl::index_t>(input_->size ())) && (result.size () < static_cast<std::size_t> (k)); ++entry.index)
179  {
180  if (std::isfinite ((*input_)[entry.index].x))
181  {
182  entry.distance = getDistSqr ((*input_)[entry.index], point);
183  result.push_back (entry);
184  }
185  }
186  queue = std::priority_queue<Entry> (result.begin (), result.end ());
187 
188  // add the rest
189  for (; entry.index < static_cast<pcl::index_t>(input_->size ()); ++entry.index)
190  {
191  if (!std::isfinite ((*input_)[entry.index].x))
192  continue;
193 
194  entry.distance = getDistSqr ((*input_)[entry.index], point);
195  if (queue.top ().distance > entry.distance)
196  {
197  queue.pop ();
198  queue.push (entry);
199  }
200  }
201  }
202 
203  k_indices.resize (queue.size ());
204  k_distances.resize (queue.size ());
205  std::size_t idx = queue.size () - 1;
206  while (!queue.empty ())
207  {
208  k_indices [idx] = queue.top ().index;
209  k_distances [idx] = queue.top ().distance;
210  queue.pop ();
211  --idx;
212  }
213  return (static_cast<int> (k_indices.size ()));
214 }
215 
216 //////////////////////////////////////////////////////////////////////////////////////////////
217 template <typename PointT> int
219  const PointT& point, double radius,
220  Indices &k_indices, std::vector<float> &k_sqr_distances,
221  unsigned int max_nn) const
222 {
223  radius *= radius;
224 
225  std::size_t reserve = max_nn;
226  if (reserve == 0)
227  {
228  if (indices_)
229  reserve = std::min (indices_->size (), input_->size ());
230  else
231  reserve = input_->size ();
232  }
233  k_indices.reserve (reserve);
234  k_sqr_distances.reserve (reserve);
235  float distance;
236  if (indices_)
237  {
238  for (const auto& idx : *indices_)
239  {
240  distance = getDistSqr ((*input_)[idx], point);
241  if (distance <= radius)
242  {
243  k_indices.push_back (idx);
244  k_sqr_distances.push_back (distance);
245  if (k_indices.size () == max_nn) // max_nn = 0 -> never true
246  break;
247  }
248  }
249  }
250  else
251  {
252  for (std::size_t index = 0; index < input_->size (); ++index)
253  {
254  distance = getDistSqr ((*input_)[index], point);
255  if (distance <= radius)
256  {
257  k_indices.push_back (index);
258  k_sqr_distances.push_back (distance);
259  if (k_indices.size () == max_nn) // never true if max_nn = 0
260  break;
261  }
262  }
263  }
264 
265  if (sorted_results_)
266  this->sortResults (k_indices, k_sqr_distances);
267 
268  return (static_cast<int> (k_indices.size ()));
269 }
270 
271 //////////////////////////////////////////////////////////////////////////////////////////////
272 template <typename PointT> int
274  const PointT& point, double radius,
275  Indices &k_indices, std::vector<float> &k_sqr_distances,
276  unsigned int max_nn) const
277 {
278  radius *= radius;
279 
280  std::size_t reserve = max_nn;
281  if (reserve == 0)
282  {
283  if (indices_)
284  reserve = std::min (indices_->size (), input_->size ());
285  else
286  reserve = input_->size ();
287  }
288  k_indices.reserve (reserve);
289  k_sqr_distances.reserve (reserve);
290 
291  float distance;
292  if (indices_)
293  {
294  for (const auto& idx : *indices_)
295  {
296  if (!std::isfinite ((*input_)[idx].x))
297  continue;
298 
299  distance = getDistSqr ((*input_)[idx], point);
300  if (distance <= radius)
301  {
302  k_indices.push_back (idx);
303  k_sqr_distances.push_back (distance);
304  if (k_indices.size () == max_nn) // never true if max_nn = 0
305  break;
306  }
307  }
308  }
309  else
310  {
311  for (std::size_t index = 0; index < input_->size (); ++index)
312  {
313  if (!std::isfinite ((*input_)[index].x))
314  continue;
315  distance = getDistSqr ((*input_)[index], point);
316  if (distance <= radius)
317  {
318  k_indices.push_back (index);
319  k_sqr_distances.push_back (distance);
320  if (k_indices.size () == max_nn) // never true if max_nn = 0
321  break;
322  }
323  }
324  }
325 
326  if (sorted_results_)
327  this->sortResults (k_indices, k_sqr_distances);
328 
329  return (static_cast<int> (k_indices.size ()));
330 }
331 
332 //////////////////////////////////////////////////////////////////////////////////////////////
333 template <typename PointT> int
335  const PointT& point, double radius, Indices &k_indices,
336  std::vector<float> &k_sqr_distances, unsigned int max_nn) const
337 {
338  assert (isFinite (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
339 
340  k_indices.clear ();
341  k_sqr_distances.clear ();
342  if (radius <= 0)
343  return 0;
344 
345  if (input_->is_dense)
346  return denseRadiusSearch (point, radius, k_indices, k_sqr_distances, max_nn);
347  return sparseRadiusSearch (point, radius, k_indices, k_sqr_distances, max_nn);
348 }
349 
350 #define PCL_INSTANTIATE_BruteForce(T) template class PCL_EXPORTS pcl::search::BruteForce<T>;
Implementation of a simple brute force search algorithm.
Definition: brute_force.h:52
int nearestKSearch(const PointT &point, int k, Indices &k_indices, std::vector< float > &k_distances) const override
Search for the k-nearest neighbors for the given query point.
Definition: brute_force.hpp:54
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.
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition: point_tests.h:55
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
A point structure representing Euclidean xyz coordinates, and the RGB color.