Point Cloud Library (PCL)  1.9.1
octree_search.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  */
38 
39 #ifndef PCL_OCTREE_SEARCH_H_
40 #define PCL_OCTREE_SEARCH_H_
41 
42 #include <pcl/point_cloud.h>
43 
44 #include <pcl/octree/octree_pointcloud.h>
45 
46 namespace pcl
47 {
48  namespace octree
49  {
50  /** \brief @b Octree pointcloud search class
51  * \note This class provides several methods for spatial neighbor search based on octree structure
52  * \note typename: PointT: type of point used in pointcloud
53  * \ingroup octree
54  * \author Julius Kammerl (julius@kammerl.de)
55  */
56  template<typename PointT, typename LeafContainerT = OctreeContainerPointIndices , typename BranchContainerT = OctreeContainerEmpty >
57  class OctreePointCloudSearch : public OctreePointCloud<PointT, LeafContainerT, BranchContainerT>
58  {
59  public:
60  // public typedefs
61  typedef boost::shared_ptr<std::vector<int> > IndicesPtr;
62  typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr;
63 
65  typedef boost::shared_ptr<PointCloud> PointCloudPtr;
66  typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
67 
68  // Boost shared pointers
69  typedef boost::shared_ptr<OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT> > Ptr;
70  typedef boost::shared_ptr<const OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT> > ConstPtr;
71 
72  // Eigen aligned allocator
73  typedef std::vector<PointT, Eigen::aligned_allocator<PointT> > AlignedPointTVector;
74 
76  typedef typename OctreeT::LeafNode LeafNode;
77  typedef typename OctreeT::BranchNode BranchNode;
78 
79  /** \brief Constructor.
80  * \param[in] resolution octree resolution at lowest octree level
81  */
82  OctreePointCloudSearch (const double resolution) :
83  OctreePointCloud<PointT, LeafContainerT, BranchContainerT> (resolution)
84  {
85  }
86 
87  /** \brief Empty class destructor. */
88  virtual
90  {
91  }
92 
93  /** \brief Search for neighbors within a voxel at given point
94  * \param[in] point point addressing a leaf node voxel
95  * \param[out] point_idx_data the resultant indices of the neighboring voxel points
96  * \return "true" if leaf node exist; "false" otherwise
97  */
98  bool
99  voxelSearch (const PointT& point, std::vector<int>& point_idx_data);
100 
101  /** \brief Search for neighbors within a voxel at given point referenced by a point index
102  * \param[in] index the index in input cloud defining the query point
103  * \param[out] point_idx_data the resultant indices of the neighboring voxel points
104  * \return "true" if leaf node exist; "false" otherwise
105  */
106  bool
107  voxelSearch (const int index, std::vector<int>& point_idx_data);
108 
109  /** \brief Search for k-nearest neighbors at the query point.
110  * \param[in] cloud the point cloud data
111  * \param[in] index the index in \a cloud representing the query point
112  * \param[in] k the number of neighbors to search for
113  * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
114  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
115  * a priori!)
116  * \return number of neighbors found
117  */
118  inline int
119  nearestKSearch (const PointCloud &cloud, int index, int k, std::vector<int> &k_indices,
120  std::vector<float> &k_sqr_distances)
121  {
122  return (nearestKSearch (cloud[index], k, k_indices, k_sqr_distances));
123  }
124 
125  /** \brief Search for k-nearest neighbors at given query point.
126  * \param[in] p_q the given query point
127  * \param[in] k the number of neighbors to search for
128  * \param[out] k_indices the resultant indices of the neighboring points (must be resized to k a priori!)
129  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to k a priori!)
130  * \return number of neighbors found
131  */
132  int
133  nearestKSearch (const PointT &p_q, int k, std::vector<int> &k_indices,
134  std::vector<float> &k_sqr_distances);
135 
136  /** \brief Search for k-nearest neighbors at query point
137  * \param[in] index index representing the query point in the dataset given by \a setInputCloud.
138  * If indices were given in setInputCloud, index will be the position in the indices vector.
139  * \param[in] k the number of neighbors to search for
140  * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
141  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
142  * a priori!)
143  * \return number of neighbors found
144  */
145  int
146  nearestKSearch (int index, int k, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances);
147 
148  /** \brief Search for approx. nearest neighbor at the query point.
149  * \param[in] cloud the point cloud data
150  * \param[in] query_index the index in \a cloud representing the query point
151  * \param[out] result_index the resultant index of the neighbor point
152  * \param[out] sqr_distance the resultant squared distance to the neighboring point
153  * \return number of neighbors found
154  */
155  inline void
156  approxNearestSearch (const PointCloud &cloud, int query_index, int &result_index, float &sqr_distance)
157  {
158  return (approxNearestSearch (cloud.points[query_index], result_index, sqr_distance));
159  }
160 
161  /** \brief Search for approx. nearest neighbor at the query point.
162  * \param[in] p_q the given query point
163  * \param[out] result_index the resultant index of the neighbor point
164  * \param[out] sqr_distance the resultant squared distance to the neighboring point
165  */
166  void
167  approxNearestSearch (const PointT &p_q, int &result_index, float &sqr_distance);
168 
169  /** \brief Search for approx. nearest neighbor at the query point.
170  * \param[in] query_index index representing the query point in the dataset given by \a setInputCloud.
171  * If indices were given in setInputCloud, index will be the position in the indices vector.
172  * \param[out] result_index the resultant index of the neighbor point
173  * \param[out] sqr_distance the resultant squared distance to the neighboring point
174  * \return number of neighbors found
175  */
176  void
177  approxNearestSearch (int query_index, int &result_index, float &sqr_distance);
178 
179  /** \brief Search for all neighbors of query point that are within a given radius.
180  * \param[in] cloud the point cloud data
181  * \param[in] index the index in \a cloud representing the query point
182  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
183  * \param[out] k_indices the resultant indices of the neighboring points
184  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
185  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
186  * \return number of neighbors found in radius
187  */
188  int
189  radiusSearch (const PointCloud &cloud, int index, double radius, std::vector<int> &k_indices,
190  std::vector<float> &k_sqr_distances, unsigned int max_nn = 0)
191  {
192  return (radiusSearch (cloud.points[index], radius, k_indices, k_sqr_distances, max_nn));
193  }
194 
195  /** \brief Search for all neighbors of query point that are within a given radius.
196  * \param[in] p_q the given query point
197  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
198  * \param[out] k_indices the resultant indices of the neighboring points
199  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
200  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
201  * \return number of neighbors found in radius
202  */
203  int
204  radiusSearch (const PointT &p_q, const double radius, std::vector<int> &k_indices,
205  std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const;
206 
207  /** \brief Search for all neighbors of query point that are within a given radius.
208  * \param[in] index index representing the query point in the dataset given by \a setInputCloud.
209  * If indices were given in setInputCloud, index will be the position in the indices vector
210  * \param[in] radius radius of the sphere bounding all of p_q's neighbors
211  * \param[out] k_indices the resultant indices of the neighboring points
212  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
213  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
214  * \return number of neighbors found in radius
215  */
216  int
217  radiusSearch (int index, const double radius, std::vector<int> &k_indices,
218  std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const;
219 
220  /** \brief Get a PointT vector of centers of all voxels that intersected by a ray (origin, direction).
221  * \param[in] origin ray origin
222  * \param[in] direction ray direction vector
223  * \param[out] voxel_center_list results are written to this vector of PointT elements
224  * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0: disable)
225  * \return number of intersected voxels
226  */
227  int
228  getIntersectedVoxelCenters (Eigen::Vector3f origin, Eigen::Vector3f direction,
229  AlignedPointTVector &voxel_center_list, int max_voxel_count = 0) const;
230 
231  /** \brief Get indices of all voxels that are intersected by a ray (origin, direction).
232  * \param[in] origin ray origin
233  * \param[in] direction ray direction vector
234  * \param[out] k_indices resulting point indices from intersected voxels
235  * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0: disable)
236  * \return number of intersected voxels
237  */
238  int
239  getIntersectedVoxelIndices (Eigen::Vector3f origin, Eigen::Vector3f direction,
240  std::vector<int> &k_indices,
241  int max_voxel_count = 0) const;
242 
243 
244  /** \brief Search for points within rectangular search area
245  * Points exactly on the edges of the search rectangle are included.
246  * \param[in] min_pt lower corner of search area
247  * \param[in] max_pt upper corner of search area
248  * \param[out] k_indices the resultant point indices
249  * \return number of points found within search area
250  */
251  int
252  boxSearch (const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, std::vector<int> &k_indices) const;
253 
254  protected:
255  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
256  // Octree-based search routines & helpers
257  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
258  /** \brief @b Priority queue entry for branch nodes
259  * \note This class defines priority queue entries for the nearest neighbor search.
260  * \author Julius Kammerl (julius@kammerl.de)
261  */
263  {
264  public:
265  /** \brief Empty constructor */
267  node (), point_distance (0), key ()
268  {
269  }
270 
271  /** \brief Constructor for initializing priority queue entry.
272  * \param _node pointer to octree node
273  * \param _key octree key addressing voxel in octree structure
274  * \param[in] _point_distance distance of query point to voxel center
275  */
276  prioBranchQueueEntry (OctreeNode* _node, OctreeKey& _key, float _point_distance) :
277  node (_node), point_distance (_point_distance), key (_key)
278  {
279  }
280 
281  /** \brief Operator< for comparing priority queue entries with each other.
282  * \param[in] rhs the priority queue to compare this against
283  */
284  bool
286  {
287  return (this->point_distance > rhs.point_distance);
288  }
289 
290  /** \brief Pointer to octree node. */
291  const OctreeNode* node;
292 
293  /** \brief Distance to query point. */
295 
296  /** \brief Octree key. */
298  };
299 
300  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
301  /** \brief @b Priority queue entry for point candidates
302  * \note This class defines priority queue entries for the nearest neighbor point candidates.
303  * \author Julius Kammerl (julius@kammerl.de)
304  */
306  {
307  public:
308 
309  /** \brief Empty constructor */
311  point_idx_ (0), point_distance_ (0)
312  {
313  }
314 
315  /** \brief Constructor for initializing priority queue entry.
316  * \param[in] point_idx an index representing a point in the dataset given by \a setInputCloud
317  * \param[in] point_distance distance of query point to voxel center
318  */
319  prioPointQueueEntry (unsigned int& point_idx, float point_distance) :
320  point_idx_ (point_idx), point_distance_ (point_distance)
321  {
322  }
323 
324  /** \brief Operator< for comparing priority queue entries with each other.
325  * \param[in] rhs priority queue to compare this against
326  */
327  bool
328  operator< (const prioPointQueueEntry& rhs) const
329  {
330  return (this->point_distance_ < rhs.point_distance_);
331  }
332 
333  /** \brief Index representing a point in the dataset given by \a setInputCloud. */
335 
336  /** \brief Distance to query point. */
338  };
339 
340  /** \brief Helper function to calculate the squared distance between two points
341  * \param[in] point_a point A
342  * \param[in] point_b point B
343  * \return squared distance between point A and point B
344  */
345  float
346  pointSquaredDist (const PointT& point_a, const PointT& point_b) const;
347 
348  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
349  // Recursive search routine methods
350  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
351 
352  /** \brief Recursive search method that explores the octree and finds neighbors within a given radius
353  * \param[in] point query point
354  * \param[in] radiusSquared squared search radius
355  * \param[in] node current octree node to be explored
356  * \param[in] key octree key addressing a leaf node.
357  * \param[in] tree_depth current depth/level in the octree
358  * \param[out] k_indices vector of indices found to be neighbors of query point
359  * \param[out] k_sqr_distances squared distances of neighbors to query point
360  * \param[in] max_nn maximum of neighbors to be found
361  */
362  void
363  getNeighborsWithinRadiusRecursive (const PointT& point, const double radiusSquared,
364  const BranchNode* node, const OctreeKey& key,
365  unsigned int tree_depth, std::vector<int>& k_indices,
366  std::vector<float>& k_sqr_distances, unsigned int max_nn) const;
367 
368  /** \brief Recursive search method that explores the octree and finds the K nearest neighbors
369  * \param[in] point query point
370  * \param[in] K amount of nearest neighbors to be found
371  * \param[in] node current octree node to be explored
372  * \param[in] key octree key addressing a leaf node.
373  * \param[in] tree_depth current depth/level in the octree
374  * \param[in] squared_search_radius squared search radius distance
375  * \param[out] point_candidates priority queue of nearest neigbor point candidates
376  * \return squared search radius based on current point candidate set found
377  */
378  double
379  getKNearestNeighborRecursive (const PointT& point, unsigned int K, const BranchNode* node,
380  const OctreeKey& key, unsigned int tree_depth,
381  const double squared_search_radius,
382  std::vector<prioPointQueueEntry>& point_candidates) const;
383 
384  /** \brief Recursive search method that explores the octree and finds the approximate nearest neighbor
385  * \param[in] point query point
386  * \param[in] node current octree node to be explored
387  * \param[in] key octree key addressing a leaf node.
388  * \param[in] tree_depth current depth/level in the octree
389  * \param[out] result_index result index is written to this reference
390  * \param[out] sqr_distance squared distance to search
391  */
392  void
393  approxNearestSearchRecursive (const PointT& point, const BranchNode* node, const OctreeKey& key,
394  unsigned int tree_depth, int& result_index, float& sqr_distance);
395 
396  /** \brief Recursively search the tree for all intersected leaf nodes and return a vector of voxel centers.
397  * This algorithm is based off the paper An Efficient Parametric Algorithm for Octree Traversal:
398  * http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf
399  * \param[in] min_x octree nodes X coordinate of lower bounding box corner
400  * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
401  * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
402  * \param[in] max_x octree nodes X coordinate of upper bounding box corner
403  * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
404  * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
405  * \param[in] a
406  * \param[in] node current octree node to be explored
407  * \param[in] key octree key addressing a leaf node.
408  * \param[out] voxel_center_list results are written to this vector of PointT elements
409  * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0: disable)
410  * \return number of voxels found
411  */
412  int
413  getIntersectedVoxelCentersRecursive (double min_x, double min_y, double min_z, double max_x, double max_y,
414  double max_z, unsigned char a, const OctreeNode* node,
415  const OctreeKey& key, AlignedPointTVector &voxel_center_list,
416  int max_voxel_count) const;
417 
418 
419  /** \brief Recursive search method that explores the octree and finds points within a rectangular search area
420  * \param[in] min_pt lower corner of search area
421  * \param[in] max_pt upper corner of search area
422  * \param[in] node current octree node to be explored
423  * \param[in] key octree key addressing a leaf node.
424  * \param[in] tree_depth current depth/level in the octree
425  * \param[out] k_indices the resultant point indices
426  */
427  void
428  boxSearchRecursive (const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, const BranchNode* node,
429  const OctreeKey& key, unsigned int tree_depth, std::vector<int>& k_indices) const;
430 
431  /** \brief Recursively search the tree for all intersected leaf nodes and return a vector of indices.
432  * This algorithm is based off the paper An Efficient Parametric Algorithm for Octree Traversal:
433  * http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf
434  * \param[in] min_x octree nodes X coordinate of lower bounding box corner
435  * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
436  * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
437  * \param[in] max_x octree nodes X coordinate of upper bounding box corner
438  * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
439  * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
440  * \param[in] a
441  * \param[in] node current octree node to be explored
442  * \param[in] key octree key addressing a leaf node.
443  * \param[out] k_indices resulting indices
444  * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0: disable)
445  * \return number of voxels found
446  */
447  int
448  getIntersectedVoxelIndicesRecursive (double min_x, double min_y, double min_z,
449  double max_x, double max_y, double max_z,
450  unsigned char a, const OctreeNode* node, const OctreeKey& key,
451  std::vector<int> &k_indices,
452  int max_voxel_count) const;
453 
454  /** \brief Initialize raytracing algorithm
455  * \param origin
456  * \param direction
457  * \param[in] min_x octree nodes X coordinate of lower bounding box corner
458  * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
459  * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
460  * \param[in] max_x octree nodes X coordinate of upper bounding box corner
461  * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
462  * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
463  * \param a
464  */
465  inline void
466  initIntersectedVoxel (Eigen::Vector3f &origin, Eigen::Vector3f &direction,
467  double &min_x, double &min_y, double &min_z,
468  double &max_x, double &max_y, double &max_z,
469  unsigned char &a) const
470  {
471  // Account for division by zero when direction vector is 0.0
472  const float epsilon = 1e-10f;
473  if (direction.x () == 0.0)
474  direction.x () = epsilon;
475  if (direction.y () == 0.0)
476  direction.y () = epsilon;
477  if (direction.z () == 0.0)
478  direction.z () = epsilon;
479 
480  // Voxel childIdx remapping
481  a = 0;
482 
483  // Handle negative axis direction vector
484  if (direction.x () < 0.0)
485  {
486  origin.x () = static_cast<float> (this->min_x_) + static_cast<float> (this->max_x_) - origin.x ();
487  direction.x () = -direction.x ();
488  a |= 4;
489  }
490  if (direction.y () < 0.0)
491  {
492  origin.y () = static_cast<float> (this->min_y_) + static_cast<float> (this->max_y_) - origin.y ();
493  direction.y () = -direction.y ();
494  a |= 2;
495  }
496  if (direction.z () < 0.0)
497  {
498  origin.z () = static_cast<float> (this->min_z_) + static_cast<float> (this->max_z_) - origin.z ();
499  direction.z () = -direction.z ();
500  a |= 1;
501  }
502  min_x = (this->min_x_ - origin.x ()) / direction.x ();
503  max_x = (this->max_x_ - origin.x ()) / direction.x ();
504  min_y = (this->min_y_ - origin.y ()) / direction.y ();
505  max_y = (this->max_y_ - origin.y ()) / direction.y ();
506  min_z = (this->min_z_ - origin.z ()) / direction.z ();
507  max_z = (this->max_z_ - origin.z ()) / direction.z ();
508  }
509 
510  /** \brief Find first child node ray will enter
511  * \param[in] min_x octree nodes X coordinate of lower bounding box corner
512  * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
513  * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
514  * \param[in] mid_x octree nodes X coordinate of bounding box mid line
515  * \param[in] mid_y octree nodes Y coordinate of bounding box mid line
516  * \param[in] mid_z octree nodes Z coordinate of bounding box mid line
517  * \return the first child node ray will enter
518  */
519  inline int
520  getFirstIntersectedNode (double min_x, double min_y, double min_z, double mid_x, double mid_y, double mid_z) const
521  {
522  int currNode = 0;
523 
524  if (min_x > min_y)
525  {
526  if (min_x > min_z)
527  {
528  // max(min_x, min_y, min_z) is min_x. Entry plane is YZ.
529  if (mid_y < min_x)
530  currNode |= 2;
531  if (mid_z < min_x)
532  currNode |= 1;
533  }
534  else
535  {
536  // max(min_x, min_y, min_z) is min_z. Entry plane is XY.
537  if (mid_x < min_z)
538  currNode |= 4;
539  if (mid_y < min_z)
540  currNode |= 2;
541  }
542  }
543  else
544  {
545  if (min_y > min_z)
546  {
547  // max(min_x, min_y, min_z) is min_y. Entry plane is XZ.
548  if (mid_x < min_y)
549  currNode |= 4;
550  if (mid_z < min_y)
551  currNode |= 1;
552  }
553  else
554  {
555  // max(min_x, min_y, min_z) is min_z. Entry plane is XY.
556  if (mid_x < min_z)
557  currNode |= 4;
558  if (mid_y < min_z)
559  currNode |= 2;
560  }
561  }
562 
563  return currNode;
564  }
565 
566  /** \brief Get the next visited node given the current node upper
567  * bounding box corner. This function accepts three float values, and
568  * three int values. The function returns the ith integer where the
569  * ith float value is the minimum of the three float values.
570  * \param[in] x current nodes X coordinate of upper bounding box corner
571  * \param[in] y current nodes Y coordinate of upper bounding box corner
572  * \param[in] z current nodes Z coordinate of upper bounding box corner
573  * \param[in] a next node if exit Plane YZ
574  * \param[in] b next node if exit Plane XZ
575  * \param[in] c next node if exit Plane XY
576  * \return the next child node ray will enter or 8 if exiting
577  */
578  inline int
579  getNextIntersectedNode (double x, double y, double z, int a, int b, int c) const
580  {
581  if (x < y)
582  {
583  if (x < z)
584  return a;
585  else
586  return c;
587  }
588  else
589  {
590  if (y < z)
591  return b;
592  else
593  return c;
594  }
595 
596  return 0;
597  }
598 
599  };
600  }
601 }
602 
603 #ifdef PCL_NO_PRECOMPILE
604 #include <pcl/octree/impl/octree_search.hpp>
605 #endif
606 
607 #endif // PCL_OCTREE_SEARCH_H_
pcl::octree::OctreePointCloud::BranchNode
OctreeT::BranchNode BranchNode
Definition: octree_pointcloud.h:79
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
pcl::K
@ K
Definition: norms.h:55
pcl::octree::OctreePointCloudSearch::prioPointQueueEntry::prioPointQueueEntry
prioPointQueueEntry()
Empty constructor
Definition: octree_search.h:310
pcl::octree::OctreePointCloudSearch::getIntersectedVoxelIndicesRecursive
int getIntersectedVoxelIndicesRecursive(double min_x, double min_y, double min_z, double max_x, double max_y, double max_z, unsigned char a, const OctreeNode *node, const OctreeKey &key, std::vector< int > &k_indices, int max_voxel_count) const
Recursively search the tree for all intersected leaf nodes and return a vector of indices.
Definition: octree_search.hpp:749
pcl::octree::OctreePointCloudSearch::getIntersectedVoxelIndices
int getIntersectedVoxelIndices(Eigen::Vector3f origin, Eigen::Vector3f direction, std::vector< int > &k_indices, int max_voxel_count=0) const
Get indices of all voxels that are intersected by a ray (origin, direction).
Definition: octree_search.hpp:608
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::prioBranchQueueEntry
prioBranchQueueEntry(OctreeNode *_node, OctreeKey &_key, float _point_distance)
Constructor for initializing priority queue entry.
Definition: octree_search.h:276
pcl::octree::OctreePointCloudSearch::getKNearestNeighborRecursive
double getKNearestNeighborRecursive(const PointT &point, unsigned int K, const BranchNode *node, const OctreeKey &key, unsigned int tree_depth, const double squared_search_radius, std::vector< prioPointQueueEntry > &point_candidates) const
Recursive search method that explores the octree and finds the K nearest neighbors.
Definition: octree_search.hpp:208
pcl::octree::OctreePointCloud
Octree pointcloud class
Definition: octree_pointcloud.h:73
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::node
const OctreeNode * node
Pointer to octree node.
Definition: octree_search.h:291
pcl::octree::OctreePointCloudSearch::prioPointQueueEntry::operator<
bool operator<(const prioPointQueueEntry &rhs) const
Operator< for comparing priority queue entries with each other.
Definition: octree_search.h:328
pcl::octree::OctreePointCloudSearch::boxSearchRecursive
void boxSearchRecursive(const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, const BranchNode *node, const OctreeKey &key, unsigned int tree_depth, std::vector< int > &k_indices) const
Recursive search method that explores the octree and finds points within a rectangular search area.
Definition: octree_search.hpp:506
pcl::octree::OctreePointCloudSearch::PointCloud
pcl::PointCloud< PointT > PointCloud
Definition: octree_search.h:64
pcl::octree::OctreePointCloudSearch::voxelSearch
bool voxelSearch(const PointT &point, std::vector< int > &point_idx_data)
Search for neighbors within a voxel at given point.
Definition: octree_search.hpp:46
pcl::octree::OctreePointCloudSearch::nearestKSearch
int nearestKSearch(const PointCloud &cloud, int index, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances)
Search for k-nearest neighbors at the query point.
Definition: octree_search.h:119
pcl::octree::OctreePointCloudSearch::getNeighborsWithinRadiusRecursive
void getNeighborsWithinRadiusRecursive(const PointT &point, const double radiusSquared, const BranchNode *node, const OctreeKey &key, unsigned int tree_depth, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn) const
Recursive search method that explores the octree and finds neighbors within a given radius.
Definition: octree_search.hpp:317
pcl::octree::OctreePointCloudSearch::getIntersectedVoxelCenters
int getIntersectedVoxelCenters(Eigen::Vector3f origin, Eigen::Vector3f direction, AlignedPointTVector &voxel_center_list, int max_voxel_count=0) const
Get a PointT vector of centers of all voxels that intersected by a ray (origin, direction).
Definition: octree_search.hpp:583
pcl::octree::OctreeKey
Octree key class
Definition: octree_key.h:51
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: projection_matrix.h:53
pcl::octree::OctreePointCloudSearch::radiusSearch
int radiusSearch(const PointCloud &cloud, int index, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0)
Search for all neighbors of query point that are within a given radius.
Definition: octree_search.h:189
pcl::octree::OctreePointCloudSearch::approxNearestSearch
void approxNearestSearch(const PointCloud &cloud, int query_index, int &result_index, float &sqr_distance)
Search for approx.
Definition: octree_search.h:156
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:619
pcl::octree::OctreePointCloudSearch::IndicesPtr
boost::shared_ptr< std::vector< int > > IndicesPtr
Definition: octree_search.h:61
pcl::octree::OctreeNode
Abstract octree node class
Definition: octree_nodes.h:68
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty >::min_y_
double min_y_
Definition: octree_pointcloud.h:532
pcl::octree::OctreePointCloudSearch::prioPointQueueEntry
Priority queue entry for point candidates
Definition: octree_search.h:305
pcl::octree::OctreePointCloudSearch::AlignedPointTVector
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
Definition: octree_search.h:73
pcl::octree::OctreePointCloudSearch::prioPointQueueEntry::point_idx_
int point_idx_
Index representing a point in the dataset given by setInputCloud.
Definition: octree_search.h:334
pcl::octree::OctreePointCloudSearch::prioPointQueueEntry::point_distance_
float point_distance_
Distance to query point.
Definition: octree_search.h:337
pcl::octree::OctreePointCloudSearch::Ptr
boost::shared_ptr< OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT > > Ptr
Definition: octree_search.h:69
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::key
OctreeKey key
Octree key.
Definition: octree_search.h:297
pcl::octree::OctreePointCloudSearch::OctreePointCloudSearch
OctreePointCloudSearch(const double resolution)
Constructor.
Definition: octree_search.h:82
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::point_distance
float point_distance
Distance to query point.
Definition: octree_search.h:294
pcl::octree::OctreePointCloudSearch::getNextIntersectedNode
int getNextIntersectedNode(double x, double y, double z, int a, int b, int c) const
Get the next visited node given the current node upper bounding box corner.
Definition: octree_search.h:579
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry
Priority queue entry for branch nodes
Definition: octree_search.h:262
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty >::min_z_
double min_z_
Definition: octree_pointcloud.h:535
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty >::max_x_
double max_x_
Definition: octree_pointcloud.h:530
pcl::octree::OctreePointCloudSearch::PointCloudConstPtr
boost::shared_ptr< const PointCloud > PointCloudConstPtr
Definition: octree_search.h:66
pcl::octree::OctreePointCloudSearch::approxNearestSearchRecursive
void approxNearestSearchRecursive(const PointT &point, const BranchNode *node, const OctreeKey &key, unsigned int tree_depth, int &result_index, float &sqr_distance)
Recursive search method that explores the octree and finds the approximate nearest neighbor.
Definition: octree_search.hpp:402
pcl::octree::OctreePointCloudSearch::initIntersectedVoxel
void initIntersectedVoxel(Eigen::Vector3f &origin, Eigen::Vector3f &direction, double &min_x, double &min_y, double &min_z, double &max_x, double &max_y, double &max_z, unsigned char &a) const
Initialize raytracing algorithm.
Definition: octree_search.h:466
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty >::max_z_
double max_z_
Definition: octree_pointcloud.h:536
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty >::max_y_
double max_y_
Definition: octree_pointcloud.h:533
pcl::octree::OctreePointCloudSearch::pointSquaredDist
float pointSquaredDist(const PointT &point_a, const PointT &point_b) const
Helper function to calculate the squared distance between two points.
Definition: octree_search.hpp:498
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::operator<
bool operator<(const prioBranchQueueEntry rhs) const
Operator< for comparing priority queue entries with each other.
Definition: octree_search.h:285
pcl::octree::OctreePointCloudSearch::PointCloudPtr
boost::shared_ptr< PointCloud > PointCloudPtr
Definition: octree_search.h:65
pcl::octree::OctreePointCloudSearch::prioPointQueueEntry::prioPointQueueEntry
prioPointQueueEntry(unsigned int &point_idx, float point_distance)
Constructor for initializing priority queue entry.
Definition: octree_search.h:319
pcl::octree::OctreePointCloudSearch::getIntersectedVoxelCentersRecursive
int getIntersectedVoxelCentersRecursive(double min_x, double min_y, double min_z, double max_x, double max_y, double max_z, unsigned char a, const OctreeNode *node, const OctreeKey &key, AlignedPointTVector &voxel_center_list, int max_voxel_count) const
Recursively search the tree for all intersected leaf nodes and return a vector of voxel centers.
Definition: octree_search.hpp:631
pcl::octree::OctreePointCloudSearch::OctreeT
OctreePointCloud< PointT, LeafContainerT, BranchContainerT > OctreeT
Definition: octree_search.h:75
pcl::octree::OctreePointCloudSearch::getFirstIntersectedNode
int getFirstIntersectedNode(double min_x, double min_y, double min_z, double mid_x, double mid_y, double mid_z) const
Find first child node ray will enter.
Definition: octree_search.h:520
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::prioBranchQueueEntry
prioBranchQueueEntry()
Empty constructor
Definition: octree_search.h:266
pcl::octree::OctreePointCloudSearch::LeafNode
OctreeT::LeafNode LeafNode
Definition: octree_search.h:76
pcl::octree::OctreePointCloudSearch::IndicesConstPtr
boost::shared_ptr< const std::vector< int > > IndicesConstPtr
Definition: octree_search.h:62
pcl::octree::OctreePointCloudSearch
Octree pointcloud search class
Definition: octree_search.h:57
pcl::octree::OctreePointCloudSearch::boxSearch
int boxSearch(const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, std::vector< int > &k_indices) const
Search for points within rectangular search area Points exactly on the edges of the search rectangle ...
Definition: octree_search.hpp:190
pcl::octree::OctreePointCloudSearch::BranchNode
OctreeT::BranchNode BranchNode
Definition: octree_search.h:77
pcl::octree::OctreePointCloud::LeafNode
OctreeT::LeafNode LeafNode
Definition: octree_pointcloud.h:78
pcl::octree::OctreePointCloudSearch::~OctreePointCloudSearch
virtual ~OctreePointCloudSearch()
Empty class destructor.
Definition: octree_search.h:89
pcl::octree::OctreePointCloudSearch::ConstPtr
boost::shared_ptr< const OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT > > ConstPtr
Definition: octree_search.h:70
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty >::min_x_
double min_x_
Definition: octree_pointcloud.h:529