Point Cloud Library (PCL)  1.9.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
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 #ifndef POINTCLOUD_DEPTH_NEIGHBOR_SEARCH_H
38 #define POINTCLOUD_DEPTH_NEIGHBOR_SEARCH_H
39 
40 #include <pcl/point_cloud.h>
41 #include <pcl/point_types.h>
42 
43 #include <algorithm>
44 #include <queue>
45 #include <vector>
46 
47 namespace pcl
48 {
49 
50  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
51  /** \brief @b OrganizedNeighborSearch class
52  * \note This class provides neighbor search routines for organized point clouds.
53  * \note
54  * \note typename: PointT: type of point used in pointcloud
55  * \author Julius Kammerl (julius@kammerl.de)
56  */
57  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
58  template<typename PointT>
60 
61  {
62  public:
63 
64  /** \brief OrganizedNeighborSearch constructor.
65  * */
69  {
70  max_distance_ = std::numeric_limits<double>::max ();
71 
72  focalLength_ = 1.0f;
73  }
74 
75  /** \brief Empty deconstructor. */
76  virtual
78  {
79  }
80 
81  // public typedefs
83  typedef boost::shared_ptr<PointCloud> PointCloudPtr;
84  typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
85 
86 
87  /** \brief Provide a pointer to the input data set.
88  * \param cloud_arg the const boost shared pointer to a PointCloud message
89  */
90  inline void
91  setInputCloud (const PointCloudConstPtr &cloud_arg)
92  {
93 
94  if (input_ != cloud_arg)
95  {
96  input_ = cloud_arg;
97 
99  generateRadiusLookupTable (input_->width, input_->height);
100  }
101  }
102 
103  /** \brief Search for all neighbors of query point that are within a given radius.
104  * \param cloud_arg the point cloud data
105  * \param index_arg the index in \a cloud representing the query point
106  * \param radius_arg the radius of the sphere bounding all of p_q's neighbors
107  * \param k_indices_arg the resultant indices of the neighboring points
108  * \param k_sqr_distances_arg the resultant squared distances to the neighboring points
109  * \param max_nn_arg if given, bounds the maximum returned neighbors to this value
110  * \return number of neighbors found in radius
111  */
112  int
113  radiusSearch (const PointCloudConstPtr &cloud_arg, int index_arg, double radius_arg,
114  std::vector<int> &k_indices_arg, std::vector<float> &k_sqr_distances_arg,
115  int max_nn_arg = INT_MAX);
116 
117  /** \brief Search for all neighbors of query point that are within a given radius.
118  * \param index_arg index representing the query point in the dataset given by \a setInputCloud.
119  * If indices were given in setInputCloud, index will be the position in the indices vector
120  * \param radius_arg radius of the sphere bounding all of p_q's neighbors
121  * \param k_indices_arg the resultant indices of the neighboring points
122  * \param k_sqr_distances_arg the resultant squared distances to the neighboring points
123  * \param max_nn_arg if given, bounds the maximum returned neighbors to this value
124  * \return number of neighbors found in radius
125  */
126  int
127  radiusSearch (int index_arg, const double radius_arg, std::vector<int> &k_indices_arg,
128  std::vector<float> &k_sqr_distances_arg, int max_nn_arg = INT_MAX) const;
129 
130  /** \brief Search for all neighbors of query point that are within a given radius.
131  * \param p_q_arg the given query point
132  * \param radius_arg the radius of the sphere bounding all of p_q's neighbors
133  * \param k_indices_arg the resultant indices of the neighboring points
134  * \param k_sqr_distances_arg the resultant squared distances to the neighboring points
135  * \param max_nn_arg if given, bounds the maximum returned neighbors to this value
136  * \return number of neighbors found in radius
137  */
138  int
139  radiusSearch (const PointT &p_q_arg, const double radius_arg, std::vector<int> &k_indices_arg,
140  std::vector<float> &k_sqr_distances_arg, int max_nn_arg = INT_MAX) const;
141 
142  /** \brief Search for k-nearest neighbors at the query point.
143  * \param cloud_arg the point cloud data
144  * \param index_arg the index in \a cloud representing the query point
145  * \param k_arg the number of neighbors to search for
146  * \param k_indices_arg the resultant indices of the neighboring points (must be resized to \a k a priori!)
147  * \param k_sqr_distances_arg the resultant squared distances to the neighboring points (must be resized to \a k
148  * a priori!)
149  * \return number of neighbors found
150  */
151  int
152  nearestKSearch (const PointCloudConstPtr &cloud_arg, int index_arg, int k_arg, std::vector<int> &k_indices_arg,
153  std::vector<float> &k_sqr_distances_arg);
154 
155  /** \brief Search for k-nearest neighbors at query point
156  * \param index_arg index representing the query point in the dataset given by \a setInputCloud.
157  * If indices were given in setInputCloud, index will be the position in the indices vector.
158  * \param k_arg the number of neighbors to search for
159  * \param k_indices_arg the resultant indices of the neighboring points (must be resized to \a k a priori!)
160  * \param k_sqr_distances_arg the resultant squared distances to the neighboring points (must be resized to \a k
161  * a priori!)
162  * \return number of neighbors found
163  */
164  int
165  nearestKSearch (int index_arg, int k_arg, std::vector<int> &k_indices_arg,
166  std::vector<float> &k_sqr_distances_arg);
167 
168  /** \brief Search for k-nearest neighbors at given query point.
169  * @param p_q_arg the given query point
170  * @param k_arg the number of neighbors to search for
171  * @param k_indices_arg the resultant indices of the neighboring points (must be resized to k a priori!)
172  * @param k_sqr_distances_arg the resultant squared distances to the neighboring points (must be resized to k a priori!)
173  * @return number of neighbors found
174  */
175  int
176  nearestKSearch (const PointT &p_q_arg, int k_arg, std::vector<int> &k_indices_arg,
177  std::vector<float> &k_sqr_distances_arg);
178 
179  /** \brief Get the maximum allowed distance between the query point and its nearest neighbors. */
180  inline double
181  getMaxDistance () const
182  {
183  return (max_distance_);
184  }
185 
186  /** \brief Set the maximum allowed distance between the query point and its nearest neighbors. */
187  inline void
188  setMaxDistance (double max_dist)
189  {
190  max_distance_ = max_dist;
191  }
192 
193  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
194  // Protected methods
195  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
196 
197  protected:
198 
199  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
200  /** \brief @b radiusSearchLoopkupEntry entry for radius search lookup vector
201  * \note This class defines entries for the radius search lookup vector
202  * \author Julius Kammerl (julius@kammerl.de)
203  */
204  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
206  {
207  public:
208 
209  /** \brief Empty constructor */
211  {
212  }
213 
214  /** \brief Empty deconstructor */
216  {
217  }
218 
219  /** \brief Define search point and calculate squared distance
220  * @param x_shift shift in x dimension
221  * @param y_shift shift in y dimension
222  */
223  void
224  defineShiftedSearchPoint(int x_shift, int y_shift)
225  {
226  x_diff_ =x_shift;
227  y_diff_ =y_shift;
228 
230  }
231 
232  /** \brief Operator< for comparing radiusSearchLoopkupEntry instances with each other. */
233  bool
234  operator< (const radiusSearchLoopkupEntry& rhs_arg) const
235  {
236  return (this->squared_distance_ < rhs_arg.squared_distance_);
237  }
238 
239  // Public globals
240  int x_diff_;
241  int y_diff_;
243 
244  };
245 
246  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
247  /** \brief @b nearestNeighborCandidate entry for the nearest neighbor candidate queue
248  * \note This class defines entries for the nearest neighbor candidate queue
249  * \author Julius Kammerl (julius@kammerl.de)
250  */
251  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
252 
254  {
255  public:
256 
257  /** \brief Empty constructor */
259  {
260  }
261 
262  /** \brief Empty deconstructor */
264  {
265  }
266 
267  /** \brief Operator< for comparing nearestNeighborCandidate instances with each other. */
268  bool
269  operator< (const nearestNeighborCandidate& rhs_arg) const
270  {
271  return (this->squared_distance_ < rhs_arg.squared_distance_);
272  }
273 
274  // Public globals
275  int index_;
277 
278  };
279 
280  /** \brief Get point at index from input pointcloud dataset
281  * \param index_arg index representing the point in the dataset given by \a setInputCloud
282  * \return PointT from input pointcloud dataset
283  */
284  const PointT&
285  getPointByIndex (const unsigned int index_arg) const;
286 
287  /** \brief Generate radius lookup table. It is used to subsequentially iterate over points
288  * which are close to the search point
289  * \param width of organized point cloud
290  * \param height of organized point cloud
291  */
292  void
293  generateRadiusLookupTable (unsigned int width, unsigned int height);
294 
295  inline void
296  pointPlaneProjection (const PointT& point, int& xpos, int& ypos) const
297  {
298  xpos = (int) pcl_round(point.x / (point.z * focalLength_));
299  ypos = (int) pcl_round(point.y / (point.z * focalLength_));
300  }
301 
302  void
303  getProjectedRadiusSearchBox (const PointT& point_arg, double squared_radius_arg, int& minX_arg, int& minY_arg, int& maxX_arg, int& maxY_arg ) const;
304 
305 
306  /** \brief Estimate focal length parameter that was used during point cloud generation
307  */
308  void
310 
311  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
312  /** \brief Class getName method. */
313  virtual std::string
314  getName () const
315  {
316  return ("Organized_Neighbor_Search");
317  }
318 
319  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
320  // Globals
321  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
322 
323  /** \brief Pointer to input point cloud dataset. */
324  PointCloudConstPtr input_;
325 
326  /** \brief Maximum allowed distance between the query point and its k-neighbors. */
328 
329  /** \brief Global focal length parameter */
330  double focalLength_;
331 
332  /** \brief Precalculated radius search lookup vector */
333  std::vector<radiusSearchLoopkupEntry> radiusSearchLookup_;
336 
337  };
338 
339 }
340 
341 //#include "organized_neighbor_search.hpp"
342 
343 #endif
344 
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.
void getProjectedRadiusSearchBox(const PointT &point_arg, double squared_radius_arg, int &minX_arg, int &minY_arg, int &maxX_arg, int &maxY_arg) const
void setInputCloud(const PointCloudConstPtr &cloud_arg)
Provide a pointer to the input data set.
boost::shared_ptr< PointCloud > PointCloudPtr
int x_diff_
void setMaxDistance(double max_dist)
Set the maximum allowed distance between the query point and its nearest neighbors.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
pcl::PointCloud< PointT > PointCloud
virtual std::string getName() const
Class getName method.
radiusSearchLoopkupEntry entry for radius search lookup vector
bool operator<(const radiusSearchLoopkupEntry &rhs_arg) const
Operator< for comparing radiusSearchLoopkupEntry instances with each other.
~radiusSearchLoopkupEntry()
Empty deconstructor.
void defineShiftedSearchPoint(int x_shift, int y_shift)
Define search point and calculate squared distance.
OrganizedNeighborSearch class
PointCloudConstPtr input_
Pointer to input point cloud dataset.
virtual ~OrganizedNeighborSearch()
Empty deconstructor.
std::vector< radiusSearchLoopkupEntry > radiusSearchLookup_
Precalculated radius search lookup vector.
double max_distance_
Maximum allowed distance between the query point and its k-neighbors.
int y_diff_
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.
boost::shared_ptr< const PointCloud > PointCloudConstPtr
PointCloud represents the base class in PCL for storing collections of 3D points. ...
double focalLength_
Global focal length parameter.
void pointPlaneProjection(const PointT &point, int &xpos, int &ypos) const
const PointT & getPointByIndex(const unsigned int index_arg) const
Get point at index from input pointcloud dataset.
OrganizedNeighborSearch()
OrganizedNeighborSearch constructor.
void generateRadiusLookupTable(unsigned int width, unsigned int height)
Generate radius lookup table.
bool operator<(const nearestNeighborCandidate &rhs_arg) const
Operator< for comparing nearestNeighborCandidate instances with each other.
A point structure representing Euclidean xyz coordinates, and the RGB color.
nearestNeighborCandidate entry for the nearest neighbor candidate queue
void estimateFocalLengthFromInputCloud()
Estimate focal length parameter that was used during point cloud generation.
double getMaxDistance() const
Get the maximum allowed distance between the query point and its nearest neighbors.
int squared_distance_
radiusSearchLoopkupEntry()
Empty constructor.