Point Cloud Library (PCL)  1.14.1
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
77  ~OrganizedNeighborSearch() = default;
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"
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.
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
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.
int x_diff_
void setMaxDistance(double max_dist)
Set the maximum allowed distance between the query point and its nearest neighbors.
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.
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.
std::vector< radiusSearchLoopkupEntry > radiusSearchLookup_
Precalculated radius search lookup vector.
virtual ~OrganizedNeighborSearch()=default
Empty deconstructor.
double max_distance_
Maximum allowed distance between the query point and its k-neighbors.
int y_diff_
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Definition: distances.h:55
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.
typename PointCloud::Ptr PointCloudPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
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.
typename PointCloud::ConstPtr PointCloudConstPtr
int squared_distance_
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.
radiusSearchLoopkupEntry()
Empty constructor.