Point Cloud Library (PCL) 1.15.0
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> bool
76 const PointCloudConstPtr& cloud, const IndicesConstPtr &indices)
77{
78 input_ = cloud;
79 indices_ = indices;
80 return true;
81}
82
83
84///////////////////////////////////////////////////////////////////////////////////////////
85template <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///////////////////////////////////////////////////////////////////////////////////////////
95template <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));
111
112///////////////////////////////////////////////////////////////////////////////////////////
113template <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///////////////////////////////////////////////////////////////////////////////////////////
136template <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///////////////////////////////////////////////////////////////////////////////////////////
147template <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///////////////////////////////////////////////////////////////////////////////////////////
162template <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///////////////////////////////////////////////////////////////////////////////////////////
188template <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.
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
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