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