Point Cloud Library (PCL)  1.8.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  * \param[in] min_pt lower corner of search area
246  * \param[in] max_pt upper corner of search area
247  * \param[out] k_indices the resultant point indices
248  * \return number of points found within search area
249  */
250  int
251  boxSearch (const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, std::vector<int> &k_indices) const;
252 
253  protected:
254  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
255  // Octree-based search routines & helpers
256  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
257  /** \brief @b Priority queue entry for branch nodes
258  * \note This class defines priority queue entries for the nearest neighbor search.
259  * \author Julius Kammerl (julius@kammerl.de)
260  */
262  {
263  public:
264  /** \brief Empty constructor */
266  node (), point_distance (0), key ()
267  {
268  }
269 
270  /** \brief Constructor for initializing priority queue entry.
271  * \param _node pointer to octree node
272  * \param _key octree key addressing voxel in octree structure
273  * \param[in] _point_distance distance of query point to voxel center
274  */
275  prioBranchQueueEntry (OctreeNode* _node, OctreeKey& _key, float _point_distance) :
276  node (_node), point_distance (_point_distance), key (_key)
277  {
278  }
279 
280  /** \brief Operator< for comparing priority queue entries with each other.
281  * \param[in] rhs the priority queue to compare this against
282  */
283  bool
285  {
286  return (this->point_distance > rhs.point_distance);
287  }
288 
289  /** \brief Pointer to octree node. */
290  const OctreeNode* node;
291 
292  /** \brief Distance to query point. */
294 
295  /** \brief Octree key. */
297  };
298 
299  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
300  /** \brief @b Priority queue entry for point candidates
301  * \note This class defines priority queue entries for the nearest neighbor point candidates.
302  * \author Julius Kammerl (julius@kammerl.de)
303  */
305  {
306  public:
307 
308  /** \brief Empty constructor */
310  point_idx_ (0), point_distance_ (0)
311  {
312  }
313 
314  /** \brief Constructor for initializing priority queue entry.
315  * \param[in] point_idx an index representing a point in the dataset given by \a setInputCloud
316  * \param[in] point_distance distance of query point to voxel center
317  */
318  prioPointQueueEntry (unsigned int& point_idx, float point_distance) :
319  point_idx_ (point_idx), point_distance_ (point_distance)
320  {
321  }
322 
323  /** \brief Operator< for comparing priority queue entries with each other.
324  * \param[in] rhs priority queue to compare this against
325  */
326  bool
327  operator< (const prioPointQueueEntry& rhs) const
328  {
329  return (this->point_distance_ < rhs.point_distance_);
330  }
331 
332  /** \brief Index representing a point in the dataset given by \a setInputCloud. */
334 
335  /** \brief Distance to query point. */
337  };
338 
339  /** \brief Helper function to calculate the squared distance between two points
340  * \param[in] point_a point A
341  * \param[in] point_b point B
342  * \return squared distance between point A and point B
343  */
344  float
345  pointSquaredDist (const PointT& point_a, const PointT& point_b) const;
346 
347  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
348  // Recursive search routine methods
349  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
350 
351  /** \brief Recursive search method that explores the octree and finds neighbors within a given radius
352  * \param[in] point query point
353  * \param[in] radiusSquared squared search radius
354  * \param[in] node current octree node to be explored
355  * \param[in] key octree key addressing a leaf node.
356  * \param[in] tree_depth current depth/level in the octree
357  * \param[out] k_indices vector of indices found to be neighbors of query point
358  * \param[out] k_sqr_distances squared distances of neighbors to query point
359  * \param[in] max_nn maximum of neighbors to be found
360  */
361  void
362  getNeighborsWithinRadiusRecursive (const PointT& point, const double radiusSquared,
363  const BranchNode* node, const OctreeKey& key,
364  unsigned int tree_depth, std::vector<int>& k_indices,
365  std::vector<float>& k_sqr_distances, unsigned int max_nn) const;
366 
367  /** \brief Recursive search method that explores the octree and finds the K nearest neighbors
368  * \param[in] point query point
369  * \param[in] K amount of nearest neighbors to be found
370  * \param[in] node current octree node to be explored
371  * \param[in] key octree key addressing a leaf node.
372  * \param[in] tree_depth current depth/level in the octree
373  * \param[in] squared_search_radius squared search radius distance
374  * \param[out] point_candidates priority queue of nearest neigbor point candidates
375  * \return squared search radius based on current point candidate set found
376  */
377  double
378  getKNearestNeighborRecursive (const PointT& point, unsigned int K, const BranchNode* node,
379  const OctreeKey& key, unsigned int tree_depth,
380  const double squared_search_radius,
381  std::vector<prioPointQueueEntry>& point_candidates) const;
382 
383  /** \brief Recursive search method that explores the octree and finds the approximate nearest neighbor
384  * \param[in] point query point
385  * \param[in] node current octree node to be explored
386  * \param[in] key octree key addressing a leaf node.
387  * \param[in] tree_depth current depth/level in the octree
388  * \param[out] result_index result index is written to this reference
389  * \param[out] sqr_distance squared distance to search
390  */
391  void
392  approxNearestSearchRecursive (const PointT& point, const BranchNode* node, const OctreeKey& key,
393  unsigned int tree_depth, int& result_index, float& sqr_distance);
394 
395  /** \brief Recursively search the tree for all intersected leaf nodes and return a vector of voxel centers.
396  * This algorithm is based off the paper An Efficient Parametric Algorithm for Octree Traversal:
397  * http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf
398  * \param[in] min_x octree nodes X coordinate of lower bounding box corner
399  * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
400  * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
401  * \param[in] max_x octree nodes X coordinate of upper bounding box corner
402  * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
403  * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
404  * \param[in] a
405  * \param[in] node current octree node to be explored
406  * \param[in] key octree key addressing a leaf node.
407  * \param[out] voxel_center_list results are written to this vector of PointT elements
408  * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0: disable)
409  * \return number of voxels found
410  */
411  int
412  getIntersectedVoxelCentersRecursive (double min_x, double min_y, double min_z, double max_x, double max_y,
413  double max_z, unsigned char a, const OctreeNode* node,
414  const OctreeKey& key, AlignedPointTVector &voxel_center_list,
415  int max_voxel_count) const;
416 
417 
418  /** \brief Recursive search method that explores the octree and finds points within a rectangular search area
419  * \param[in] min_pt lower corner of search area
420  * \param[in] max_pt upper corner of search area
421  * \param[in] node current octree node to be explored
422  * \param[in] key octree key addressing a leaf node.
423  * \param[in] tree_depth current depth/level in the octree
424  * \param[out] k_indices the resultant point indices
425  */
426  void
427  boxSearchRecursive (const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, const BranchNode* node,
428  const OctreeKey& key, unsigned int tree_depth, std::vector<int>& k_indices) const;
429 
430  /** \brief Recursively search the tree for all intersected leaf nodes and return a vector of indices.
431  * This algorithm is based off the paper An Efficient Parametric Algorithm for Octree Traversal:
432  * http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf
433  * \param[in] min_x octree nodes X coordinate of lower bounding box corner
434  * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
435  * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
436  * \param[in] max_x octree nodes X coordinate of upper bounding box corner
437  * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
438  * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
439  * \param[in] a
440  * \param[in] node current octree node to be explored
441  * \param[in] key octree key addressing a leaf node.
442  * \param[out] k_indices resulting indices
443  * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0: disable)
444  * \return number of voxels found
445  */
446  int
447  getIntersectedVoxelIndicesRecursive (double min_x, double min_y, double min_z,
448  double max_x, double max_y, double max_z,
449  unsigned char a, const OctreeNode* node, const OctreeKey& key,
450  std::vector<int> &k_indices,
451  int max_voxel_count) const;
452 
453  /** \brief Initialize raytracing algorithm
454  * \param origin
455  * \param direction
456  * \param[in] min_x octree nodes X coordinate of lower bounding box corner
457  * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
458  * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
459  * \param[in] max_x octree nodes X coordinate of upper bounding box corner
460  * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
461  * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
462  * \param a
463  */
464  inline void
465  initIntersectedVoxel (Eigen::Vector3f &origin, Eigen::Vector3f &direction,
466  double &min_x, double &min_y, double &min_z,
467  double &max_x, double &max_y, double &max_z,
468  unsigned char &a) const
469  {
470  // Account for division by zero when direction vector is 0.0
471  const float epsilon = 1e-10f;
472  if (direction.x () == 0.0)
473  direction.x () = epsilon;
474  if (direction.y () == 0.0)
475  direction.y () = epsilon;
476  if (direction.z () == 0.0)
477  direction.z () = epsilon;
478 
479  // Voxel childIdx remapping
480  a = 0;
481 
482  // Handle negative axis direction vector
483  if (direction.x () < 0.0)
484  {
485  origin.x () = static_cast<float> (this->min_x_) + static_cast<float> (this->max_x_) - origin.x ();
486  direction.x () = -direction.x ();
487  a |= 4;
488  }
489  if (direction.y () < 0.0)
490  {
491  origin.y () = static_cast<float> (this->min_y_) + static_cast<float> (this->max_y_) - origin.y ();
492  direction.y () = -direction.y ();
493  a |= 2;
494  }
495  if (direction.z () < 0.0)
496  {
497  origin.z () = static_cast<float> (this->min_z_) + static_cast<float> (this->max_z_) - origin.z ();
498  direction.z () = -direction.z ();
499  a |= 1;
500  }
501  min_x = (this->min_x_ - origin.x ()) / direction.x ();
502  max_x = (this->max_x_ - origin.x ()) / direction.x ();
503  min_y = (this->min_y_ - origin.y ()) / direction.y ();
504  max_y = (this->max_y_ - origin.y ()) / direction.y ();
505  min_z = (this->min_z_ - origin.z ()) / direction.z ();
506  max_z = (this->max_z_ - origin.z ()) / direction.z ();
507  }
508 
509  /** \brief Find first child node ray will enter
510  * \param[in] min_x octree nodes X coordinate of lower bounding box corner
511  * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
512  * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
513  * \param[in] mid_x octree nodes X coordinate of bounding box mid line
514  * \param[in] mid_y octree nodes Y coordinate of bounding box mid line
515  * \param[in] mid_z octree nodes Z coordinate of bounding box mid line
516  * \return the first child node ray will enter
517  */
518  inline int
519  getFirstIntersectedNode (double min_x, double min_y, double min_z, double mid_x, double mid_y, double mid_z) const
520  {
521  int currNode = 0;
522 
523  if (min_x > min_y)
524  {
525  if (min_x > min_z)
526  {
527  // max(min_x, min_y, min_z) is min_x. Entry plane is YZ.
528  if (mid_y < min_x)
529  currNode |= 2;
530  if (mid_z < min_x)
531  currNode |= 1;
532  }
533  else
534  {
535  // max(min_x, min_y, min_z) is min_z. Entry plane is XY.
536  if (mid_x < min_z)
537  currNode |= 4;
538  if (mid_y < min_z)
539  currNode |= 2;
540  }
541  }
542  else
543  {
544  if (min_y > min_z)
545  {
546  // max(min_x, min_y, min_z) is min_y. Entry plane is XZ.
547  if (mid_x < min_y)
548  currNode |= 4;
549  if (mid_z < min_y)
550  currNode |= 1;
551  }
552  else
553  {
554  // max(min_x, min_y, min_z) is min_z. Entry plane is XY.
555  if (mid_x < min_z)
556  currNode |= 4;
557  if (mid_y < min_z)
558  currNode |= 2;
559  }
560  }
561 
562  return currNode;
563  }
564 
565  /** \brief Get the next visited node given the current node upper
566  * bounding box corner. This function accepts three float values, and
567  * three int values. The function returns the ith integer where the
568  * ith float value is the minimum of the three float values.
569  * \param[in] x current nodes X coordinate of upper bounding box corner
570  * \param[in] y current nodes Y coordinate of upper bounding box corner
571  * \param[in] z current nodes Z coordinate of upper bounding box corner
572  * \param[in] a next node if exit Plane YZ
573  * \param[in] b next node if exit Plane XZ
574  * \param[in] c next node if exit Plane XY
575  * \return the next child node ray will enter or 8 if exiting
576  */
577  inline int
578  getNextIntersectedNode (double x, double y, double z, int a, int b, int c) const
579  {
580  if (x < y)
581  {
582  if (x < z)
583  return a;
584  else
585  return c;
586  }
587  else
588  {
589  if (y < z)
590  return b;
591  else
592  return c;
593  }
594 
595  return 0;
596  }
597 
598  };
599  }
600 }
601 
602 #ifdef PCL_NO_PRECOMPILE
603 #include <pcl/octree/impl/octree_search.hpp>
604 #endif
605 
606 #endif // PCL_OCTREE_SEARCH_H_
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...
Octree pointcloud class
prioBranchQueueEntry(OctreeNode *_node, OctreeKey &_key, float _point_distance)
Constructor for initializing priority queue entry.
bool voxelSearch(const PointT &point, std::vector< int > &point_idx_data)
Search for neighbors within a voxel at given point.
Priority queue entry for branch nodes
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
boost::shared_ptr< const PointCloud > PointCloudConstPtr
Definition: octree_search.h:66
boost::shared_ptr< std::vector< int > > IndicesPtr
Definition: octree_search.h:61
OctreePointCloud< PointT, LeafContainerT, BranchContainerT > OctreeT
Definition: octree_search.h:75
float pointSquaredDist(const PointT &point_a, const PointT &point_b) const
Helper function to calculate the squared distance between two points.
Priority queue entry for point candidates
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.
void approxNearestSearch(const PointCloud &cloud, int query_index, int &result_index, float &sqr_distance)
Search for approx.
boost::shared_ptr< const std::vector< int > > IndicesConstPtr
Definition: octree_search.h:62
virtual ~OctreePointCloudSearch()
Empty class destructor.
Definition: octree_search.h:89
prioPointQueueEntry(unsigned int &point_idx, float point_distance)
Constructor for initializing priority queue entry.
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.
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.
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).
boost::shared_ptr< OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT > > Ptr
Definition: octree_search.h:69
int point_idx_
Index representing a point in the dataset given by setInputCloud.
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.
const OctreeNode * node
Pointer to octree node.
OctreeKey key
Octree key.
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).
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...
boost::shared_ptr< PointCloud > PointCloudPtr
Definition: octree_search.h:65
prioPointQueueEntry()
Empty constructor.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
Definition: octree_search.h:73
PointCloud represents the base class in PCL for storing collections of 3D points. ...
pcl::PointCloud< PointT > PointCloud
Definition: octree_search.h:64
Octree key class
Definition: octree_key.h:51
float point_distance
Distance to query point.
Abstract octree leaf class
Definition: octree_nodes.h:97
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: norms.h:55
Octree pointcloud search class
Definition: octree_search.h:57
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.
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...
bool operator<(const prioBranchQueueEntry rhs) const
Operator< for comparing priority queue entries with each other.
OctreePointCloudSearch(const double resolution)
Constructor.
Definition: octree_search.h:82
A point structure representing Euclidean xyz coordinates, and the RGB color.
Abstract octree branch class
Definition: octree_nodes.h:204
float point_distance_
Distance to query point.
Abstract octree node class
Definition: octree_nodes.h:68
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.
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.
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...
prioBranchQueueEntry()
Empty constructor.
boost::shared_ptr< const OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT > > ConstPtr
Definition: octree_search.h:70