Point Cloud Library (PCL)  1.15.1-dev
search.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2012-, Open Perception, 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_SEARCH_IMPL_HPP_
39 #define PCL_SEARCH_SEARCH_IMPL_HPP_
40 
41 #include <pcl/search/search.h>
42 
43 ///////////////////////////////////////////////////////////////////////////////////////////
44 template <typename PointT>
45 pcl::search::Search<PointT>::Search (const std::string& name, bool sorted)
46  : input_ ()
47  , sorted_results_ (sorted)
48  , name_ (name)
49 {
50 }
51 
52 ///////////////////////////////////////////////////////////////////////////////////////////
53 template <typename PointT> const std::string&
55 {
56  return (name_);
57 }
58 
59 ///////////////////////////////////////////////////////////////////////////////////////////
60 template <typename PointT> void
62 {
63  sorted_results_ = sorted;
64 }
65 
66 ///////////////////////////////////////////////////////////////////////////////////////////
67 template <typename PointT> bool
69 {
70  return (sorted_results_);
71 }
72 
73 ///////////////////////////////////////////////////////////////////////////////////////////
74 template <typename PointT> bool
76  const PointCloudConstPtr& cloud, const IndicesConstPtr &indices)
77 {
78  input_ = cloud;
79  indices_ = indices;
80  return true;
81 }
82 
83 
84 ///////////////////////////////////////////////////////////////////////////////////////////
85 template <typename PointT> int
87  const PointCloud &cloud, index_t index, int k,
88  Indices &k_indices, std::vector<float> &k_sqr_distances) const
89 {
90  assert (index >= 0 && index < static_cast<index_t> (cloud.size ()) && "Out-of-bounds error in nearestKSearch!");
91  return (nearestKSearch (cloud[index], k, k_indices, k_sqr_distances));
92 }
93 
94 ///////////////////////////////////////////////////////////////////////////////////////////
95 template <typename PointT> int
97  index_t index, int k,
98  Indices &k_indices,
99  std::vector<float> &k_sqr_distances) const
100 {
101  if (!indices_)
102  {
103  assert (index >= 0 && index < static_cast<index_t> (input_->size ()) && "Out-of-bounds error in nearestKSearch!");
104  return (nearestKSearch ((*input_)[index], k, k_indices, k_sqr_distances));
105  }
106  assert (index >= 0 && index < static_cast<index_t> (indices_->size ()) && "Out-of-bounds error in nearestKSearch!");
107  if (index >= static_cast<index_t> (indices_->size ()) || index < 0)
108  return (0);
109  return (nearestKSearch ((*input_)[(*indices_)[index]], k, k_indices, k_sqr_distances));
110 }
111 
112 ///////////////////////////////////////////////////////////////////////////////////////////
113 template <typename PointT> void
115  const PointCloud& cloud, const Indices& indices,
116  int k, std::vector<Indices>& k_indices,
117  std::vector< std::vector<float> >& k_sqr_distances) const
118 {
119  if (indices.empty ())
120  {
121  k_indices.resize (cloud.size ());
122  k_sqr_distances.resize (cloud.size ());
123  #pragma omp parallel for num_threads(num_threads_) default(none) shared(cloud, k, k_indices, k_sqr_distances)
124  for (std::ptrdiff_t i = 0; i < static_cast<std::ptrdiff_t>(cloud.size ()); i++)
125  nearestKSearch (cloud, static_cast<index_t> (i), k, k_indices[i], k_sqr_distances[i]);
126  }
127  else
128  {
129  k_indices.resize (indices.size ());
130  k_sqr_distances.resize (indices.size ());
131  #pragma omp parallel for num_threads(num_threads_) default(none) shared(cloud, indices, k, k_indices, k_sqr_distances)
132  for (std::ptrdiff_t i = 0; i < static_cast<std::ptrdiff_t>(indices.size ()); i++)
133  nearestKSearch (cloud, indices[i], k, k_indices[i], k_sqr_distances[i]);
134  }
135 }
136 
137 ///////////////////////////////////////////////////////////////////////////////////////////
138 template <typename PointT> int
140  const PointCloud &cloud, index_t index, double radius,
141  Indices &k_indices, std::vector<float> &k_sqr_distances,
142  unsigned int max_nn) const
143 {
144  assert (index >= 0 && index < static_cast<index_t> (cloud.size ()) && "Out-of-bounds error in radiusSearch!");
145  return (radiusSearch(cloud[index], radius, k_indices, k_sqr_distances, max_nn));
146 }
147 
148 ///////////////////////////////////////////////////////////////////////////////////////////
149 template <typename PointT> int
151  index_t index, double radius, Indices &k_indices,
152  std::vector<float> &k_sqr_distances, unsigned int max_nn ) const
153 {
154  if (!indices_)
155  {
156  assert (index >= 0 && index < static_cast<index_t> (input_->size ()) && "Out-of-bounds error in radiusSearch!");
157  return (radiusSearch ((*input_)[index], radius, k_indices, k_sqr_distances, max_nn));
158  }
159  assert (index >= 0 && index < static_cast<index_t> (indices_->size ()) && "Out-of-bounds error in radiusSearch!");
160  return (radiusSearch ((*input_)[(*indices_)[index]], radius, k_indices, k_sqr_distances, max_nn));
161 }
162 
163 ///////////////////////////////////////////////////////////////////////////////////////////
164 template <typename PointT> void
166  const PointCloud& cloud,
167  const Indices& indices,
168  double radius,
169  std::vector<Indices>& k_indices,
170  std::vector< std::vector<float> > &k_sqr_distances,
171  unsigned int max_nn) const
172 {
173  if (indices.empty ())
174  {
175  k_indices.resize (cloud.size ());
176  k_sqr_distances.resize (cloud.size ());
177  #pragma omp parallel for num_threads(num_threads_) default(none) shared(cloud, radius, k_indices, k_sqr_distances, max_nn)
178  for (std::ptrdiff_t i = 0; i < static_cast<std::ptrdiff_t>(cloud.size ()); i++)
179  radiusSearch (cloud, static_cast<index_t> (i), radius,k_indices[i], k_sqr_distances[i], max_nn);
180  }
181  else
182  {
183  k_indices.resize (indices.size ());
184  k_sqr_distances.resize (indices.size ());
185  #pragma omp parallel for num_threads(num_threads_) default(none) shared(cloud, indices, radius, k_indices, k_sqr_distances, max_nn)
186  for (std::ptrdiff_t i = 0; i < static_cast<std::ptrdiff_t>(indices.size ()); i++)
187  radiusSearch (cloud,indices[i],radius,k_indices[i],k_sqr_distances[i], max_nn);
188  }
189 }
190 
191 ///////////////////////////////////////////////////////////////////////////////////////////
192 template <typename PointT> void
194  Indices& indices, std::vector<float>& distances) const
195 {
196  Indices order (indices.size ());
197  for (std::size_t idx = 0; idx < order.size (); ++idx)
198  order [idx] = static_cast<index_t> (idx);
199 
200  Compare compare (distances);
201  std::sort (order.begin (), order.end (), compare);
202 
203  Indices sorted (indices.size ());
204  for (std::size_t idx = 0; idx < order.size (); ++idx)
205  sorted [idx] = indices[order [idx]];
206 
207  indices = sorted;
208 
209  // sort the according distances.
210  std::sort (distances.begin (), distances.end ());
211 }
212 
213 #define PCL_INSTANTIATE_Search(T) template class PCL_EXPORTS pcl::search::Search<T>;
214 
215 #endif //#ifndef _PCL_SEARCH_SEARCH_IMPL_HPP_
216 
217 
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:174
std::size_t size() const
Definition: point_cloud.h:444
virtual bool getSortedResults()
Gets whether the results should be sorted (ascending in the distance) or not Otherwise the results ma...
Definition: search.hpp:68
void sortResults(Indices &indices, std::vector< float > &distances) const
Definition: search.hpp:193
virtual const std::string & getName() const
Returns the search method name.
Definition: search.hpp:54
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
Search(const std::string &name="", bool sorted=false)
Constructor.
Definition: search.hpp:45
pcl::IndicesConstPtr IndicesConstPtr
Definition: search.h:85
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.
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.
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