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