Point Cloud Library (PCL) 1.13.1
Loading...
Searching...
No Matches
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//////////////////////////////////////////////////////////////////////////////////////////////
45template <typename PointT> float
47 const PointT& point1, const PointT& point2) const
48{
49 return (point1.getVector3fMap () - point2.getVector3fMap ()).squaredNorm ();
50}
51
52//////////////////////////////////////////////////////////////////////////////////////////////
53template <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//////////////////////////////////////////////////////////////////////////////////////////////
70template <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//////////////////////////////////////////////////////////////////////////////////////////////
138template <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//////////////////////////////////////////////////////////////////////////////////////////////
217template <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//////////////////////////////////////////////////////////////////////////////////////////////
272template <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//////////////////////////////////////////////////////////////////////////////////////////////
333template <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.
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.