Point Cloud Library (PCL)  1.9.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 = boost::shared_ptr<PointCloud>;
83  using PointCloudConstPtr = boost::shared_ptr<const PointCloud>;
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"
double getMaxDistance() const
Get the maximum allowed distance between the query point and its nearest neighbors.
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 setInputCloud(const PointCloudConstPtr &cloud_arg)
Provide a pointer to the input data set.
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
boost::shared_ptr< PointCloud > PointCloudPtr
__inline double pcl_round(double number)
Win32 doesn&#39;t seem to have rounding functions.
Definition: pcl_macros.h:154
radiusSearchLoopkupEntry entry for radius search lookup vector
const PointT & getPointByIndex(const unsigned int index_arg) const
Get point at index from input pointcloud dataset.
~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.
Defines all the PCL implemented PointT point type structures.
std::vector< radiusSearchLoopkupEntry > radiusSearchLookup_
Precalculated radius search lookup vector.
void pointPlaneProjection(const PointT &point, int &xpos, int &ypos) const
boost::shared_ptr< const PointCloud > PointCloudConstPtr
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.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
double focalLength_
Global focal length parameter.
OrganizedNeighborSearch()
OrganizedNeighborSearch constructor.
void generateRadiusLookupTable(unsigned int width, unsigned int height)
Generate radius lookup table.
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.
int squared_distance_
radiusSearchLoopkupEntry()
Empty constructor.
bool operator<(const radiusSearchLoopkupEntry &rhs_arg) const
Operator< for comparing radiusSearchLoopkupEntry instances with each other.
void getProjectedRadiusSearchBox(const PointT &point_arg, double squared_radius_arg, int &minX_arg, int &minY_arg, int &maxX_arg, int &maxY_arg) const
virtual std::string getName() const
Class getName method.