Point Cloud Library (PCL)  1.9.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> void
76  const PointCloudConstPtr& cloud, const IndicesConstPtr &indices)
77 {
78  input_ = cloud;
79  indices_ = indices;
80 }
81 
82 
83 ///////////////////////////////////////////////////////////////////////////////////////////
84 template <typename PointT> int
86  const PointCloud &cloud, int index, int k,
87  std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const
88 {
89  assert (index >= 0 && index < static_cast<int> (cloud.points.size ()) && "Out-of-bounds error in nearestKSearch!");
90  return (nearestKSearch (cloud.points[index], k, k_indices, k_sqr_distances));
91 }
92 
93 ///////////////////////////////////////////////////////////////////////////////////////////
94 template <typename PointT> int
96  int index, int k,
97  std::vector<int> &k_indices,
98  std::vector<float> &k_sqr_distances) const
99 {
100  if (!indices_)
101  {
102  assert (index >= 0 && index < static_cast<int> (input_->points.size ()) && "Out-of-bounds error in nearestKSearch!");
103  return (nearestKSearch (input_->points[index], k, k_indices, k_sqr_distances));
104  }
105  assert (index >= 0 && index < static_cast<int> (indices_->size ()) && "Out-of-bounds error in nearestKSearch!");
106  if (index >= static_cast<int> (indices_->size ()) || index < 0)
107  return (0);
108  return (nearestKSearch (input_->points[(*indices_)[index]], k, k_indices, k_sqr_distances));
109 }
110 
111 ///////////////////////////////////////////////////////////////////////////////////////////
112 template <typename PointT> void
114  const PointCloud& cloud, const std::vector<int>& indices,
115  int k, std::vector< std::vector<int> >& k_indices,
116  std::vector< std::vector<float> >& k_sqr_distances) const
117 {
118  if (indices.empty ())
119  {
120  k_indices.resize (cloud.size ());
121  k_sqr_distances.resize (cloud.size ());
122  for (size_t i = 0; i < cloud.size (); i++)
123  nearestKSearch (cloud, static_cast<int> (i), k, k_indices[i], k_sqr_distances[i]);
124  }
125  else
126  {
127  k_indices.resize (indices.size ());
128  k_sqr_distances.resize (indices.size ());
129  for (size_t i = 0; i < indices.size (); i++)
130  nearestKSearch (cloud, indices[i], k, k_indices[i], k_sqr_distances[i]);
131  }
132 }
133 
134 ///////////////////////////////////////////////////////////////////////////////////////////
135 template <typename PointT> int
137  const PointCloud &cloud, int index, double radius,
138  std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
139  unsigned int max_nn) const
140 {
141  assert (index >= 0 && index < static_cast<int> (cloud.points.size ()) && "Out-of-bounds error in radiusSearch!");
142  return (radiusSearch(cloud.points[index], radius, k_indices, k_sqr_distances, max_nn));
143 }
144 
145 ///////////////////////////////////////////////////////////////////////////////////////////
146 template <typename PointT> int
148  int index, double radius, std::vector<int> &k_indices,
149  std::vector<float> &k_sqr_distances, unsigned int max_nn ) const
150 {
151  if (!indices_)
152  {
153  assert (index >= 0 && index < static_cast<int> (input_->points.size ()) && "Out-of-bounds error in radiusSearch!");
154  return (radiusSearch (input_->points[index], radius, k_indices, k_sqr_distances, max_nn));
155  }
156  assert (index >= 0 && index < static_cast<int> (indices_->size ()) && "Out-of-bounds error in radiusSearch!");
157  return (radiusSearch (input_->points[(*indices_)[index]], radius, k_indices, k_sqr_distances, max_nn));
158 }
159 
160 ///////////////////////////////////////////////////////////////////////////////////////////
161 template <typename PointT> void
163  const PointCloud& cloud,
164  const std::vector<int>& indices,
165  double radius,
166  std::vector< std::vector<int> >& k_indices,
167  std::vector< std::vector<float> > &k_sqr_distances,
168  unsigned int max_nn) const
169 {
170  if (indices.empty ())
171  {
172  k_indices.resize (cloud.size ());
173  k_sqr_distances.resize (cloud.size ());
174  for (size_t i = 0; i < cloud.size (); i++)
175  radiusSearch (cloud, static_cast<int> (i), radius,k_indices[i], k_sqr_distances[i], max_nn);
176  }
177  else
178  {
179  k_indices.resize (indices.size ());
180  k_sqr_distances.resize (indices.size ());
181  for (size_t i = 0; i < indices.size (); i++)
182  radiusSearch (cloud,indices[i],radius,k_indices[i],k_sqr_distances[i], max_nn);
183  }
184 }
185 
186 ///////////////////////////////////////////////////////////////////////////////////////////
187 template <typename PointT> void
189  std::vector<int>& indices, std::vector<float>& distances) const
190 {
191  std::vector<int> order (indices.size ());
192  for (size_t idx = 0; idx < order.size (); ++idx)
193  order [idx] = static_cast<int> (idx);
194 
195  Compare compare (distances);
196  sort (order.begin (), order.end (), compare);
197 
198  std::vector<int> sorted (indices.size ());
199  for (size_t idx = 0; idx < order.size (); ++idx)
200  sorted [idx] = indices[order [idx]];
201 
202  indices = sorted;
203 
204  // sort the according distances.
205  sort (distances.begin (), distances.end ());
206 }
207 
208 #define PCL_INSTANTIATE_Search(T) template class PCL_EXPORTS pcl::search::Search<T>;
209 
210 #endif //#ifndef _PCL_SEARCH_SEARCH_IMPL_HPP_
211 
212 
size_t size() const
Definition: point_cloud.h:449
virtual const std::string & getName() const
Returns the search method name.
Definition: search.hpp:54
virtual bool getSortedResults()
Gets whether the results should be sorted (ascending in the distance) or not Otherwise the results ma...
Definition: search.hpp:68
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:411
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
virtual int radiusSearch(const PointT &point, double radius, std::vector< int > &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.
PointCloudConstPtr input_
Definition: search.h:402
virtual void setSortedResults(bool sorted)
sets whether the results should be sorted (ascending in the distance) or not
Definition: search.hpp:61
IndicesConstPtr indices_
Definition: search.h:403
bool sorted_results_
Definition: search.h:404
std::string name_
Definition: search.h:405
PointCloud represents the base class in PCL for storing collections of 3D points. ...
virtual int nearestKSearch(const PointT &point, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const =0
Search for the k-nearest neighbors for the given query point.
void sortResults(std::vector< int > &indices, std::vector< float > &distances) const
Definition: search.hpp:188
boost::shared_ptr< const std::vector< int > > IndicesConstPtr
Definition: search.h:84
Search(const std::string &name="", bool sorted=false)
Constructor.
Definition: search.hpp:45
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: search.h:78