Point Cloud Library (PCL)  1.8.0
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 #include <pcl/point_types.h>
44 
45 #include "octree_pointcloud.h"
46 
47 namespace pcl
48 {
49  namespace octree
50  {
51  /** \brief @b Octree pointcloud search class
52  * \note This class provides several methods for spatial neighbor search based on octree structure
53  * \note typename: PointT: type of point used in pointcloud
54  * \ingroup octree
55  * \author Julius Kammerl (julius@kammerl.de)
56  */
57  template<typename PointT, typename LeafContainerT = OctreeContainerPointIndices , typename BranchContainerT = OctreeContainerEmpty >
58  class OctreePointCloudSearch : public OctreePointCloud<PointT, LeafContainerT, BranchContainerT>
59  {
60  public:
61  // public typedefs
62  typedef boost::shared_ptr<std::vector<int> > IndicesPtr;
63  typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr;
64 
66  typedef boost::shared_ptr<PointCloud> PointCloudPtr;
67  typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
68 
69  // Boost shared pointers
70  typedef boost::shared_ptr<OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT> > Ptr;
71  typedef boost::shared_ptr<const OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT> > ConstPtr;
72 
73  // Eigen aligned allocator
74  typedef std::vector<PointT, Eigen::aligned_allocator<PointT> > AlignedPointTVector;
75 
77  typedef typename OctreeT::LeafNode LeafNode;
78  typedef typename OctreeT::BranchNode BranchNode;
79 
80  /** \brief Constructor.
81  * \param[in] resolution octree resolution at lowest octree level
82  */
83  OctreePointCloudSearch (const double resolution) :
84  OctreePointCloud<PointT, LeafContainerT, BranchContainerT> (resolution)
85  {
86  }
87 
88  /** \brief Empty class destructor. */
89  virtual
91  {
92  }
93 
94  /** \brief Search for neighbors within a voxel at given point
95  * \param[in] point point addressing a leaf node voxel
96  * \param[out] point_idx_data the resultant indices of the neighboring voxel points
97  * \return "true" if leaf node exist; "false" otherwise
98  */
99  bool
100  voxelSearch (const PointT& point, std::vector<int>& point_idx_data);
101 
102  /** \brief Search for neighbors within a voxel at given point referenced by a point index
103  * \param[in] index the index in input cloud defining the query point
104  * \param[out] point_idx_data the resultant indices of the neighboring voxel points
105  * \return "true" if leaf node exist; "false" otherwise
106  */
107  bool
108  voxelSearch (const int index, std::vector<int>& point_idx_data);
109 
110  /** \brief Search for k-nearest neighbors at the query point.
111  * \param[in] cloud the point cloud data
112  * \param[in] index the index in \a cloud representing the query point
113  * \param[in] k the number of neighbors to search for
114  * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
115  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
116  * a priori!)
117  * \return number of neighbors found
118  */
119  inline int
120  nearestKSearch (const PointCloud &cloud, int index, int k, std::vector<int> &k_indices,
121  std::vector<float> &k_sqr_distances)
122  {
123  return (nearestKSearch (cloud[index], k, k_indices, k_sqr_distances));
124  }
125 
126  /** \brief Search for k-nearest neighbors at given query point.
127  * \param[in] p_q the given query point
128  * \param[in] k the number of neighbors to search for
129  * \param[out] k_indices the resultant indices of the neighboring points (must be resized to k a priori!)
130  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to k a priori!)
131  * \return number of neighbors found
132  */
133  int
134  nearestKSearch (const PointT &p_q, int k, std::vector<int> &k_indices,
135  std::vector<float> &k_sqr_distances);
136 
137  /** \brief Search for k-nearest neighbors at query point
138  * \param[in] index index representing the query point in the dataset given by \a setInputCloud.
139  * If indices were given in setInputCloud, index will be the position in the indices vector.
140  * \param[in] k the number of neighbors to search for
141  * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
142  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
143  * a priori!)
144  * \return number of neighbors found
145  */
146  int
147  nearestKSearch (int index, int k, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances);
148 
149  /** \brief Search for approx. nearest neighbor at the query point.
150  * \param[in] cloud the point cloud data
151  * \param[in] query_index the index in \a cloud representing the query point
152  * \param[out] result_index the resultant index of the neighbor point
153  * \param[out] sqr_distance the resultant squared distance to the neighboring point
154  * \return number of neighbors found
155  */
156  inline void
157  approxNearestSearch (const PointCloud &cloud, int query_index, int &result_index, float &sqr_distance)
158  {
159  return (approxNearestSearch (cloud.points[query_index], result_index, sqr_distance));
160  }
161 
162  /** \brief Search for approx. nearest neighbor at the query point.
163  * \param[in] p_q the given query point
164  * \param[out] result_index the resultant index of the neighbor point
165  * \param[out] sqr_distance the resultant squared distance to the neighboring point
166  */
167  void
168  approxNearestSearch (const PointT &p_q, int &result_index, float &sqr_distance);
169 
170  /** \brief Search for approx. nearest neighbor at the query point.
171  * \param[in] query_index index representing the query point in the dataset given by \a setInputCloud.
172  * If indices were given in setInputCloud, index will be the position in the indices vector.
173  * \param[out] result_index the resultant index of the neighbor point
174  * \param[out] sqr_distance the resultant squared distance to the neighboring point
175  * \return number of neighbors found
176  */
177  void
178  approxNearestSearch (int query_index, int &result_index, float &sqr_distance);
179 
180  /** \brief Search for all neighbors of query point that are within a given radius.
181  * \param[in] cloud the point cloud data
182  * \param[in] index the index in \a cloud representing the query point
183  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
184  * \param[out] k_indices the resultant indices of the neighboring points
185  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
186  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
187  * \return number of neighbors found in radius
188  */
189  int
190  radiusSearch (const PointCloud &cloud, int index, double radius, std::vector<int> &k_indices,
191  std::vector<float> &k_sqr_distances, unsigned int max_nn = 0)
192  {
193  return (radiusSearch (cloud.points[index], radius, k_indices, k_sqr_distances, max_nn));
194  }
195 
196  /** \brief Search for all neighbors of query point that are within a given radius.
197  * \param[in] p_q the given query point
198  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
199  * \param[out] k_indices the resultant indices of the neighboring points
200  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
201  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
202  * \return number of neighbors found in radius
203  */
204  int
205  radiusSearch (const PointT &p_q, const double radius, std::vector<int> &k_indices,
206  std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const;
207 
208  /** \brief Search for all neighbors of query point that are within a given radius.
209  * \param[in] index index representing the query point in the dataset given by \a setInputCloud.
210  * If indices were given in setInputCloud, index will be the position in the indices vector
211  * \param[in] radius radius of the sphere bounding all of p_q's neighbors
212  * \param[out] k_indices the resultant indices of the neighboring points
213  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
214  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
215  * \return number of neighbors found in radius
216  */
217  int
218  radiusSearch (int index, const double radius, std::vector<int> &k_indices,
219  std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const;
220 
221  /** \brief Get a PointT vector of centers of all voxels that intersected by a ray (origin, direction).
222  * \param[in] origin ray origin
223  * \param[in] direction ray direction vector
224  * \param[out] voxel_center_list results are written to this vector of PointT elements
225  * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0: disable)
226  * \return number of intersected voxels
227  */
228  int
229  getIntersectedVoxelCenters (Eigen::Vector3f origin, Eigen::Vector3f direction,
230  AlignedPointTVector &voxel_center_list, int max_voxel_count = 0) const;
231 
232  /** \brief Get indices of all voxels that are intersected by a ray (origin, direction).
233  * \param[in] origin ray origin
234  * \param[in] direction ray direction vector
235  * \param[out] k_indices resulting point indices from intersected voxels
236  * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0: disable)
237  * \return number of intersected voxels
238  */
239  int
240  getIntersectedVoxelIndices (Eigen::Vector3f origin, Eigen::Vector3f direction,
241  std::vector<int> &k_indices,
242  int max_voxel_count = 0) const;
243 
244 
245  /** \brief Search for points within rectangular search area
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 #else
606 #define PCL_INSTANTIATE_OctreePointCloudSearch(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudSearch<T>;
607 #endif // PCL_NO_PRECOMPILE
608 
609 #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:67
boost::shared_ptr< std::vector< int > > IndicesPtr
Definition: octree_search.h:62
OctreePointCloud< PointT, LeafContainerT, BranchContainerT > OctreeT
Definition: octree_search.h:76
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:63
virtual ~OctreePointCloudSearch()
Empty class destructor.
Definition: octree_search.h:90
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:70
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...
Defines all the PCL implemented PointT point type structures.
boost::shared_ptr< PointCloud > PointCloudPtr
Definition: octree_search.h:66
prioPointQueueEntry()
Empty constructor.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
Definition: octree_search.h:74
PointCloud represents the base class in PCL for storing collections of 3D points. ...
pcl::PointCloud< PointT > PointCloud
Definition: octree_search.h:65
Octree key class
Definition: octree_key.h:53
float point_distance
Distance to query point.
Abstract octree leaf class
Definition: octree_nodes.h:100
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:58
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:83
A point structure representing Euclidean xyz coordinates, and the RGB color.
Abstract octree branch class
Definition: octree_nodes.h:207
float point_distance_
Distance to query point.
Abstract octree node class
Definition: octree_nodes.h:71
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:71