Point Cloud Library (PCL)  1.14.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  for (std::size_t i = 0; i < cloud.size (); i++)
124  nearestKSearch (cloud, static_cast<index_t> (i), k, k_indices[i], k_sqr_distances[i]);
125  }
126  else
127  {
128  k_indices.resize (indices.size ());
129  k_sqr_distances.resize (indices.size ());
130  for (std::size_t i = 0; i < indices.size (); i++)
131  nearestKSearch (cloud, indices[i], k, k_indices[i], k_sqr_distances[i]);
132  }
133 }
134 
135 ///////////////////////////////////////////////////////////////////////////////////////////
136 template <typename PointT> int
138  const PointCloud &cloud, index_t index, double radius,
139  Indices &k_indices, std::vector<float> &k_sqr_distances,
140  unsigned int max_nn) const
141 {
142  assert (index >= 0 && index < static_cast<index_t> (cloud.size ()) && "Out-of-bounds error in radiusSearch!");
143  return (radiusSearch(cloud[index], radius, k_indices, k_sqr_distances, max_nn));
144 }
145 
146 ///////////////////////////////////////////////////////////////////////////////////////////
147 template <typename PointT> int
149  index_t index, double radius, Indices &k_indices,
150  std::vector<float> &k_sqr_distances, unsigned int max_nn ) const
151 {
152  if (!indices_)
153  {
154  assert (index >= 0 && index < static_cast<index_t> (input_->size ()) && "Out-of-bounds error in radiusSearch!");
155  return (radiusSearch ((*input_)[index], radius, k_indices, k_sqr_distances, max_nn));
156  }
157  assert (index >= 0 && index < static_cast<index_t> (indices_->size ()) && "Out-of-bounds error in radiusSearch!");
158  return (radiusSearch ((*input_)[(*indices_)[index]], radius, k_indices, k_sqr_distances, max_nn));
159 }
160 
161 ///////////////////////////////////////////////////////////////////////////////////////////
162 template <typename PointT> void
164  const PointCloud& cloud,
165  const Indices& indices,
166  double radius,
167  std::vector<Indices>& k_indices,
168  std::vector< std::vector<float> > &k_sqr_distances,
169  unsigned int max_nn) const
170 {
171  if (indices.empty ())
172  {
173  k_indices.resize (cloud.size ());
174  k_sqr_distances.resize (cloud.size ());
175  for (std::size_t i = 0; i < cloud.size (); i++)
176  radiusSearch (cloud, static_cast<index_t> (i), radius,k_indices[i], k_sqr_distances[i], max_nn);
177  }
178  else
179  {
180  k_indices.resize (indices.size ());
181  k_sqr_distances.resize (indices.size ());
182  for (std::size_t i = 0; i < indices.size (); i++)
183  radiusSearch (cloud,indices[i],radius,k_indices[i],k_sqr_distances[i], max_nn);
184  }
185 }
186 
187 ///////////////////////////////////////////////////////////////////////////////////////////
188 template <typename PointT> void
190  Indices& indices, std::vector<float>& distances) const
191 {
192  Indices order (indices.size ());
193  for (std::size_t idx = 0; idx < order.size (); ++idx)
194  order [idx] = static_cast<index_t> (idx);
195 
196  Compare compare (distances);
197  sort (order.begin (), order.end (), compare);
198 
199  Indices sorted (indices.size ());
200  for (std::size_t idx = 0; idx < order.size (); ++idx)
201  sorted [idx] = indices[order [idx]];
202 
203  indices = sorted;
204 
205  // sort the according distances.
206  sort (distances.begin (), distances.end ());
207 }
208 
209 #define PCL_INSTANTIATE_Search(T) template class PCL_EXPORTS pcl::search::Search<T>;
210 
211 #endif //#ifndef _PCL_SEARCH_SEARCH_IMPL_HPP_
212 
213 
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
std::size_t size() const
Definition: point_cloud.h:443
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:189
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