Point Cloud Library (PCL) 1.14.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
81 using Ptr = shared_ptr<pcl::search::Search<PointT> >;
82 using ConstPtr = shared_ptr<const pcl::search::Search<PointT> >;
83
86
87 /** Constructor. */
88 Search (const std::string& name = "", bool sorted = false);
89
90 /** Destructor. */
91 virtual
92 ~Search () = default;
93
94 /** \brief Returns the search method name
95 */
96 virtual const std::string&
97 getName () const;
98
99 /** \brief sets whether the results should be sorted (ascending in the distance) or not
100 * \param[in] sorted should be true if the results should be sorted by the distance in ascending order.
101 * Otherwise the results may be returned in any order.
102 */
103 virtual void
104 setSortedResults (bool sorted);
105
106 /** \brief Gets whether the results should be sorted (ascending in the distance) or not
107 * Otherwise the results may be returned in any order.
108 */
109 virtual bool
111
112
113 /** \brief Pass the input dataset that the search will be performed on.
114 * \param[in] cloud a const pointer to the PointCloud data
115 * \param[in] indices the point indices subset that is to be used from the cloud
116 * \return True if successful, false if an error occurred, for example because the point cloud is unsuited for the search method.
117 */
118 virtual bool
119 setInputCloud (const PointCloudConstPtr& cloud,
120 const IndicesConstPtr &indices = IndicesConstPtr ());
121
122 /** \brief Get a pointer to the input point cloud dataset. */
123 virtual PointCloudConstPtr
125 {
126 return (input_);
127 }
128
129 /** \brief Get a pointer to the vector of indices used. */
130 virtual IndicesConstPtr
131 getIndices () const
132 {
133 return (indices_);
134 }
135
136 /** \brief Search for the k-nearest neighbors for the given query point.
137 * \param[in] point the given query point
138 * \param[in] k the number of neighbors to search for
139 * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
140 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
141 * a priori!)
142 * \return number of neighbors found
143 */
144 virtual int
145 nearestKSearch (const PointT &point, int k, Indices &k_indices,
146 std::vector<float> &k_sqr_distances) const = 0;
147
148 /** \brief Search for k-nearest neighbors for the given query point.
149 * This method accepts a different template parameter for the point type.
150 * \param[in] point the given query point
151 * \param[in] k the number of neighbors to search for
152 * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
153 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
154 * a priori!)
155 * \return number of neighbors found
156 */
157 template <typename PointTDiff> inline int
158 nearestKSearchT (const PointTDiff &point, int k,
159 Indices &k_indices, std::vector<float> &k_sqr_distances) const
160 {
161 PointT p;
162 copyPoint (point, p);
163 return (nearestKSearch (p, k, k_indices, k_sqr_distances));
164 }
165
166 /** \brief Search for k-nearest neighbors for the given query point.
167 *
168 * \attention This method does not do any bounds checking for the input index
169 * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data.
170 *
171 * \param[in] cloud the point cloud data
172 * \param[in] index a \a valid index in \a cloud representing a \a valid (i.e., finite) query point
173 * \param[in] k the number of neighbors to search for
174 * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
175 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
176 * a priori!)
177 *
178 * \return number of neighbors found
179 *
180 * \exception asserts in debug mode if the index is not between 0 and the maximum number of points
181 */
182 virtual int
183 nearestKSearch (const PointCloud &cloud, index_t index, int k,
184 Indices &k_indices,
185 std::vector<float> &k_sqr_distances) const;
186
187 /** \brief Search for k-nearest neighbors for the given query point (zero-copy).
188 *
189 * \attention This method does not do any bounds checking for the input index
190 * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data.
191 *
192 * \param[in] index a \a valid index representing a \a valid query point in the dataset given
193 * by \a setInputCloud. If indices were given in setInputCloud, index will be the position in
194 * the indices vector.
195 *
196 * \param[in] k the number of neighbors to search for
197 * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
198 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
199 * a priori!)
200 * \return number of neighbors found
201 *
202 * \exception asserts in debug mode if the index is not between 0 and the maximum number of points
203 */
204 virtual int
205 nearestKSearch (index_t index, int k,
206 Indices &k_indices,
207 std::vector<float> &k_sqr_distances) const;
208
209 /** \brief Search for the k-nearest neighbors for the given query point.
210 * \param[in] cloud the point cloud data
211 * \param[in] indices a vector of point cloud indices to query for nearest neighbors
212 * \param[in] k the number of neighbors to search for
213 * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i
214 * \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
215 */
216 virtual void
217 nearestKSearch (const PointCloud& cloud, const Indices& indices,
218 int k, std::vector<Indices>& k_indices,
219 std::vector< std::vector<float> >& k_sqr_distances) const;
220
221 /** \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).
222 * \param[in] cloud the point cloud data
223 * \param[in] indices a vector of point cloud indices to query for nearest neighbors
224 * \param[in] k the number of neighbors to search for
225 * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i
226 * \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
227 * \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.
228 */
229 template <typename PointTDiff> void
230 nearestKSearchT (const pcl::PointCloud<PointTDiff> &cloud, const Indices& indices, int k, std::vector<Indices> &k_indices,
231 std::vector< std::vector<float> > &k_sqr_distances) const
232 {
233 // Copy all the data fields from the input cloud to the output one
234 using FieldListInT = typename pcl::traits::fieldList<PointT>::type;
235 using FieldListOutT = typename pcl::traits::fieldList<PointTDiff>::type;
236 using FieldList = typename pcl::intersect<FieldListInT, FieldListOutT>::type;
237
239 if (indices.empty ())
240 {
241 pc.resize (cloud.size());
242 for (std::size_t i = 0; i < cloud.size(); i++)
243 {
244 pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointTDiff, PointT> (
245 cloud[i], pc[i]));
246 }
247 nearestKSearch (pc,Indices(),k,k_indices,k_sqr_distances);
248 }
249 else
250 {
251 pc.resize (indices.size());
252 for (std::size_t i = 0; i < indices.size(); i++)
253 {
254 pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointTDiff, PointT> (
255 cloud[indices[i]], pc[i]));
256 }
257 nearestKSearch (pc,Indices(),k,k_indices,k_sqr_distances);
258 }
259 }
260
261 /** \brief Search for all the nearest neighbors of the query point in a given radius.
262 * \param[in] point the given query point
263 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
264 * \param[out] k_indices the resultant indices of the neighboring points
265 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
266 * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
267 * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
268 * returned.
269 * \return number of neighbors found in radius
270 */
271 virtual int
272 radiusSearch (const PointT& point, double radius, Indices& k_indices,
273 std::vector<float>& k_sqr_distances, unsigned int max_nn = 0) const = 0;
274
275 /** \brief Search for all the nearest neighbors of the query point in a given radius.
276 * \param[in] point the given query point
277 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
278 * \param[out] k_indices the resultant indices of the neighboring points
279 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
280 * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
281 * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
282 * returned.
283 * \return number of neighbors found in radius
284 */
285 template <typename PointTDiff> inline int
286 radiusSearchT (const PointTDiff &point, double radius, Indices &k_indices,
287 std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const
288 {
289 PointT p;
290 copyPoint (point, p);
291 return (radiusSearch (p, radius, k_indices, k_sqr_distances, max_nn));
292 }
293
294 /** \brief Search for all the nearest neighbors of the query point in a given radius.
295 *
296 * \attention This method does not do any bounds checking for the input index
297 * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data.
298 *
299 * \param[in] cloud the point cloud data
300 * \param[in] index a \a valid index in \a cloud representing a \a valid (i.e., finite) query point
301 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
302 * \param[out] k_indices the resultant indices of the neighboring points
303 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
304 * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
305 * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
306 * returned.
307 * \return number of neighbors found in radius
308 *
309 * \exception asserts in debug mode if the index is not between 0 and the maximum number of points
310 */
311 virtual int
312 radiusSearch (const PointCloud &cloud, index_t index, double radius,
313 Indices &k_indices, std::vector<float> &k_sqr_distances,
314 unsigned int max_nn = 0) const;
315
316 /** \brief Search for all the nearest neighbors of the query point in a given radius (zero-copy).
317 *
318 * \attention This method does not do any bounds checking for the input index
319 * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data.
320 *
321 * \param[in] index a \a valid index representing a \a valid query point in the dataset given
322 * by \a setInputCloud. If indices were given in setInputCloud, index will be the position in
323 * the indices vector.
324 *
325 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
326 * \param[out] k_indices the resultant indices of the neighboring points
327 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
328 * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
329 * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
330 * returned.
331 * \return number of neighbors found in radius
332 *
333 * \exception asserts in debug mode if the index is not between 0 and the maximum number of points
334 */
335 virtual int
336 radiusSearch (index_t index, double radius, Indices &k_indices,
337 std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const;
338
339 /** \brief Search for all the nearest neighbors of the query point in a given radius.
340 * \param[in] cloud the point cloud data
341 * \param[in] indices the indices in \a cloud. If indices is empty, neighbors will be searched for all points.
342 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
343 * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i
344 * \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
345 * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
346 * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
347 * returned.
348 */
349 virtual void
350 radiusSearch (const PointCloud& cloud,
351 const Indices& indices,
352 double radius,
353 std::vector<Indices>& k_indices,
354 std::vector< std::vector<float> > &k_sqr_distances,
355 unsigned int max_nn = 0) const;
356
357 /** \brief Search for all the nearest neighbors of the query points in a given radius.
358 * \param[in] cloud the point cloud data
359 * \param[in] indices a vector of point cloud indices to query for nearest neighbors
360 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
361 * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i
362 * \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
363 * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
364 * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
365 * returned.
366 * \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.
367 */
368 template <typename PointTDiff> void
370 const Indices& indices,
371 double radius,
372 std::vector<Indices> &k_indices,
373 std::vector< std::vector<float> > &k_sqr_distances,
374 unsigned int max_nn = 0) const
375 {
376 // Copy all the data fields from the input cloud to the output one
377 using FieldListInT = typename pcl::traits::fieldList<PointT>::type;
378 using FieldListOutT = typename pcl::traits::fieldList<PointTDiff>::type;
379 using FieldList = typename pcl::intersect<FieldListInT, FieldListOutT>::type;
380
382 if (indices.empty ())
383 {
384 pc.resize (cloud.size ());
385 for (std::size_t i = 0; i < cloud.size (); ++i)
386 pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointTDiff, PointT> (cloud[i], pc[i]));
387 radiusSearch (pc, Indices (), radius, k_indices, k_sqr_distances, max_nn);
388 }
389 else
390 {
391 pc.resize (indices.size ());
392 for (std::size_t i = 0; i < indices.size (); ++i)
393 pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointTDiff, PointT> (cloud[indices[i]], pc[i]));
394 radiusSearch (pc, Indices(), radius, k_indices, k_sqr_distances, max_nn);
395 }
396 }
397
398 protected:
399 void
400 sortResults (Indices& indices, std::vector<float>& distances) const;
401
405 std::string name_;
406
407 private:
408 struct Compare
409 {
410 Compare (const std::vector<float>& distances)
411 : distances_ (distances)
412 {
413 }
414
415 bool
416 operator () (index_t first, index_t second) const
417 {
418 return (distances_ [first] < distances_[second]);
419 }
420
421 const std::vector<float>& distances_;
422 };
423 }; // class Search
424 } // namespace search
425} // namespace pcl
426
427#ifdef PCL_NO_PRECOMPILE
428#include <pcl/search/impl/search.hpp>
429#endif
PointCloud represents the base class in PCL for storing collections of 3D points.
void resize(std::size_t count)
Resizes the container to contain count elements.
std::size_t size() const
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
Generic search class.
Definition search.h:75
shared_ptr< const pcl::search::Search< PointT > > ConstPtr
Definition search.h:82
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:131
PointCloudConstPtr input_
Definition search.h:402
void sortResults(Indices &indices, std::vector< float > &distances) const
Definition search.hpp:189
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:230
typename PointCloud::ConstPtr PointCloudConstPtr
Definition search.h:79
virtual bool setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Pass the input dataset that the search will be performed on.
Definition search.hpp:75
IndicesConstPtr indices_
Definition search.h:403
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:369
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:124
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:286
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:405
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:158
virtual ~Search()=default
Destructor.
void copyPoint(const PointInT &point_in, PointOutT &point_out)
Copy the fields of a source point into a target point.
shared_ptr< const Indices > IndicesConstPtr
Definition pcl_base.h:59
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
shared_ptr< Indices > IndicesPtr
Definition pcl_base.h:58
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