Point Cloud Library (PCL)  1.12.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 <limits>
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  using PointCloudPtr = typename PointCloud::Ptr;
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 = std::numeric_limits<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 = std::numeric_limits<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 = std::numeric_limits<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. */
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"
nearestNeighborCandidate entry for the nearest neighbor candidate queue
bool operator<(const nearestNeighborCandidate &rhs_arg) const
Operator< for comparing nearestNeighborCandidate instances with each other.
radiusSearchLoopkupEntry entry for radius search lookup vector
int squared_distance_
int y_diff_
bool operator<(const radiusSearchLoopkupEntry &rhs_arg) const
Operator< for comparing radiusSearchLoopkupEntry instances with each other.
~radiusSearchLoopkupEntry()
Empty deconstructor
radiusSearchLoopkupEntry()
Empty constructor
void defineShiftedSearchPoint(int x_shift, int y_shift)
Define search point and calculate squared distance.
int x_diff_
OrganizedNeighborSearch class
PointCloudConstPtr input_
Pointer to input point cloud dataset.
std::vector< radiusSearchLoopkupEntry > radiusSearchLookup_
Precalculated radius search lookup vector.
void getProjectedRadiusSearchBox(const PointT &point_arg, double squared_radius_arg, int &minX_arg, int &minY_arg, int &maxX_arg, int &maxY_arg) const
virtual ~OrganizedNeighborSearch()
Empty deconstructor.
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 setMaxDistance(double max_dist)
Set the maximum allowed distance between the query point and its nearest neighbors.
void estimateFocalLengthFromInputCloud()
Estimate focal length parameter that was used during point cloud generation.
typename PointCloud::ConstPtr PointCloudConstPtr
OrganizedNeighborSearch()
OrganizedNeighborSearch constructor.
double getMaxDistance() const
Get the maximum allowed distance between the query point and its nearest neighbors.
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=std::numeric_limits< int >::max())
Search for all neighbors of query point that are within a given radius.
virtual std::string getName() const
Class getName method.
typename PointCloud::Ptr PointCloudPtr
double max_distance_
Maximum allowed distance between the query point and its k-neighbors.
void pointPlaneProjection(const PointT &point, int &xpos, int &ypos) const
double focalLength_
Global focal length parameter.
void generateRadiusLookupTable(unsigned int width, unsigned int height)
Generate radius lookup table.
void setInputCloud(const PointCloudConstPtr &cloud_arg)
Provide a pointer to the input data set.
const PointT & getPointByIndex(const unsigned int index_arg) const
Get point at index from input pointcloud dataset.
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
Defines all the PCL implemented PointT point type structures.
__inline double pcl_round(double number)
Win32 doesn't seem to have rounding functions.
Definition: pcl_macros.h:239
A point structure representing Euclidean xyz coordinates, and the RGB color.