38 #ifndef PCL_SEARCH_IMPL_BRUTE_FORCE_SEARCH_H_ 39 #define PCL_SEARCH_IMPL_BRUTE_FORCE_SEARCH_H_ 41 #include <pcl/search/brute_force.h> 45 template <
typename Po
intT>
float 49 return (point1.getVector3fMap () - point2.getVector3fMap ()).squaredNorm ();
53 template <
typename Po
intT>
int 55 const PointT& point,
int k, std::vector<int>& k_indices, std::vector<float>& k_distances)
const 57 assert (
isFinite (point) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
65 return denseKSearch (point, k, k_indices, k_distances);
67 return sparseKSearch (point, k, k_indices, k_distances);
71 template <
typename Po
intT>
int 73 const PointT &point,
int k, std::vector<int> &k_indices, std::vector<float> &k_distances)
const 76 std::vector<Entry> result;
78 std::priority_queue<Entry> queue;
81 std::vector<int>::const_iterator iIt =indices_->begin ();
82 std::vector<int>::const_iterator iEnd = indices_->begin () + std::min (static_cast<unsigned> (k), static_cast<unsigned> (indices_->size ()));
83 for (; iIt != iEnd; ++iIt)
84 result.push_back (Entry (*iIt, getDistSqr (input_->points[*iIt], point)));
86 queue = std::priority_queue<Entry> (result.begin (), result.end ());
90 for (; iIt != indices_->end (); ++iIt)
92 entry.distance = getDistSqr (input_->points[*iIt], point);
93 if (queue.top ().distance > entry.distance)
104 for (entry.index = 0; entry.index < std::min (static_cast<unsigned> (k), static_cast<unsigned> (input_->size ())); ++entry.index)
106 entry.distance = getDistSqr (input_->points[entry.index], point);
107 result.push_back (entry);
110 queue = std::priority_queue<Entry> (result.begin (), result.end ());
113 for (; entry.index < input_->size (); ++entry.index)
115 entry.distance = getDistSqr (input_->points[entry.index], point);
116 if (queue.top ().distance > entry.distance)
124 k_indices.resize (queue.size ());
125 k_distances.resize (queue.size ());
126 size_t idx = queue.size () - 1;
127 while (!queue.empty ())
129 k_indices [idx] = queue.top ().index;
130 k_distances [idx] = queue.top ().distance;
135 return (static_cast<int> (k_indices.size ()));
139 template <
typename Po
intT>
int 141 const PointT &point,
int k, std::vector<int> &k_indices, std::vector<float> &k_distances)
const 144 std::vector<Entry> result;
147 std::priority_queue<Entry> queue;
148 if (indices_ != NULL)
150 std::vector<int>::const_iterator iIt =indices_->begin ();
151 for (; iIt != indices_->end () && result.size () < static_cast<unsigned> (k); ++iIt)
153 if (pcl_isfinite (input_->points[*iIt].x))
154 result.push_back (Entry (*iIt, getDistSqr (input_->points[*iIt], point)));
157 queue = std::priority_queue<Entry> (result.begin (), result.end ());
162 for (; iIt != indices_->end (); ++iIt)
164 if (!pcl_isfinite (input_->points[*iIt].x))
167 entry.distance = getDistSqr (input_->points[*iIt], point);
168 if (queue.top ().distance > entry.distance)
179 for (entry.index = 0; entry.index < input_->size () && result.size () < static_cast<unsigned> (k); ++entry.index)
181 if (pcl_isfinite (input_->points[entry.index].x))
183 entry.distance = getDistSqr (input_->points[entry.index], point);
184 result.push_back (entry);
187 queue = std::priority_queue<Entry> (result.begin (), result.end ());
190 for (; entry.index < input_->size (); ++entry.index)
192 if (!pcl_isfinite (input_->points[entry.index].x))
195 entry.distance = getDistSqr (input_->points[entry.index], point);
196 if (queue.top ().distance > entry.distance)
204 k_indices.resize (queue.size ());
205 k_distances.resize (queue.size ());
206 size_t idx = queue.size () - 1;
207 while (!queue.empty ())
209 k_indices [idx] = queue.top ().index;
210 k_distances [idx] = queue.top ().distance;
214 return (static_cast<int> (k_indices.size ()));
218 template <
typename Po
intT>
int 220 const PointT& point,
double radius,
221 std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
222 unsigned int max_nn)
const 226 size_t reserve = max_nn;
229 if (indices_ != NULL)
230 reserve = std::min (indices_->size (), input_->size ());
232 reserve = input_->size ();
234 k_indices.reserve (reserve);
235 k_sqr_distances.reserve (reserve);
237 if (indices_ != NULL)
239 for (std::vector<int>::const_iterator iIt =indices_->begin (); iIt != indices_->end (); ++iIt)
241 distance = getDistSqr (input_->points[*iIt], point);
244 k_indices.push_back (*iIt);
245 k_sqr_distances.push_back (
distance);
246 if (k_indices.size () == max_nn)
253 for (
unsigned index = 0; index < input_->size (); ++index)
255 distance = getDistSqr (input_->points[index], point);
258 k_indices.push_back (index);
259 k_sqr_distances.push_back (
distance);
260 if (k_indices.size () == max_nn)
267 this->sortResults (k_indices, k_sqr_distances);
269 return (static_cast<int> (k_indices.size ()));
273 template <
typename Po
intT>
int 275 const PointT& point,
double radius,
276 std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
277 unsigned int max_nn)
const 281 size_t reserve = max_nn;
284 if (indices_ != NULL)
285 reserve = std::min (indices_->size (), input_->size ());
287 reserve = input_->size ();
289 k_indices.reserve (reserve);
290 k_sqr_distances.reserve (reserve);
293 if (indices_ != NULL)
295 for (std::vector<int>::const_iterator iIt =indices_->begin (); iIt != indices_->end (); ++iIt)
297 if (!pcl_isfinite (input_->points[*iIt].x))
300 distance = getDistSqr (input_->points[*iIt], point);
303 k_indices.push_back (*iIt);
304 k_sqr_distances.push_back (
distance);
305 if (k_indices.size () == max_nn)
312 for (
unsigned index = 0; index < input_->size (); ++index)
314 if (!pcl_isfinite (input_->points[index].x))
316 distance = getDistSqr (input_->points[index], point);
319 k_indices.push_back (index);
320 k_sqr_distances.push_back (
distance);
321 if (k_indices.size () == max_nn)
328 this->sortResults (k_indices, k_sqr_distances);
330 return (static_cast<int> (k_indices.size ()));
334 template <
typename Po
intT>
int 336 const PointT& point,
double radius, std::vector<int> &k_indices,
337 std::vector<float> &k_sqr_distances,
unsigned int max_nn)
const 339 assert (
isFinite (point) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
342 k_sqr_distances.clear ();
346 if (input_->is_dense)
347 return denseRadiusSearch (point, radius, k_indices, k_sqr_distances, max_nn);
349 return sparseRadiusSearch (point, radius, k_indices, k_sqr_distances, max_nn);
352 #define PCL_INSTANTIATE_BruteForce(T) template class PCL_EXPORTS pcl::search::BruteForce<T>; 354 #endif //PCL_SEARCH_IMPL_BRUTE_FORCE_SEARCH_H_ bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
int radiusSearch(const PointT &point, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const
Search for all the nearest neighbors of the query point in a given radius.
Implementation of a simple brute force search algorithm.
int nearestKSearch(const PointT &point, int k, std::vector< int > &k_indices, std::vector< float > &k_distances) const
Search for the k-nearest neighbors for the given query point.
float distance(const PointT &p1, const PointT &p2)
A point structure representing Euclidean xyz coordinates, and the RGB color.