Point Cloud Library (PCL)  1.7.0
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  , indices_ ()
48  , sorted_results_ (sorted)
49  , name_ (name)
50 {
51 }
52 
53 ///////////////////////////////////////////////////////////////////////////////////////////
54 template <typename PointT> const std::string&
56 {
57  return (name_);
58 }
59 
60 ///////////////////////////////////////////////////////////////////////////////////////////
61 template <typename PointT> void
63 {
64  sorted_results_ = sorted;
65 }
66 
67 ///////////////////////////////////////////////////////////////////////////////////////////
68 template <typename PointT> bool
70 {
71  return (sorted_results_);
72 }
73 
74 ///////////////////////////////////////////////////////////////////////////////////////////
75 template <typename PointT> void
77  const PointCloudConstPtr& cloud, const IndicesConstPtr &indices)
78 {
79  input_ = cloud;
80  indices_ = indices;
81 }
82 
83 
84 ///////////////////////////////////////////////////////////////////////////////////////////
85 template <typename PointT> int
87  const PointCloud &cloud, int index, int k,
88  std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const
89 {
90  assert (index >= 0 && index < static_cast<int> (cloud.points.size ()) && "Out-of-bounds error in nearestKSearch!");
91  return (nearestKSearch (cloud.points[index], k, k_indices, k_sqr_distances));
92 }
93 
94 ///////////////////////////////////////////////////////////////////////////////////////////
95 template <typename PointT> int
97  int index, int k,
98  std::vector<int> &k_indices,
99  std::vector<float> &k_sqr_distances) const
100 {
101  if (indices_ == NULL)
102  {
103  assert (index >= 0 && index < static_cast<int> (input_->points.size ()) && "Out-of-bounds error in nearestKSearch!");
104  return (nearestKSearch (input_->points[index], k, k_indices, k_sqr_distances));
105  }
106  else
107  {
108  assert (index >= 0 && index < static_cast<int> (indices_->size ()) && "Out-of-bounds error in nearestKSearch!");
109  if (index >= static_cast<int> (indices_->size ()) || index < 0)
110  return (0);
111  return (nearestKSearch (input_->points[(*indices_)[index]], k, k_indices, k_sqr_distances));
112  }
113 }
114 
115 ///////////////////////////////////////////////////////////////////////////////////////////
116 template <typename PointT> void
118  const PointCloud& cloud, const std::vector<int>& indices,
119  int k, std::vector< std::vector<int> >& k_indices,
120  std::vector< std::vector<float> >& k_sqr_distances) const
121 {
122  if (indices.empty ())
123  {
124  k_indices.resize (cloud.size ());
125  k_sqr_distances.resize (cloud.size ());
126  for (size_t i = 0; i < cloud.size (); i++)
127  nearestKSearch (cloud, static_cast<int> (i), k, k_indices[i], k_sqr_distances[i]);
128  }
129  else
130  {
131  k_indices.resize (indices.size ());
132  k_sqr_distances.resize (indices.size ());
133  for (size_t i = 0; i < indices.size (); i++)
134  nearestKSearch (cloud, indices[i], k, k_indices[i], k_sqr_distances[i]);
135  }
136 }
137 
138 ///////////////////////////////////////////////////////////////////////////////////////////
139 template <typename PointT> int
141  const PointCloud &cloud, int index, double radius,
142  std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
143  unsigned int max_nn) const
144 {
145  assert (index >= 0 && index < static_cast<int> (cloud.points.size ()) && "Out-of-bounds error in radiusSearch!");
146  return (radiusSearch(cloud.points[index], radius, k_indices, k_sqr_distances, max_nn));
147 }
148 
149 ///////////////////////////////////////////////////////////////////////////////////////////
150 template <typename PointT> int
152  int index, double radius, std::vector<int> &k_indices,
153  std::vector<float> &k_sqr_distances, unsigned int max_nn ) const
154 {
155  if (indices_ == NULL)
156  {
157  assert (index >= 0 && index < static_cast<int> (input_->points.size ()) && "Out-of-bounds error in radiusSearch!");
158  return (radiusSearch (input_->points[index], radius, k_indices, k_sqr_distances, max_nn));
159  }
160  else
161  {
162  assert (index >= 0 && index < static_cast<int> (indices_->size ()) && "Out-of-bounds error in radiusSearch!");
163  return (radiusSearch (input_->points[(*indices_)[index]], radius, k_indices, k_sqr_distances, max_nn));
164  }
165 }
166 
167 ///////////////////////////////////////////////////////////////////////////////////////////
168 template <typename PointT> void
170  const PointCloud& cloud,
171  const std::vector<int>& indices,
172  double radius,
173  std::vector< std::vector<int> >& k_indices,
174  std::vector< std::vector<float> > &k_sqr_distances,
175  unsigned int max_nn) const
176 {
177  if (indices.empty ())
178  {
179  k_indices.resize (cloud.size ());
180  k_sqr_distances.resize (cloud.size ());
181  for (size_t i = 0; i < cloud.size (); i++)
182  radiusSearch (cloud, static_cast<int> (i), radius,k_indices[i], k_sqr_distances[i], max_nn);
183  }
184  else
185  {
186  k_indices.resize (indices.size ());
187  k_sqr_distances.resize (indices.size ());
188  for (size_t i = 0; i < indices.size (); i++)
189  radiusSearch (cloud,indices[i],radius,k_indices[i],k_sqr_distances[i], max_nn);
190  }
191 }
192 
193 ///////////////////////////////////////////////////////////////////////////////////////////
194 template <typename PointT> void
196  std::vector<int>& indices, std::vector<float>& distances) const
197 {
198  std::vector<int> order (indices.size ());
199  for (size_t idx = 0; idx < order.size (); ++idx)
200  order [idx] = static_cast<int> (idx);
201 
202  Compare compare (distances);
203  sort (order.begin (), order.end (), compare);
204 
205  std::vector<int> sorted (indices.size ());
206  for (size_t idx = 0; idx < order.size (); ++idx)
207  sorted [idx] = indices[order [idx]];
208 
209  indices = sorted;
210 
211  // sort the according distances.
212  sort (distances.begin (), distances.end ());
213 }
214 
215 #define PCL_INSTANTIATE_Search(T) template class PCL_EXPORTS pcl::search::Search<T>;
216 
217 #endif //#ifndef _PCL_SEARCH_SEARCH_IMPL_HPP_
218 
219