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_
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
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
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
bool operator<(const prioPointQueueEntry &rhs) const
Operator< for comparing priority queue entries with each other.
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 Points exactly on the edges of the search rectangle ...
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