Point Cloud Library (PCL) 1.13.1
Loading...
Searching...
No Matches
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///////////////////////////////////////////////////////////////////////////////////////////
44template <typename PointT>
45pcl::search::Search<PointT>::Search (const std::string& name, bool sorted)
46 : input_ ()
47 , sorted_results_ (sorted)
48 , name_ (name)
49{
50}
51
52///////////////////////////////////////////////////////////////////////////////////////////
53template <typename PointT> const std::string&
55{
56 return (name_);
57}
58
59///////////////////////////////////////////////////////////////////////////////////////////
60template <typename PointT> void
62{
63 sorted_results_ = sorted;
64}
65
66///////////////////////////////////////////////////////////////////////////////////////////
67template <typename PointT> bool
69{
70 return (sorted_results_);
71}
72
73///////////////////////////////////////////////////////////////////////////////////////////
74template <typename PointT> void
76 const PointCloudConstPtr& cloud, const IndicesConstPtr &indices)
77{
78 input_ = cloud;
79 indices_ = indices;
80}
81
82
83///////////////////////////////////////////////////////////////////////////////////////////
84template <typename PointT> int
86 const PointCloud &cloud, index_t index, int k,
87 Indices &k_indices, std::vector<float> &k_sqr_distances) const
89 assert (index >= 0 && index < static_cast<index_t> (cloud.size ()) && "Out-of-bounds error in nearestKSearch!");
90 return (nearestKSearch (cloud[index], k, k_indices, k_sqr_distances));
91}
92
93///////////////////////////////////////////////////////////////////////////////////////////
94template <typename PointT> int
96 index_t index, int k,
97 Indices &k_indices,
98 std::vector<float> &k_sqr_distances) const
99{
100 if (!indices_)
101 {
102 assert (index >= 0 && index < static_cast<index_t> (input_->size ()) && "Out-of-bounds error in nearestKSearch!");
103 return (nearestKSearch ((*input_)[index], k, k_indices, k_sqr_distances));
105 assert (index >= 0 && index < static_cast<index_t> (indices_->size ()) && "Out-of-bounds error in nearestKSearch!");
106 if (index >= static_cast<index_t> (indices_->size ()) || index < 0)
107 return (0);
108 return (nearestKSearch ((*input_)[(*indices_)[index]], k, k_indices, k_sqr_distances));
109}
111///////////////////////////////////////////////////////////////////////////////////////////
112template <typename PointT> void
114 const PointCloud& cloud, const Indices& indices,
115 int k, std::vector<Indices>& 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 (std::size_t i = 0; i < cloud.size (); i++)
123 nearestKSearch (cloud, static_cast<index_t> (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 (std::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///////////////////////////////////////////////////////////////////////////////////////////
135template <typename PointT> int
137 const PointCloud &cloud, index_t index, double radius,
138 Indices &k_indices, std::vector<float> &k_sqr_distances,
139 unsigned int max_nn) const
140{
141 assert (index >= 0 && index < static_cast<index_t> (cloud.size ()) && "Out-of-bounds error in radiusSearch!");
142 return (radiusSearch(cloud[index], radius, k_indices, k_sqr_distances, max_nn));
143}
144
145///////////////////////////////////////////////////////////////////////////////////////////
146template <typename PointT> int
148 index_t index, double radius, Indices &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<index_t> (input_->size ()) && "Out-of-bounds error in radiusSearch!");
154 return (radiusSearch ((*input_)[index], radius, k_indices, k_sqr_distances, max_nn));
155 }
156 assert (index >= 0 && index < static_cast<index_t> (indices_->size ()) && "Out-of-bounds error in radiusSearch!");
157 return (radiusSearch ((*input_)[(*indices_)[index]], radius, k_indices, k_sqr_distances, max_nn));
158}
159
160///////////////////////////////////////////////////////////////////////////////////////////
161template <typename PointT> void
163 const PointCloud& cloud,
164 const Indices& indices,
165 double radius,
166 std::vector<Indices>& 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 (std::size_t i = 0; i < cloud.size (); i++)
175 radiusSearch (cloud, static_cast<index_t> (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 (std::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///////////////////////////////////////////////////////////////////////////////////////////
187template <typename PointT> void
189 Indices& indices, std::vector<float>& distances) const
190{
191 Indices order (indices.size ());
192 for (std::size_t idx = 0; idx < order.size (); ++idx)
193 order [idx] = static_cast<index_t> (idx);
194
195 Compare compare (distances);
196 sort (order.begin (), order.end (), compare);
197
198 Indices sorted (indices.size ());
199 for (std::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
PointCloud represents the base class in PCL for storing collections of 3D points.
std::size_t size() const
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 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
typename PointCloud::ConstPtr PointCloudConstPtr
Definition search.h:79
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