Point Cloud Library (PCL)  1.11.1-dev
organized_neighbor_search.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  * Author: Julius Kammerl (julius@kammerl.de)
35  */
36 
37 #pragma once
38 
39 #include <pcl/point_cloud.h>
40 #include <pcl/point_types.h>
41 
42 #include <algorithm>
43 #include <queue>
44 #include <vector>
45 
46 namespace pcl
47 {
48 
49  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
50  /** \brief @b OrganizedNeighborSearch class
51  * \note This class provides neighbor search routines for organized point clouds.
52  * \note
53  * \note typename: PointT: type of point used in pointcloud
54  * \author Julius Kammerl (julius@kammerl.de)
55  */
56  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
57  template<typename PointT>
59 
60  {
61  public:
62 
63  /** \brief OrganizedNeighborSearch constructor.
64  * */
68  {
69  max_distance_ = std::numeric_limits<double>::max ();
70 
71  focalLength_ = 1.0f;
72  }
73 
74  /** \brief Empty deconstructor. */
75  virtual
77  {
78  }
79 
80  // public typedefs
82  using PointCloudPtr = typename PointCloud::Ptr;
84 
85 
86  /** \brief Provide a pointer to the input data set.
87  * \param cloud_arg the const boost shared pointer to a PointCloud message
88  */
89  inline void
90  setInputCloud (const PointCloudConstPtr &cloud_arg)
91  {
92 
93  if (input_ != cloud_arg)
94  {
95  input_ = cloud_arg;
96 
98  generateRadiusLookupTable (input_->width, input_->height);
99  }
100  }
101 
102  /** \brief Search for all neighbors of query point that are within a given radius.
103  * \param cloud_arg the point cloud data
104  * \param index_arg the index in \a cloud representing the query point
105  * \param radius_arg the radius of the sphere bounding all of p_q's neighbors
106  * \param k_indices_arg the resultant indices of the neighboring points
107  * \param k_sqr_distances_arg the resultant squared distances to the neighboring points
108  * \param max_nn_arg if given, bounds the maximum returned neighbors to this value
109  * \return number of neighbors found in radius
110  */
111  int
112  radiusSearch (const PointCloudConstPtr &cloud_arg, int index_arg, double radius_arg,
113  std::vector<int> &k_indices_arg, std::vector<float> &k_sqr_distances_arg,
114  int max_nn_arg = INT_MAX);
115 
116  /** \brief Search for all neighbors of query point that are within a given radius.
117  * \param index_arg index representing the query point in the dataset given by \a setInputCloud.
118  * If indices were given in setInputCloud, index will be the position in the indices vector
119  * \param radius_arg radius of the sphere bounding all of p_q's neighbors
120  * \param k_indices_arg the resultant indices of the neighboring points
121  * \param k_sqr_distances_arg the resultant squared distances to the neighboring points
122  * \param max_nn_arg if given, bounds the maximum returned neighbors to this value
123  * \return number of neighbors found in radius
124  */
125  int
126  radiusSearch (int index_arg, const double radius_arg, std::vector<int> &k_indices_arg,
127  std::vector<float> &k_sqr_distances_arg, int max_nn_arg = INT_MAX) const;
128 
129  /** \brief Search for all neighbors of query point that are within a given radius.
130  * \param p_q_arg the given query point
131  * \param radius_arg the radius of the sphere bounding all of p_q's neighbors
132  * \param k_indices_arg the resultant indices of the neighboring points
133  * \param k_sqr_distances_arg the resultant squared distances to the neighboring points
134  * \param max_nn_arg if given, bounds the maximum returned neighbors to this value
135  * \return number of neighbors found in radius
136  */
137  int
138  radiusSearch (const PointT &p_q_arg, const double radius_arg, std::vector<int> &k_indices_arg,
139  std::vector<float> &k_sqr_distances_arg, int max_nn_arg = INT_MAX) const;
140 
141  /** \brief Search for k-nearest neighbors at the query point.
142  * \param cloud_arg the point cloud data
143  * \param index_arg the index in \a cloud representing the query point
144  * \param k_arg the number of neighbors to search for
145  * \param k_indices_arg the resultant indices of the neighboring points (must be resized to \a k a priori!)
146  * \param k_sqr_distances_arg the resultant squared distances to the neighboring points (must be resized to \a k
147  * a priori!)
148  * \return number of neighbors found
149  */
150  int
151  nearestKSearch (const PointCloudConstPtr &cloud_arg, int index_arg, int k_arg, std::vector<int> &k_indices_arg,
152  std::vector<float> &k_sqr_distances_arg);
153 
154  /** \brief Search for k-nearest neighbors at query point
155  * \param index_arg index representing the query point in the dataset given by \a setInputCloud.
156  * If indices were given in setInputCloud, index will be the position in the indices vector.
157  * \param k_arg the number of neighbors to search for
158  * \param k_indices_arg the resultant indices of the neighboring points (must be resized to \a k a priori!)
159  * \param k_sqr_distances_arg the resultant squared distances to the neighboring points (must be resized to \a k
160  * a priori!)
161  * \return number of neighbors found
162  */
163  int
164  nearestKSearch (int index_arg, int k_arg, std::vector<int> &k_indices_arg,
165  std::vector<float> &k_sqr_distances_arg);
166 
167  /** \brief Search for k-nearest neighbors at given query point.
168  * @param p_q_arg the given query point
169  * @param k_arg the number of neighbors to search for
170  * @param k_indices_arg the resultant indices of the neighboring points (must be resized to k a priori!)
171  * @param k_sqr_distances_arg the resultant squared distances to the neighboring points (must be resized to k a priori!)
172  * @return number of neighbors found
173  */
174  int
175  nearestKSearch (const PointT &p_q_arg, int k_arg, std::vector<int> &k_indices_arg,
176  std::vector<float> &k_sqr_distances_arg);
177 
178  /** \brief Get the maximum allowed distance between the query point and its nearest neighbors. */
179  inline double
180  getMaxDistance () const
181  {
182  return (max_distance_);
183  }
184 
185  /** \brief Set the maximum allowed distance between the query point and its nearest neighbors. */
186  inline void
187  setMaxDistance (double max_dist)
188  {
189  max_distance_ = max_dist;
190  }
191 
192  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
193  // Protected methods
194  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
195 
196  protected:
197 
198  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
199  /** \brief @b radiusSearchLoopkupEntry entry for radius search lookup vector
200  * \note This class defines entries for the radius search lookup vector
201  * \author Julius Kammerl (julius@kammerl.de)
202  */
203  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
205  {
206  public:
207 
208  /** \brief Empty constructor */
210  {
211  }
212 
213  /** \brief Empty deconstructor */
215  {
216  }
217 
218  /** \brief Define search point and calculate squared distance
219  * @param x_shift shift in x dimension
220  * @param y_shift shift in y dimension
221  */
222  void
223  defineShiftedSearchPoint(int x_shift, int y_shift)
224  {
225  x_diff_ =x_shift;
226  y_diff_ =y_shift;
227 
229  }
230 
231  /** \brief Operator< for comparing radiusSearchLoopkupEntry instances with each other. */
232  bool
233  operator< (const radiusSearchLoopkupEntry& rhs_arg) const
234  {
235  return (this->squared_distance_ < rhs_arg.squared_distance_);
236  }
237 
238  // Public globals
239  int x_diff_;
240  int y_diff_;
242 
243  };
244 
245  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
246  /** \brief @b nearestNeighborCandidate entry for the nearest neighbor candidate queue
247  * \note This class defines entries for the nearest neighbor candidate queue
248  * \author Julius Kammerl (julius@kammerl.de)
249  */
250  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
251 
253  {
254  public:
255 
256  /** \brief Empty constructor */
258  {
259  }
260 
261  /** \brief Empty deconstructor */
263  {
264  }
265 
266  /** \brief Operator< for comparing nearestNeighborCandidate instances with each other. */
267  bool
268  operator< (const nearestNeighborCandidate& rhs_arg) const
269  {
270  return (this->squared_distance_ < rhs_arg.squared_distance_);
271  }
272 
273  // Public globals
274  int index_;
276 
277  };
278 
279  /** \brief Get point at index from input pointcloud dataset
280  * \param index_arg index representing the point in the dataset given by \a setInputCloud
281  * \return PointT from input pointcloud dataset
282  */
283  const PointT&
284  getPointByIndex (const unsigned int index_arg) const;
285 
286  /** \brief Generate radius lookup table. It is used to subsequentially iterate over points
287  * which are close to the search point
288  * \param width of organized point cloud
289  * \param height of organized point cloud
290  */
291  void
292  generateRadiusLookupTable (unsigned int width, unsigned int height);
293 
294  inline void
295  pointPlaneProjection (const PointT& point, int& xpos, int& ypos) const
296  {
297  xpos = (int) pcl_round(point.x / (point.z * focalLength_));
298  ypos = (int) pcl_round(point.y / (point.z * focalLength_));
299  }
300 
301  void
302  getProjectedRadiusSearchBox (const PointT& point_arg, double squared_radius_arg, int& minX_arg, int& minY_arg, int& maxX_arg, int& maxY_arg ) const;
303 
304 
305  /** \brief Estimate focal length parameter that was used during point cloud generation
306  */
307  void
309 
310  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
311  /** \brief Class getName method. */
312  virtual std::string
313  getName () const
314  {
315  return ("Organized_Neighbor_Search");
316  }
317 
318  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
319  // Globals
320  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
321 
322  /** \brief Pointer to input point cloud dataset. */
324 
325  /** \brief Maximum allowed distance between the query point and its k-neighbors. */
327 
328  /** \brief Global focal length parameter */
329  double focalLength_;
330 
331  /** \brief Precalculated radius search lookup vector */
332  std::vector<radiusSearchLoopkupEntry> radiusSearchLookup_;
335 
336  };
337 
338 }
339 
340 //#include "organized_neighbor_search.hpp"
pcl::OrganizedNeighborSearch::radiusSearchLoopkupEntry::x_diff_
int x_diff_
Definition: organized_neighbor_search.h:239
pcl::OrganizedNeighborSearch::radiusSearchLoopkupEntry::squared_distance_
int squared_distance_
Definition: organized_neighbor_search.h:241
pcl
Definition: convolution.h:46
point_types.h
pcl::OrganizedNeighborSearch::OrganizedNeighborSearch
OrganizedNeighborSearch()
OrganizedNeighborSearch constructor.
Definition: organized_neighbor_search.h:65
pcl::OrganizedNeighborSearch::radiusSearch
int radiusSearch(const PointCloudConstPtr &cloud_arg, int index_arg, double radius_arg, std::vector< int > &k_indices_arg, std::vector< float > &k_sqr_distances_arg, int max_nn_arg=INT_MAX)
Search for all neighbors of query point that are within a given radius.
Definition: organized_neighbor_search.hpp:14
pcl::OrganizedNeighborSearch::setMaxDistance
void setMaxDistance(double max_dist)
Set the maximum allowed distance between the query point and its nearest neighbors.
Definition: organized_neighbor_search.h:187
pcl::OrganizedNeighborSearch::radiusSearchLoopkupEntry
radiusSearchLoopkupEntry entry for radius search lookup vector
Definition: organized_neighbor_search.h:204
pcl::OrganizedNeighborSearch::nearestNeighborCandidate::~nearestNeighborCandidate
~nearestNeighborCandidate()
Empty deconstructor
Definition: organized_neighbor_search.h:262
pcl::OrganizedNeighborSearch::PointCloudPtr
typename PointCloud::Ptr PointCloudPtr
Definition: organized_neighbor_search.h:82
pcl::OrganizedNeighborSearch::focalLength_
double focalLength_
Global focal length parameter.
Definition: organized_neighbor_search.h:329
pcl::OrganizedNeighborSearch::getProjectedRadiusSearchBox
void getProjectedRadiusSearchBox(const PointT &point_arg, double squared_radius_arg, int &minX_arg, int &minY_arg, int &maxX_arg, int &maxY_arg) const
Definition: organized_neighbor_search.hpp:91
pcl::OrganizedNeighborSearch::nearestNeighborCandidate
nearestNeighborCandidate entry for the nearest neighbor candidate queue
Definition: organized_neighbor_search.h:252
pcl::OrganizedNeighborSearch::estimateFocalLengthFromInputCloud
void estimateFocalLengthFromInputCloud()
Estimate focal length parameter that was used during point cloud generation.
Definition: organized_neighbor_search.hpp:337
pcl::OrganizedNeighborSearch::nearestNeighborCandidate::nearestNeighborCandidate
nearestNeighborCandidate()
Empty constructor
Definition: organized_neighbor_search.h:257
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: distances.h:55
pcl::OrganizedNeighborSearch::nearestNeighborCandidate::operator<
bool operator<(const nearestNeighborCandidate &rhs_arg) const
Operator< for comparing nearestNeighborCandidate instances with each other.
Definition: organized_neighbor_search.h:268
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:628
pcl::OrganizedNeighborSearch::radiusSearchLoopkupEntry::~radiusSearchLoopkupEntry
~radiusSearchLoopkupEntry()
Empty deconstructor
Definition: organized_neighbor_search.h:214
pcl::OrganizedNeighborSearch::getName
virtual std::string getName() const
Class getName method.
Definition: organized_neighbor_search.h:313
pcl::OrganizedNeighborSearch::PointCloudConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: organized_neighbor_search.h:83
pcl::OrganizedNeighborSearch::generateRadiusLookupTable
void generateRadiusLookupTable(unsigned int width, unsigned int height)
Generate radius lookup table.
Definition: organized_neighbor_search.hpp:366
pcl::OrganizedNeighborSearch::getMaxDistance
double getMaxDistance() const
Get the maximum allowed distance between the query point and its nearest neighbors.
Definition: organized_neighbor_search.h:180
pcl::OrganizedNeighborSearch::radiusLookupTableWidth_
int radiusLookupTableWidth_
Definition: organized_neighbor_search.h:333
pcl::OrganizedNeighborSearch
OrganizedNeighborSearch class
Definition: organized_neighbor_search.h:58
pcl::OrganizedNeighborSearch::~OrganizedNeighborSearch
virtual ~OrganizedNeighborSearch()
Empty deconstructor.
Definition: organized_neighbor_search.h:76
pcl::OrganizedNeighborSearch::max_distance_
double max_distance_
Maximum allowed distance between the query point and its k-neighbors.
Definition: organized_neighbor_search.h:326
pcl::OrganizedNeighborSearch::radiusLookupTableHeight_
int radiusLookupTableHeight_
Definition: organized_neighbor_search.h:334
pcl::OrganizedNeighborSearch::getPointByIndex
const PointT & getPointByIndex(const unsigned int index_arg) const
Get point at index from input pointcloud dataset.
Definition: organized_neighbor_search.hpp:392
pcl::OrganizedNeighborSearch::radiusSearchLoopkupEntry::defineShiftedSearchPoint
void defineShiftedSearchPoint(int x_shift, int y_shift)
Define search point and calculate squared distance.
Definition: organized_neighbor_search.h:223
pcl::OrganizedNeighborSearch::radiusSearchLookup_
std::vector< radiusSearchLoopkupEntry > radiusSearchLookup_
Precalculated radius search lookup vector.
Definition: organized_neighbor_search.h:332
pcl::OrganizedNeighborSearch::nearestKSearch
int nearestKSearch(const PointCloudConstPtr &cloud_arg, int index_arg, int k_arg, std::vector< int > &k_indices_arg, std::vector< float > &k_sqr_distances_arg)
Search for k-nearest neighbors at the query point.
Definition: organized_neighbor_search.hpp:149
pcl::OrganizedNeighborSearch::nearestNeighborCandidate::index_
int index_
Definition: organized_neighbor_search.h:274
pcl_round
__inline double pcl_round(double number)
Win32 doesn't seem to have rounding functions.
Definition: pcl_macros.h:239
pcl::OrganizedNeighborSearch::setInputCloud
void setInputCloud(const PointCloudConstPtr &cloud_arg)
Provide a pointer to the input data set.
Definition: organized_neighbor_search.h:90
pcl::OrganizedNeighborSearch::nearestNeighborCandidate::squared_distance_
double squared_distance_
Definition: organized_neighbor_search.h:275
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:407
pcl::OrganizedNeighborSearch::pointPlaneProjection
void pointPlaneProjection(const PointT &point, int &xpos, int &ypos) const
Definition: organized_neighbor_search.h:295
pcl::OrganizedNeighborSearch::input_
PointCloudConstPtr input_
Pointer to input point cloud dataset.
Definition: organized_neighbor_search.h:323
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:408
pcl::OrganizedNeighborSearch::radiusSearchLoopkupEntry::y_diff_
int y_diff_
Definition: organized_neighbor_search.h:240
pcl::OrganizedNeighborSearch::radiusSearchLoopkupEntry::radiusSearchLoopkupEntry
radiusSearchLoopkupEntry()
Empty constructor
Definition: organized_neighbor_search.h:209
pcl::OrganizedNeighborSearch::radiusSearchLoopkupEntry::operator<
bool operator<(const radiusSearchLoopkupEntry &rhs_arg) const
Operator< for comparing radiusSearchLoopkupEntry instances with each other.
Definition: organized_neighbor_search.h:233