Point Cloud Library (PCL)  1.7.0
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 #ifndef PCL_SEARCH_IMPL_BRUTE_FORCE_SEARCH_H_
39 #define PCL_SEARCH_IMPL_BRUTE_FORCE_SEARCH_H_
40 
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, std::vector<int>& 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  else
67  return sparseKSearch (point, k, k_indices, k_distances);
68 }
69 
70 //////////////////////////////////////////////////////////////////////////////////////////////
71 template <typename PointT> int
73  const PointT &point, int k, std::vector<int> &k_indices, std::vector<float> &k_distances) const
74 {
75  // container for first k elements -> O(1) for insertion, since order not required here
76  std::vector<Entry> result;
77  result.reserve (k);
78  std::priority_queue<Entry> queue;
79  if (indices_ != NULL)
80  {
81  std::vector<int>::const_iterator iIt =indices_->begin ();
82  std::vector<int>::const_iterator iEnd = indices_->begin () + std::min (static_cast<unsigned> (k), static_cast<unsigned> (indices_->size ()));
83  for (; iIt != iEnd; ++iIt)
84  result.push_back (Entry (*iIt, getDistSqr (input_->points[*iIt], point)));
85 
86  queue = std::priority_queue<Entry> (result.begin (), result.end ());
87 
88  // add the rest
89  Entry entry;
90  for (; iIt != indices_->end (); ++iIt)
91  {
92  entry.distance = getDistSqr (input_->points[*iIt], point);
93  if (queue.top ().distance > entry.distance)
94  {
95  entry.index = *iIt;
96  queue.pop ();
97  queue.push (entry);
98  }
99  }
100  }
101  else
102  {
103  Entry entry;
104  for (entry.index = 0; entry.index < std::min (static_cast<unsigned> (k), static_cast<unsigned> (input_->size ())); ++entry.index)
105  {
106  entry.distance = getDistSqr (input_->points[entry.index], point);
107  result.push_back (entry);
108  }
109 
110  queue = std::priority_queue<Entry> (result.begin (), result.end ());
111 
112  // add the rest
113  for (; entry.index < input_->size (); ++entry.index)
114  {
115  entry.distance = getDistSqr (input_->points[entry.index], point);
116  if (queue.top ().distance > entry.distance)
117  {
118  queue.pop ();
119  queue.push (entry);
120  }
121  }
122  }
123 
124  k_indices.resize (queue.size ());
125  k_distances.resize (queue.size ());
126  size_t idx = queue.size () - 1;
127  while (!queue.empty ())
128  {
129  k_indices [idx] = queue.top ().index;
130  k_distances [idx] = queue.top ().distance;
131  queue.pop ();
132  --idx;
133  }
134 
135  return (static_cast<int> (k_indices.size ()));
136 }
137 
138 //////////////////////////////////////////////////////////////////////////////////////////////
139 template <typename PointT> int
141  const PointT &point, int k, std::vector<int> &k_indices, std::vector<float> &k_distances) const
142 {
143  // result used to collect the first k neighbors -> unordered
144  std::vector<Entry> result;
145  result.reserve (k);
146 
147  std::priority_queue<Entry> queue;
148  if (indices_ != NULL)
149  {
150  std::vector<int>::const_iterator iIt =indices_->begin ();
151  for (; iIt != indices_->end () && result.size () < static_cast<unsigned> (k); ++iIt)
152  {
153  if (pcl_isfinite (input_->points[*iIt].x))
154  result.push_back (Entry (*iIt, getDistSqr (input_->points[*iIt], point)));
155  }
156 
157  queue = std::priority_queue<Entry> (result.begin (), result.end ());
158 
159  // either we have k elements, or there are none left to iterate >in either case we're fine
160  // add the rest
161  Entry entry;
162  for (; iIt != indices_->end (); ++iIt)
163  {
164  if (!pcl_isfinite (input_->points[*iIt].x))
165  continue;
166 
167  entry.distance = getDistSqr (input_->points[*iIt], point);
168  if (queue.top ().distance > entry.distance)
169  {
170  entry.index = *iIt;
171  queue.pop ();
172  queue.push (entry);
173  }
174  }
175  }
176  else
177  {
178  Entry entry;
179  for (entry.index = 0; entry.index < input_->size () && result.size () < static_cast<unsigned> (k); ++entry.index)
180  {
181  if (pcl_isfinite (input_->points[entry.index].x))
182  {
183  entry.distance = getDistSqr (input_->points[entry.index], point);
184  result.push_back (entry);
185  }
186  }
187  queue = std::priority_queue<Entry> (result.begin (), result.end ());
188 
189  // add the rest
190  for (; entry.index < input_->size (); ++entry.index)
191  {
192  if (!pcl_isfinite (input_->points[entry.index].x))
193  continue;
194 
195  entry.distance = getDistSqr (input_->points[entry.index], point);
196  if (queue.top ().distance > entry.distance)
197  {
198  queue.pop ();
199  queue.push (entry);
200  }
201  }
202  }
203 
204  k_indices.resize (queue.size ());
205  k_distances.resize (queue.size ());
206  size_t idx = queue.size () - 1;
207  while (!queue.empty ())
208  {
209  k_indices [idx] = queue.top ().index;
210  k_distances [idx] = queue.top ().distance;
211  queue.pop ();
212  --idx;
213  }
214  return (static_cast<int> (k_indices.size ()));
215 }
216 
217 //////////////////////////////////////////////////////////////////////////////////////////////
218 template <typename PointT> int
220  const PointT& point, double radius,
221  std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
222  unsigned int max_nn) const
223 {
224  radius *= radius;
225 
226  size_t reserve = max_nn;
227  if (reserve == 0)
228  {
229  if (indices_ != NULL)
230  reserve = std::min (indices_->size (), input_->size ());
231  else
232  reserve = input_->size ();
233  }
234  k_indices.reserve (reserve);
235  k_sqr_distances.reserve (reserve);
236  float distance;
237  if (indices_ != NULL)
238  {
239  for (std::vector<int>::const_iterator iIt =indices_->begin (); iIt != indices_->end (); ++iIt)
240  {
241  distance = getDistSqr (input_->points[*iIt], point);
242  if (distance <= radius)
243  {
244  k_indices.push_back (*iIt);
245  k_sqr_distances.push_back (distance);
246  if (k_indices.size () == max_nn) // max_nn = 0 -> never true
247  break;
248  }
249  }
250  }
251  else
252  {
253  for (unsigned index = 0; index < input_->size (); ++index)
254  {
255  distance = getDistSqr (input_->points[index], point);
256  if (distance <= radius)
257  {
258  k_indices.push_back (index);
259  k_sqr_distances.push_back (distance);
260  if (k_indices.size () == max_nn) // never true if max_nn = 0
261  break;
262  }
263  }
264  }
265 
266  if (sorted_results_)
267  this->sortResults (k_indices, k_sqr_distances);
268 
269  return (static_cast<int> (k_indices.size ()));
270 }
271 
272 //////////////////////////////////////////////////////////////////////////////////////////////
273 template <typename PointT> int
275  const PointT& point, double radius,
276  std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
277  unsigned int max_nn) const
278 {
279  radius *= radius;
280 
281  size_t reserve = max_nn;
282  if (reserve == 0)
283  {
284  if (indices_ != NULL)
285  reserve = std::min (indices_->size (), input_->size ());
286  else
287  reserve = input_->size ();
288  }
289  k_indices.reserve (reserve);
290  k_sqr_distances.reserve (reserve);
291 
292  float distance;
293  if (indices_ != NULL)
294  {
295  for (std::vector<int>::const_iterator iIt =indices_->begin (); iIt != indices_->end (); ++iIt)
296  {
297  if (!pcl_isfinite (input_->points[*iIt].x))
298  continue;
299 
300  distance = getDistSqr (input_->points[*iIt], point);
301  if (distance <= radius)
302  {
303  k_indices.push_back (*iIt);
304  k_sqr_distances.push_back (distance);
305  if (k_indices.size () == max_nn) // never true if max_nn = 0
306  break;
307  }
308  }
309  }
310  else
311  {
312  for (unsigned index = 0; index < input_->size (); ++index)
313  {
314  if (!pcl_isfinite (input_->points[index].x))
315  continue;
316  distance = getDistSqr (input_->points[index], point);
317  if (distance <= radius)
318  {
319  k_indices.push_back (index);
320  k_sqr_distances.push_back (distance);
321  if (k_indices.size () == max_nn) // never true if max_nn = 0
322  break;
323  }
324  }
325  }
326 
327  if (sorted_results_)
328  this->sortResults (k_indices, k_sqr_distances);
329 
330  return (static_cast<int> (k_indices.size ()));
331 }
332 
333 //////////////////////////////////////////////////////////////////////////////////////////////
334 template <typename PointT> int
336  const PointT& point, double radius, std::vector<int> &k_indices,
337  std::vector<float> &k_sqr_distances, unsigned int max_nn) const
338 {
339  assert (isFinite (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
340 
341  k_indices.clear ();
342  k_sqr_distances.clear ();
343  if (radius <= 0)
344  return 0;
345 
346  if (input_->is_dense)
347  return denseRadiusSearch (point, radius, k_indices, k_sqr_distances, max_nn);
348  else
349  return sparseRadiusSearch (point, radius, k_indices, k_sqr_distances, max_nn);
350 }
351 
352 #define PCL_INSTANTIATE_BruteForce(T) template class PCL_EXPORTS pcl::search::BruteForce<T>;
353 
354 #endif //PCL_SEARCH_IMPL_BRUTE_FORCE_SEARCH_H_