Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
search.h
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 * 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#pragma once
40
41#include <pcl/pcl_base.h> // for IndicesConstPtr
42#include <pcl/point_cloud.h>
43#include <pcl/for_each_type.h>
44#include <pcl/common/concatenate.h>
45#include <pcl/common/copy_point.h>
46
47namespace pcl
48{
49 namespace search
50 {
51 /** \brief Generic search class. All search wrappers must inherit from this.
52 *
53 * Each search method must implement 2 different types of search:
54 * - \b nearestKSearch - search for K-nearest neighbors.
55 * - \b radiusSearch - search for all nearest neighbors in a sphere of a given radius
56 *
57 * The input to each search method can be given in 3 different ways:
58 * - as a query point
59 * - as a (cloud, index) pair
60 * - as an index
61 *
62 * For the latter option, it is assumed that the user specified the input
63 * via a \ref setInputCloud () method first.
64 *
65 * \note In case of an error, all methods are supposed to return 0 as the number of neighbors found.
66 *
67 * \note libpcl_search deals with three-dimensional search problems. For higher
68 * level dimensional search, please refer to the libpcl_kdtree module.
69 *
70 * \author Radu B. Rusu
71 * \ingroup search
72 */
73 template<typename PointT>
74 class Search
75 {
76 public:
80
83
86
87 /** Constructor. */
88 Search (const std::string& name = "", bool sorted = false);
89
90 /** Destructor. */
91 virtual
93 {
94 }
95
96 /** \brief Returns the search method name
97 */
98 virtual const std::string&
99 getName () const;
100
101 /** \brief sets whether the results should be sorted (ascending in the distance) or not
102 * \param[in] sorted should be true if the results should be sorted by the distance in ascending order.
103 * Otherwise the results may be returned in any order.
104 */
105 virtual void
107
108 /** \brief Gets whether the results should be sorted (ascending in the distance) or not
109 * Otherwise the results may be returned in any order.
110 */
111 virtual bool
113
114
115 /** \brief Pass the input dataset that the search will be performed on.
116 * \param[in] cloud a const pointer to the PointCloud data
117 * \param[in] indices the point indices subset that is to be used from the cloud
118 */
119 virtual void
120 setInputCloud (const PointCloudConstPtr& cloud,
121 const IndicesConstPtr &indices = IndicesConstPtr ());
122
123 /** \brief Get a pointer to the input point cloud dataset. */
124 virtual PointCloudConstPtr
126 {
127 return (input_);
128 }
129
130 /** \brief Get a pointer to the vector of indices used. */
131 virtual IndicesConstPtr
132 getIndices () const
133 {
134 return (indices_);
135 }
136
137 /** \brief Search for the k-nearest neighbors for the given query point.
138 * \param[in] point the given query point
139 * \param[in] k the number of neighbors to search for
140 * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
141 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
142 * a priori!)
143 * \return number of neighbors found
144 */
145 virtual int
146 nearestKSearch (const PointT &point, int k, Indices &k_indices,
147 std::vector<float> &k_sqr_distances) const = 0;
148
149 /** \brief Search for k-nearest neighbors for the given query point.
150 * This method accepts a different template parameter for the point type.
151 * \param[in] point the given query point
152 * \param[in] k the number of neighbors to search for
153 * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
154 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
155 * a priori!)
156 * \return number of neighbors found
157 */
158 template <typename PointTDiff> inline int
159 nearestKSearchT (const PointTDiff &point, int k,
160 Indices &k_indices, std::vector<float> &k_sqr_distances) const
161 {
162 PointT p;
163 copyPoint (point, p);
164 return (nearestKSearch (p, k, k_indices, k_sqr_distances));
165 }
166
167 /** \brief Search for k-nearest neighbors for the given query point.
168 *
169 * \attention This method does not do any bounds checking for the input index
170 * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data.
171 *
172 * \param[in] cloud the point cloud data
173 * \param[in] index a \a valid index in \a cloud representing a \a valid (i.e., finite) query point
174 * \param[in] k the number of neighbors to search for
175 * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
176 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
177 * a priori!)
178 *
179 * \return number of neighbors found
180 *
181 * \exception asserts in debug mode if the index is not between 0 and the maximum number of points
182 */
183 virtual int
184 nearestKSearch (const PointCloud &cloud, index_t index, int k,
186 std::vector<float> &k_sqr_distances) const;
187
188 /** \brief Search for k-nearest neighbors for the given query point (zero-copy).
189 *
190 * \attention This method does not do any bounds checking for the input index
191 * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data.
192 *
193 * \param[in] index a \a valid index representing a \a valid query point in the dataset given
194 * by \a setInputCloud. If indices were given in setInputCloud, index will be the position in
195 * the indices vector.
196 *
197 * \param[in] k the number of neighbors to search for
198 * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
199 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
200 * a priori!)
201 * \return number of neighbors found
202 *
203 * \exception asserts in debug mode if the index is not between 0 and the maximum number of points
204 */
205 virtual int
206 nearestKSearch (index_t index, int k,
208 std::vector<float> &k_sqr_distances) const;
209
210 /** \brief Search for the k-nearest neighbors for the given query point.
211 * \param[in] cloud the point cloud data
212 * \param[in] indices a vector of point cloud indices to query for nearest neighbors
213 * \param[in] k the number of neighbors to search for
214 * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i
215 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points, k_sqr_distances[i] corresponds to the neighbors of the query point i
216 */
217 virtual void
218 nearestKSearch (const PointCloud& cloud, const Indices& indices,
219 int k, std::vector<Indices>& k_indices,
220 std::vector< std::vector<float> >& k_sqr_distances) const;
221
222 /** \brief Search for the k-nearest neighbors for the given query point. Use this method if the query points are of a different type than the points in the data set (e.g. PointXYZRGBA instead of PointXYZ).
223 * \param[in] cloud the point cloud data
224 * \param[in] indices a vector of point cloud indices to query for nearest neighbors
225 * \param[in] k the number of neighbors to search for
226 * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i
227 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points, k_sqr_distances[i] corresponds to the neighbors of the query point i
228 * \note This method copies the input point cloud of type PointTDiff to a temporary cloud of type PointT and performs the batch search on the new cloud. You should prefer the single-point search if you don't use a search algorithm that accelerates batch NN search.
229 */
230 template <typename PointTDiff> void
231 nearestKSearchT (const pcl::PointCloud<PointTDiff> &cloud, const Indices& indices, int k, std::vector<Indices> &k_indices,
232 std::vector< std::vector<float> > &k_sqr_distances) const
233 {
234 // Copy all the data fields from the input cloud to the output one
235 using FieldListInT = typename pcl::traits::fieldList<PointT>::type;
236 using FieldListOutT = typename pcl::traits::fieldList<PointTDiff>::type;
237 using FieldList = typename pcl::intersect<FieldListInT, FieldListOutT>::type;
238
240 if (indices.empty ())
241 {
242 pc.resize (cloud.size());
243 for (std::size_t i = 0; i < cloud.size(); i++)
244 {
246 cloud[i], pc[i]));
247 }
249 }
250 else
251 {
252 pc.resize (indices.size());
253 for (std::size_t i = 0; i < indices.size(); i++)
254 {
256 cloud[indices[i]], pc[i]));
257 }
259 }
260 }
261
262 /** \brief Search for all the nearest neighbors of the query point in a given radius.
263 * \param[in] point the given query point
264 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
265 * \param[out] k_indices the resultant indices of the neighboring points
266 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
267 * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
268 * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
269 * returned.
270 * \return number of neighbors found in radius
271 */
272 virtual int
273 radiusSearch (const PointT& point, double radius, Indices& k_indices,
274 std::vector<float>& k_sqr_distances, unsigned int max_nn = 0) const = 0;
275
276 /** \brief Search for all the nearest neighbors of the query point in a given radius.
277 * \param[in] point the given query point
278 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
279 * \param[out] k_indices the resultant indices of the neighboring points
280 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
281 * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
282 * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
283 * returned.
284 * \return number of neighbors found in radius
285 */
286 template <typename PointTDiff> inline int
287 radiusSearchT (const PointTDiff &point, double radius, Indices &k_indices,
288 std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const
289 {
290 PointT p;
291 copyPoint (point, p);
292 return (radiusSearch (p, radius, k_indices, k_sqr_distances, max_nn));
293 }
294
295 /** \brief Search for all the nearest neighbors of the query point in a given radius.
296 *
297 * \attention This method does not do any bounds checking for the input index
298 * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data.
299 *
300 * \param[in] cloud the point cloud data
301 * \param[in] index a \a valid index in \a cloud representing a \a valid (i.e., finite) query point
302 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
303 * \param[out] k_indices the resultant indices of the neighboring points
304 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
305 * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
306 * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
307 * returned.
308 * \return number of neighbors found in radius
309 *
310 * \exception asserts in debug mode if the index is not between 0 and the maximum number of points
311 */
312 virtual int
313 radiusSearch (const PointCloud &cloud, index_t index, double radius,
314 Indices &k_indices, std::vector<float> &k_sqr_distances,
315 unsigned int max_nn = 0) const;
316
317 /** \brief Search for all the nearest neighbors of the query point in a given radius (zero-copy).
318 *
319 * \attention This method does not do any bounds checking for the input index
320 * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data.
321 *
322 * \param[in] index a \a valid index representing a \a valid query point in the dataset given
323 * by \a setInputCloud. If indices were given in setInputCloud, index will be the position in
324 * the indices vector.
325 *
326 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
327 * \param[out] k_indices the resultant indices of the neighboring points
328 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
329 * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
330 * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
331 * returned.
332 * \return number of neighbors found in radius
333 *
334 * \exception asserts in debug mode if the index is not between 0 and the maximum number of points
335 */
336 virtual int
337 radiusSearch (index_t index, double radius, Indices &k_indices,
338 std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const;
339
340 /** \brief Search for all the nearest neighbors of the query point in a given radius.
341 * \param[in] cloud the point cloud data
342 * \param[in] indices the indices in \a cloud. If indices is empty, neighbors will be searched for all points.
343 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
344 * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i
345 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points, k_sqr_distances[i] corresponds to the neighbors of the query point i
346 * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
347 * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
348 * returned.
349 */
350 virtual void
351 radiusSearch (const PointCloud& cloud,
352 const Indices& indices,
353 double radius,
354 std::vector<Indices>& k_indices,
355 std::vector< std::vector<float> > &k_sqr_distances,
356 unsigned int max_nn = 0) const;
357
358 /** \brief Search for all the nearest neighbors of the query points in a given radius.
359 * \param[in] cloud the point cloud data
360 * \param[in] indices a vector of point cloud indices to query for nearest neighbors
361 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
362 * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i
363 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points, k_sqr_distances[i] corresponds to the neighbors of the query point i
364 * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
365 * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
366 * returned.
367 * \note This method copies the input point cloud of type PointTDiff to a temporary cloud of type PointT and performs the batch search on the new cloud. You should prefer the single-point search if you don't use a search algorithm that accelerates batch NN search.
368 */
369 template <typename PointTDiff> void
371 const Indices& indices,
372 double radius,
373 std::vector<Indices> &k_indices,
374 std::vector< std::vector<float> > &k_sqr_distances,
375 unsigned int max_nn = 0) const
376 {
377 // Copy all the data fields from the input cloud to the output one
378 using FieldListInT = typename pcl::traits::fieldList<PointT>::type;
379 using FieldListOutT = typename pcl::traits::fieldList<PointTDiff>::type;
380 using FieldList = typename pcl::intersect<FieldListInT, FieldListOutT>::type;
381
383 if (indices.empty ())
384 {
385 pc.resize (cloud.size ());
386 for (std::size_t i = 0; i < cloud.size (); ++i)
389 }
390 else
391 {
392 pc.resize (indices.size ());
393 for (std::size_t i = 0; i < indices.size (); ++i)
396 }
397 }
398
399 protected:
400 void
401 sortResults (Indices& indices, std::vector<float>& distances) const;
402
406 std::string name_;
407
408 private:
409 struct Compare
410 {
411 Compare (const std::vector<float>& distances)
412 : distances_ (distances)
413 {
414 }
415
416 bool
417 operator () (index_t first, index_t second) const
418 {
419 return (distances_ [first] < distances_[second]);
420 }
421
422 const std::vector<float>& distances_;
423 };
424 }; // class Search
425 } // namespace search
426} // namespace pcl
427
428#ifdef PCL_NO_PRECOMPILE
429#include <pcl/search/impl/search.hpp>
430#endif
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
Generic search class.
Definition search.h:75
virtual bool getSortedResults()
Gets whether the results should be sorted (ascending in the distance) or not Otherwise the results ma...
Definition search.hpp:68
virtual IndicesConstPtr getIndices() const
Get a pointer to the vector of indices used.
Definition search.h:132
virtual ~Search()
Destructor.
Definition search.h:92
PointCloudConstPtr input_
Definition search.h:403
virtual void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Pass the input dataset that the search will be performed on.
Definition search.hpp:75
void sortResults(Indices &indices, std::vector< float > &distances) const
Definition search.hpp:188
virtual const std::string & getName() const
Returns the search method name.
Definition search.hpp:54
void nearestKSearchT(const pcl::PointCloud< PointTDiff > &cloud, const Indices &indices, int k, std::vector< Indices > &k_indices, std::vector< std::vector< float > > &k_sqr_distances) const
Search for the k-nearest neighbors for the given query point.
Definition search.h:231
typename PointCloud::ConstPtr PointCloudConstPtr
Definition search.h:79
IndicesConstPtr indices_
Definition search.h:404
Search(const std::string &name="", bool sorted=false)
Constructor.
Definition search.hpp:45
pcl::IndicesConstPtr IndicesConstPtr
Definition search.h:85
pcl::IndicesPtr IndicesPtr
Definition search.h:84
void radiusSearchT(const pcl::PointCloud< PointTDiff > &cloud, const Indices &indices, double radius, std::vector< Indices > &k_indices, std::vector< std::vector< float > > &k_sqr_distances, unsigned int max_nn=0) const
Search for all the nearest neighbors of the query points in a given radius.
Definition search.h:370
shared_ptr< pcl::search::Search< PointT > > Ptr
Definition search.h:81
virtual PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
Definition search.h:125
int radiusSearchT(const PointTDiff &point, double radius, Indices &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const
Search for all the nearest neighbors of the query point in a given radius.
Definition search.h:287
typename PointCloud::Ptr PointCloudPtr
Definition search.h:78
virtual int nearestKSearch(const PointT &point, int k, Indices &k_indices, std::vector< float > &k_sqr_distances) const =0
Search for the k-nearest neighbors for the given query point.
std::string name_
Definition search.h:406
virtual void setSortedResults(bool sorted)
sets whether the results should be sorted (ascending in the distance) or not
Definition search.hpp:61
virtual int radiusSearch(const PointT &point, double radius, Indices &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const =0
Search for all the nearest neighbors of the query point in a given radius.
int nearestKSearchT(const PointTDiff &point, int k, Indices &k_indices, std::vector< float > &k_sqr_distances) const
Search for k-nearest neighbors for the given query point.
Definition search.h:159
void copyPoint(const PointInT &point_in, PointOutT &point_out)
Copy the fields of a source point into a target point.
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
shared_ptr< Indices > IndicesPtr
Definition pcl_base.h:58
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
shared_ptr< const Indices > IndicesConstPtr
Definition pcl_base.h:59
A point structure representing Euclidean xyz coordinates, and the RGB color.
typename boost::mpl::remove_if< Sequence1, boost::mpl::not_< boost::mpl::contains< Sequence2, boost::mpl::_1 > > >::type type