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  using IndicesPtr = boost::shared_ptr<std::vector<int> >;
61  using IndicesConstPtr = boost::shared_ptr<const std::vector<int> >;
62 
64  using PointCloudPtr = boost::shared_ptr<PointCloud>;
65  using PointCloudConstPtr = boost::shared_ptr<const PointCloud>;
66 
67  // Boost shared pointers
68  using Ptr = boost::shared_ptr<OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT> >;
69  using ConstPtr = boost::shared_ptr<const OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT> >;
70 
71  // Eigen aligned allocator
72  using AlignedPointTVector = std::vector<PointT, Eigen::aligned_allocator<PointT> >;
73 
75  using LeafNode = typename OctreeT::LeafNode;
76  using BranchNode = typename OctreeT::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  return c;
585  }
586  if (y < z)
587  return b;
588  return c;
589  }
590 
591  };
592  }
593 }
594 
595 #ifdef PCL_NO_PRECOMPILE
596 #include <pcl/octree/impl/octree_search.hpp>
597 #endif
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.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
Definition: octree_search.h:72
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
Octree class.
Definition: octree_base.h:61
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:426
typename OctreeBase< LeafContainerT, BranchContainerT > ::BranchNode BranchNode
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.
boost::shared_ptr< OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT > > Ptr
Definition: octree_search.h:68
void approxNearestSearch(const PointCloud &cloud, int query_index, int &result_index, float &sqr_distance)
Search for approx.
boost::shared_ptr< PointCloud > PointCloudPtr
Definition: octree_search.h:64
boost::shared_ptr< std::vector< int > > IndicesPtr
Definition: octree_search.h:60
prioPointQueueEntry(unsigned int &point_idx, float point_distance)
Constructor for initializing priority queue entry.
boost::shared_ptr< const PointCloud > PointCloudConstPtr
Definition: octree_search.h:65
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).
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< const OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT > > ConstPtr
Definition: octree_search.h:69
prioPointQueueEntry()
Empty constructor.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Octree key class
Definition: octree_key.h:50
float point_distance
Distance to query point.
Abstract octree leaf class
Definition: octree_nodes.h:96
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: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.
boost::shared_ptr< const std::vector< int > > IndicesConstPtr
Definition: octree_search.h:61
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...
~OctreePointCloudSearch()
Empty class destructor.
Definition: octree_search.h:88
bool operator<(const prioBranchQueueEntry rhs) const
Operator< for comparing priority queue entries with each other.
OctreePointCloudSearch(const double resolution)
Constructor.
Definition: octree_search.h:81
typename OctreeBase< LeafContainerT, BranchContainerT > ::LeafNode LeafNode
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
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.