Point Cloud Library (PCL)  1.14.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  // public typedefs
81  using PointCloudPtr = typename PointCloud::Ptr;
83 
84 
85  /** \brief Provide a pointer to the input data set.
86  * \param cloud_arg the const boost shared pointer to a PointCloud message
87  */
88  inline void
89  setInputCloud (const PointCloudConstPtr &cloud_arg)
90  {
91 
92  if (input_ != cloud_arg)
93  {
94  input_ = cloud_arg;
95 
97  generateRadiusLookupTable (input_->width, input_->height);
98  }
99  }
100 
101  /** \brief Search for all neighbors of query point that are within a given radius.
102  * \param cloud_arg the point cloud data
103  * \param index_arg the index in \a cloud representing the query point
104  * \param radius_arg the radius of the sphere bounding all of p_q's neighbors
105  * \param k_indices_arg the resultant indices of the neighboring points
106  * \param k_sqr_distances_arg the resultant squared distances to the neighboring points
107  * \param max_nn_arg if given, bounds the maximum returned neighbors to this value
108  * \return number of neighbors found in radius
109  */
110  int
111  radiusSearch (const PointCloudConstPtr &cloud_arg, int index_arg, double radius_arg,
112  std::vector<int> &k_indices_arg, std::vector<float> &k_sqr_distances_arg,
113  int max_nn_arg = std::numeric_limits<int>::max());
114 
115  /** \brief Search for all neighbors of query point that are within a given radius.
116  * \param index_arg index representing the query point in the dataset given by \a setInputCloud.
117  * If indices were given in setInputCloud, index will be the position in the indices vector
118  * \param radius_arg radius of the sphere bounding all of p_q's neighbors
119  * \param k_indices_arg the resultant indices of the neighboring points
120  * \param k_sqr_distances_arg the resultant squared distances to the neighboring points
121  * \param max_nn_arg if given, bounds the maximum returned neighbors to this value
122  * \return number of neighbors found in radius
123  */
124  int
125  radiusSearch (int index_arg, const double radius_arg, std::vector<int> &k_indices_arg,
126  std::vector<float> &k_sqr_distances_arg, int max_nn_arg = std::numeric_limits<int>::max()) const;
127 
128  /** \brief Search for all neighbors of query point that are within a given radius.
129  * \param p_q_arg the given query point
130  * \param radius_arg the radius of the sphere bounding all of p_q's neighbors
131  * \param k_indices_arg the resultant indices of the neighboring points
132  * \param k_sqr_distances_arg the resultant squared distances to the neighboring points
133  * \param max_nn_arg if given, bounds the maximum returned neighbors to this value
134  * \return number of neighbors found in radius
135  */
136  int
137  radiusSearch (const PointT &p_q_arg, const double radius_arg, std::vector<int> &k_indices_arg,
138  std::vector<float> &k_sqr_distances_arg, int max_nn_arg = std::numeric_limits<int>::max()) const;
139 
140  /** \brief Search for k-nearest neighbors at the query point.
141  * \param cloud_arg the point cloud data
142  * \param index_arg the index in \a cloud representing the query point
143  * \param k_arg the number of neighbors to search for
144  * \param k_indices_arg the resultant indices of the neighboring points (must be resized to \a k a priori!)
145  * \param k_sqr_distances_arg the resultant squared distances to the neighboring points (must be resized to \a k
146  * a priori!)
147  * \return number of neighbors found
148  */
149  int
150  nearestKSearch (const PointCloudConstPtr &cloud_arg, int index_arg, int k_arg, std::vector<int> &k_indices_arg,
151  std::vector<float> &k_sqr_distances_arg);
152 
153  /** \brief Search for k-nearest neighbors at query point
154  * \param index_arg index representing the query point in the dataset given by \a setInputCloud.
155  * If indices were given in setInputCloud, index will be the position in the indices vector.
156  * \param k_arg the number of neighbors to search for
157  * \param k_indices_arg the resultant indices of the neighboring points (must be resized to \a k a priori!)
158  * \param k_sqr_distances_arg the resultant squared distances to the neighboring points (must be resized to \a k
159  * a priori!)
160  * \return number of neighbors found
161  */
162  int
163  nearestKSearch (int index_arg, int k_arg, std::vector<int> &k_indices_arg,
164  std::vector<float> &k_sqr_distances_arg);
165 
166  /** \brief Search for k-nearest neighbors at given query point.
167  * @param p_q_arg the given query point
168  * @param k_arg the number of neighbors to search for
169  * @param k_indices_arg the resultant indices of the neighboring points (must be resized to k a priori!)
170  * @param k_sqr_distances_arg the resultant squared distances to the neighboring points (must be resized to k a priori!)
171  * @return number of neighbors found
172  */
173  int
174  nearestKSearch (const PointT &p_q_arg, int k_arg, std::vector<int> &k_indices_arg,
175  std::vector<float> &k_sqr_distances_arg);
176 
177  /** \brief Get the maximum allowed distance between the query point and its nearest neighbors. */
178  inline double
179  getMaxDistance () const
180  {
181  return (max_distance_);
182  }
183 
184  /** \brief Set the maximum allowed distance between the query point and its nearest neighbors. */
185  inline void
186  setMaxDistance (double max_dist)
187  {
188  max_distance_ = max_dist;
189  }
190 
191  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
192  // Protected methods
193  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
194 
195  protected:
196 
197  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
198  /** \brief @b radiusSearchLoopkupEntry entry for radius search lookup vector
199  * \note This class defines entries for the radius search lookup vector
200  * \author Julius Kammerl (julius@kammerl.de)
201  */
202  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
204  {
205  public:
206 
207  /** \brief Empty constructor */
209  {
210  }
211 
212  /** \brief Define search point and calculate squared distance
213  * @param x_shift shift in x dimension
214  * @param y_shift shift in y dimension
215  */
216  void
217  defineShiftedSearchPoint(int x_shift, int y_shift)
218  {
219  x_diff_ =x_shift;
220  y_diff_ =y_shift;
221 
223  }
224 
225  /** \brief Operator< for comparing radiusSearchLoopkupEntry instances with each other. */
226  bool
227  operator< (const radiusSearchLoopkupEntry& rhs_arg) const
228  {
229  return (this->squared_distance_ < rhs_arg.squared_distance_);
230  }
231 
232  // Public globals
233  int x_diff_;
234  int y_diff_;
236 
237  };
238 
239  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
240  /** \brief @b nearestNeighborCandidate entry for the nearest neighbor candidate queue
241  * \note This class defines entries for the nearest neighbor candidate queue
242  * \author Julius Kammerl (julius@kammerl.de)
243  */
244  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
245 
247  {
248  public:
249 
250  /** \brief Empty constructor */
252  {
253  }
254 
255  /** \brief Operator< for comparing nearestNeighborCandidate instances with each other. */
256  bool
257  operator< (const nearestNeighborCandidate& rhs_arg) const
258  {
259  return (this->squared_distance_ < rhs_arg.squared_distance_);
260  }
261 
262  // Public globals
263  int index_;
265 
266  };
267 
268  /** \brief Get point at index from input pointcloud dataset
269  * \param index_arg index representing the point in the dataset given by \a setInputCloud
270  * \return PointT from input pointcloud dataset
271  */
272  const PointT&
273  getPointByIndex (const unsigned int index_arg) const;
274 
275  /** \brief Generate radius lookup table. It is used to subsequentially iterate over points
276  * which are close to the search point
277  * \param width of organized point cloud
278  * \param height of organized point cloud
279  */
280  void
281  generateRadiusLookupTable (unsigned int width, unsigned int height);
282 
283  inline void
284  pointPlaneProjection (const PointT& point, int& xpos, int& ypos) const
285  {
286  xpos = (int) pcl_round(point.x / (point.z * focalLength_));
287  ypos = (int) pcl_round(point.y / (point.z * focalLength_));
288  }
289 
290  void
291  getProjectedRadiusSearchBox (const PointT& point_arg, double squared_radius_arg, int& minX_arg, int& minY_arg, int& maxX_arg, int& maxY_arg ) const;
292 
293 
294  /** \brief Estimate focal length parameter that was used during point cloud generation
295  */
296  void
298 
299  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
300  /** \brief Class getName method. */
301  virtual std::string
302  getName () const
303  {
304  return ("Organized_Neighbor_Search");
305  }
306 
307  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
308  // Globals
309  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
310 
311  /** \brief Pointer to input point cloud dataset. */
313 
314  /** \brief Maximum allowed distance between the query point and its k-neighbors. */
316 
317  /** \brief Global focal length parameter */
318  double focalLength_;
319 
320  /** \brief Precalculated radius search lookup vector */
321  std::vector<radiusSearchLoopkupEntry> radiusSearchLookup_;
324 
325  };
326 
327 }
328 
329 //#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 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
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.
virtual ~OrganizedNeighborSearch()=default
Empty deconstructor.
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.