Point Cloud Library (PCL)  1.10.0
octree_search.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  */
38 
39 #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
62 
64  using PointCloudPtr = typename PointCloud::Ptr;
66 
67  // Boost shared pointers
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 Search for neighbors within a voxel at given point
87  * \param[in] point point addressing a leaf node voxel
88  * \param[out] point_idx_data the resultant indices of the neighboring voxel points
89  * \return "true" if leaf node exist; "false" otherwise
90  */
91  bool
92  voxelSearch (const PointT& point, std::vector<int>& point_idx_data);
93 
94  /** \brief Search for neighbors within a voxel at given point referenced by a point index
95  * \param[in] index the index in input cloud defining the query point
96  * \param[out] point_idx_data the resultant indices of the neighboring voxel points
97  * \return "true" if leaf node exist; "false" otherwise
98  */
99  bool
100  voxelSearch (const int index, std::vector<int>& point_idx_data);
101 
102  /** \brief Search for k-nearest neighbors at the query point.
103  * \param[in] cloud the point cloud data
104  * \param[in] index the index in \a cloud representing the query point
105  * \param[in] k the number of neighbors to search for
106  * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
107  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
108  * a priori!)
109  * \return number of neighbors found
110  */
111  inline int
112  nearestKSearch (const PointCloud &cloud, int index, int k, std::vector<int> &k_indices,
113  std::vector<float> &k_sqr_distances)
114  {
115  return (nearestKSearch (cloud[index], k, k_indices, k_sqr_distances));
116  }
117 
118  /** \brief Search for k-nearest neighbors at given query point.
119  * \param[in] p_q the given query point
120  * \param[in] k the number of neighbors to search for
121  * \param[out] k_indices the resultant indices of the neighboring points (must be resized to k a priori!)
122  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to k a priori!)
123  * \return number of neighbors found
124  */
125  int
126  nearestKSearch (const PointT &p_q, int k, std::vector<int> &k_indices,
127  std::vector<float> &k_sqr_distances);
128 
129  /** \brief Search for k-nearest neighbors at query point
130  * \param[in] index index representing the query point in the dataset given by \a setInputCloud.
131  * If indices were given in setInputCloud, index will be the position in the indices vector.
132  * \param[in] k the number of neighbors to search for
133  * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
134  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
135  * a priori!)
136  * \return number of neighbors found
137  */
138  int
139  nearestKSearch (int index, int k, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances);
140 
141  /** \brief Search for approx. nearest neighbor at the query point.
142  * \param[in] cloud the point cloud data
143  * \param[in] query_index the index in \a cloud representing the query point
144  * \param[out] result_index the resultant index of the neighbor point
145  * \param[out] sqr_distance the resultant squared distance to the neighboring point
146  * \return number of neighbors found
147  */
148  inline void
149  approxNearestSearch (const PointCloud &cloud, int query_index, int &result_index, float &sqr_distance)
150  {
151  return (approxNearestSearch (cloud.points[query_index], result_index, sqr_distance));
152  }
153 
154  /** \brief Search for approx. nearest neighbor at the query point.
155  * \param[in] p_q the given query point
156  * \param[out] result_index the resultant index of the neighbor point
157  * \param[out] sqr_distance the resultant squared distance to the neighboring point
158  */
159  void
160  approxNearestSearch (const PointT &p_q, int &result_index, float &sqr_distance);
161 
162  /** \brief Search for approx. nearest neighbor at the query point.
163  * \param[in] query_index index representing the query point in the dataset given by \a setInputCloud.
164  * If indices were given in setInputCloud, index will be the position in the indices vector.
165  * \param[out] result_index the resultant index of the neighbor point
166  * \param[out] sqr_distance the resultant squared distance to the neighboring point
167  * \return number of neighbors found
168  */
169  void
170  approxNearestSearch (int query_index, int &result_index, float &sqr_distance);
171 
172  /** \brief Search for all neighbors of query point that are within a given radius.
173  * \param[in] cloud the point cloud data
174  * \param[in] index the index in \a cloud representing the query point
175  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
176  * \param[out] k_indices the resultant indices of the neighboring points
177  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
178  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
179  * \return number of neighbors found in radius
180  */
181  int
182  radiusSearch (const PointCloud &cloud, int index, double radius, std::vector<int> &k_indices,
183  std::vector<float> &k_sqr_distances, unsigned int max_nn = 0)
184  {
185  return (radiusSearch (cloud.points[index], radius, k_indices, k_sqr_distances, max_nn));
186  }
187 
188  /** \brief Search for all neighbors of query point that are within a given radius.
189  * \param[in] p_q the given query point
190  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
191  * \param[out] k_indices the resultant indices of the neighboring points
192  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
193  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
194  * \return number of neighbors found in radius
195  */
196  int
197  radiusSearch (const PointT &p_q, const double radius, std::vector<int> &k_indices,
198  std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const;
199 
200  /** \brief Search for all neighbors of query point that are within a given radius.
201  * \param[in] index index representing the query point in the dataset given by \a setInputCloud.
202  * If indices were given in setInputCloud, index will be the position in the indices vector
203  * \param[in] radius radius of the sphere bounding all of p_q's neighbors
204  * \param[out] k_indices the resultant indices of the neighboring points
205  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
206  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
207  * \return number of neighbors found in radius
208  */
209  int
210  radiusSearch (int index, const double radius, std::vector<int> &k_indices,
211  std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const;
212 
213  /** \brief Get a PointT vector of centers of all voxels that intersected by a ray (origin, direction).
214  * \param[in] origin ray origin
215  * \param[in] direction ray direction vector
216  * \param[out] voxel_center_list results are written to this vector of PointT elements
217  * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0: disable)
218  * \return number of intersected voxels
219  */
220  int
221  getIntersectedVoxelCenters (Eigen::Vector3f origin, Eigen::Vector3f direction,
222  AlignedPointTVector &voxel_center_list, int max_voxel_count = 0) const;
223 
224  /** \brief Get indices of all voxels that are intersected by a ray (origin, direction).
225  * \param[in] origin ray origin
226  * \param[in] direction ray direction vector
227  * \param[out] k_indices resulting point indices from intersected voxels
228  * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0: disable)
229  * \return number of intersected voxels
230  */
231  int
232  getIntersectedVoxelIndices (Eigen::Vector3f origin, Eigen::Vector3f direction,
233  std::vector<int> &k_indices,
234  int max_voxel_count = 0) const;
235 
236 
237  /** \brief Search for points within rectangular search area
238  * Points exactly on the edges of the search rectangle are included.
239  * \param[in] min_pt lower corner of search area
240  * \param[in] max_pt upper corner of search area
241  * \param[out] k_indices the resultant point indices
242  * \return number of points found within search area
243  */
244  int
245  boxSearch (const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, std::vector<int> &k_indices) const;
246 
247  protected:
248  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
249  // Octree-based search routines & helpers
250  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
251  /** \brief @b Priority queue entry for branch nodes
252  * \note This class defines priority queue entries for the nearest neighbor search.
253  * \author Julius Kammerl (julius@kammerl.de)
254  */
256  {
257  public:
258  /** \brief Empty constructor */
260  node (), point_distance (0)
261  {
262  }
263 
264  /** \brief Constructor for initializing priority queue entry.
265  * \param _node pointer to octree node
266  * \param _key octree key addressing voxel in octree structure
267  * \param[in] _point_distance distance of query point to voxel center
268  */
269  prioBranchQueueEntry (OctreeNode* _node, OctreeKey& _key, float _point_distance) :
270  node (_node), point_distance (_point_distance), key (_key)
271  {
272  }
273 
274  /** \brief Operator< for comparing priority queue entries with each other.
275  * \param[in] rhs the priority queue to compare this against
276  */
277  bool
279  {
280  return (this->point_distance > rhs.point_distance);
281  }
282 
283  /** \brief Pointer to octree node. */
284  const OctreeNode* node;
285 
286  /** \brief Distance to query point. */
288 
289  /** \brief Octree key. */
291  };
292 
293  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
294  /** \brief @b Priority queue entry for point candidates
295  * \note This class defines priority queue entries for the nearest neighbor point candidates.
296  * \author Julius Kammerl (julius@kammerl.de)
297  */
299  {
300  public:
301 
302  /** \brief Empty constructor */
304  point_idx_ (0), point_distance_ (0)
305  {
306  }
307 
308  /** \brief Constructor for initializing priority queue entry.
309  * \param[in] point_idx an index representing a point in the dataset given by \a setInputCloud
310  * \param[in] point_distance distance of query point to voxel center
311  */
312  prioPointQueueEntry (unsigned int& point_idx, float point_distance) :
313  point_idx_ (point_idx), point_distance_ (point_distance)
314  {
315  }
316 
317  /** \brief Operator< for comparing priority queue entries with each other.
318  * \param[in] rhs priority queue to compare this against
319  */
320  bool
321  operator< (const prioPointQueueEntry& rhs) const
322  {
323  return (this->point_distance_ < rhs.point_distance_);
324  }
325 
326  /** \brief Index representing a point in the dataset given by \a setInputCloud. */
328 
329  /** \brief Distance to query point. */
331  };
332 
333  /** \brief Helper function to calculate the squared distance between two points
334  * \param[in] point_a point A
335  * \param[in] point_b point B
336  * \return squared distance between point A and point B
337  */
338  float
339  pointSquaredDist (const PointT& point_a, const PointT& point_b) const;
340 
341  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
342  // Recursive search routine methods
343  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
344 
345  /** \brief Recursive search method that explores the octree and finds neighbors within a given radius
346  * \param[in] point query point
347  * \param[in] radiusSquared squared search radius
348  * \param[in] node current octree node to be explored
349  * \param[in] key octree key addressing a leaf node.
350  * \param[in] tree_depth current depth/level in the octree
351  * \param[out] k_indices vector of indices found to be neighbors of query point
352  * \param[out] k_sqr_distances squared distances of neighbors to query point
353  * \param[in] max_nn maximum of neighbors to be found
354  */
355  void
356  getNeighborsWithinRadiusRecursive (const PointT& point, const double radiusSquared,
357  const BranchNode* node, const OctreeKey& key,
358  unsigned int tree_depth, std::vector<int>& k_indices,
359  std::vector<float>& k_sqr_distances, unsigned int max_nn) const;
360 
361  /** \brief Recursive search method that explores the octree and finds the K nearest neighbors
362  * \param[in] point query point
363  * \param[in] K amount of nearest neighbors to be found
364  * \param[in] node current octree node to be explored
365  * \param[in] key octree key addressing a leaf node.
366  * \param[in] tree_depth current depth/level in the octree
367  * \param[in] squared_search_radius squared search radius distance
368  * \param[out] point_candidates priority queue of nearest neigbor point candidates
369  * \return squared search radius based on current point candidate set found
370  */
371  double
372  getKNearestNeighborRecursive (const PointT& point, unsigned int K, const BranchNode* node,
373  const OctreeKey& key, unsigned int tree_depth,
374  const double squared_search_radius,
375  std::vector<prioPointQueueEntry>& point_candidates) const;
376 
377  /** \brief Recursive search method that explores the octree and finds the approximate nearest neighbor
378  * \param[in] point query point
379  * \param[in] node current octree node to be explored
380  * \param[in] key octree key addressing a leaf node.
381  * \param[in] tree_depth current depth/level in the octree
382  * \param[out] result_index result index is written to this reference
383  * \param[out] sqr_distance squared distance to search
384  */
385  void
386  approxNearestSearchRecursive (const PointT& point, const BranchNode* node, const OctreeKey& key,
387  unsigned int tree_depth, int& result_index, float& sqr_distance);
388 
389  /** \brief Recursively search the tree for all intersected leaf nodes and return a vector of voxel centers.
390  * This algorithm is based off the paper An Efficient Parametric Algorithm for Octree Traversal:
391  * http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf
392  * \param[in] min_x octree nodes X coordinate of lower bounding box corner
393  * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
394  * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
395  * \param[in] max_x octree nodes X coordinate of upper bounding box corner
396  * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
397  * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
398  * \param[in] a
399  * \param[in] node current octree node to be explored
400  * \param[in] key octree key addressing a leaf node.
401  * \param[out] voxel_center_list results are written to this vector of PointT elements
402  * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0: disable)
403  * \return number of voxels found
404  */
405  int
406  getIntersectedVoxelCentersRecursive (double min_x, double min_y, double min_z, double max_x, double max_y,
407  double max_z, unsigned char a, const OctreeNode* node,
408  const OctreeKey& key, AlignedPointTVector &voxel_center_list,
409  int max_voxel_count) const;
410 
411 
412  /** \brief Recursive search method that explores the octree and finds points within a rectangular search area
413  * \param[in] min_pt lower corner of search area
414  * \param[in] max_pt upper corner of search area
415  * \param[in] node current octree node to be explored
416  * \param[in] key octree key addressing a leaf node.
417  * \param[in] tree_depth current depth/level in the octree
418  * \param[out] k_indices the resultant point indices
419  */
420  void
421  boxSearchRecursive (const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, const BranchNode* node,
422  const OctreeKey& key, unsigned int tree_depth, std::vector<int>& k_indices) const;
423 
424  /** \brief Recursively search the tree for all intersected leaf nodes and return a vector of indices.
425  * This algorithm is based off the paper An Efficient Parametric Algorithm for Octree Traversal:
426  * http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf
427  * \param[in] min_x octree nodes X coordinate of lower bounding box corner
428  * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
429  * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
430  * \param[in] max_x octree nodes X coordinate of upper bounding box corner
431  * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
432  * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
433  * \param[in] a
434  * \param[in] node current octree node to be explored
435  * \param[in] key octree key addressing a leaf node.
436  * \param[out] k_indices resulting indices
437  * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0: disable)
438  * \return number of voxels found
439  */
440  int
441  getIntersectedVoxelIndicesRecursive (double min_x, double min_y, double min_z,
442  double max_x, double max_y, double max_z,
443  unsigned char a, const OctreeNode* node, const OctreeKey& key,
444  std::vector<int> &k_indices,
445  int max_voxel_count) const;
446 
447  /** \brief Initialize raytracing algorithm
448  * \param origin
449  * \param direction
450  * \param[in] min_x octree nodes X coordinate of lower bounding box corner
451  * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
452  * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
453  * \param[in] max_x octree nodes X coordinate of upper bounding box corner
454  * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
455  * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
456  * \param a
457  */
458  inline void
459  initIntersectedVoxel (Eigen::Vector3f &origin, Eigen::Vector3f &direction,
460  double &min_x, double &min_y, double &min_z,
461  double &max_x, double &max_y, double &max_z,
462  unsigned char &a) const
463  {
464  // Account for division by zero when direction vector is 0.0
465  const float epsilon = 1e-10f;
466  if (direction.x () == 0.0)
467  direction.x () = epsilon;
468  if (direction.y () == 0.0)
469  direction.y () = epsilon;
470  if (direction.z () == 0.0)
471  direction.z () = epsilon;
472 
473  // Voxel childIdx remapping
474  a = 0;
475 
476  // Handle negative axis direction vector
477  if (direction.x () < 0.0)
478  {
479  origin.x () = static_cast<float> (this->min_x_) + static_cast<float> (this->max_x_) - origin.x ();
480  direction.x () = -direction.x ();
481  a |= 4;
482  }
483  if (direction.y () < 0.0)
484  {
485  origin.y () = static_cast<float> (this->min_y_) + static_cast<float> (this->max_y_) - origin.y ();
486  direction.y () = -direction.y ();
487  a |= 2;
488  }
489  if (direction.z () < 0.0)
490  {
491  origin.z () = static_cast<float> (this->min_z_) + static_cast<float> (this->max_z_) - origin.z ();
492  direction.z () = -direction.z ();
493  a |= 1;
494  }
495  min_x = (this->min_x_ - origin.x ()) / direction.x ();
496  max_x = (this->max_x_ - origin.x ()) / direction.x ();
497  min_y = (this->min_y_ - origin.y ()) / direction.y ();
498  max_y = (this->max_y_ - origin.y ()) / direction.y ();
499  min_z = (this->min_z_ - origin.z ()) / direction.z ();
500  max_z = (this->max_z_ - origin.z ()) / direction.z ();
501  }
502 
503  /** \brief Find first child node ray will enter
504  * \param[in] min_x octree nodes X coordinate of lower bounding box corner
505  * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
506  * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
507  * \param[in] mid_x octree nodes X coordinate of bounding box mid line
508  * \param[in] mid_y octree nodes Y coordinate of bounding box mid line
509  * \param[in] mid_z octree nodes Z coordinate of bounding box mid line
510  * \return the first child node ray will enter
511  */
512  inline int
513  getFirstIntersectedNode (double min_x, double min_y, double min_z, double mid_x, double mid_y, double mid_z) const
514  {
515  int currNode = 0;
516 
517  if (min_x > min_y)
518  {
519  if (min_x > min_z)
520  {
521  // max(min_x, min_y, min_z) is min_x. Entry plane is YZ.
522  if (mid_y < min_x)
523  currNode |= 2;
524  if (mid_z < min_x)
525  currNode |= 1;
526  }
527  else
528  {
529  // max(min_x, min_y, min_z) is min_z. Entry plane is XY.
530  if (mid_x < min_z)
531  currNode |= 4;
532  if (mid_y < min_z)
533  currNode |= 2;
534  }
535  }
536  else
537  {
538  if (min_y > min_z)
539  {
540  // max(min_x, min_y, min_z) is min_y. Entry plane is XZ.
541  if (mid_x < min_y)
542  currNode |= 4;
543  if (mid_z < min_y)
544  currNode |= 1;
545  }
546  else
547  {
548  // max(min_x, min_y, min_z) is min_z. Entry plane is XY.
549  if (mid_x < min_z)
550  currNode |= 4;
551  if (mid_y < min_z)
552  currNode |= 2;
553  }
554  }
555 
556  return currNode;
557  }
558 
559  /** \brief Get the next visited node given the current node upper
560  * bounding box corner. This function accepts three float values, and
561  * three int values. The function returns the ith integer where the
562  * ith float value is the minimum of the three float values.
563  * \param[in] x current nodes X coordinate of upper bounding box corner
564  * \param[in] y current nodes Y coordinate of upper bounding box corner
565  * \param[in] z current nodes Z coordinate of upper bounding box corner
566  * \param[in] a next node if exit Plane YZ
567  * \param[in] b next node if exit Plane XZ
568  * \param[in] c next node if exit Plane XY
569  * \return the next child node ray will enter or 8 if exiting
570  */
571  inline int
572  getNextIntersectedNode (double x, double y, double z, int a, int b, int c) const
573  {
574  if (x < y)
575  {
576  if (x < z)
577  return a;
578  return c;
579  }
580  if (y < z)
581  return b;
582  return c;
583  }
584 
585  };
586  }
587 }
588 
589 #ifdef PCL_NO_PRECOMPILE
590 #include <pcl/octree/impl/octree_search.hpp>
591 #endif
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
pcl::K
Definition: norms.h:54
pcl::octree::OctreePointCloudSearch::prioPointQueueEntry::prioPointQueueEntry
prioPointQueueEntry()
Empty constructor
Definition: octree_search.h:303
pcl::octree::OctreePointCloudSearch::getIntersectedVoxelIndicesRecursive
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.
Definition: octree_search.hpp:724
pcl::octree::OctreePointCloudSearch::getIntersectedVoxelIndices
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).
Definition: octree_search.hpp:583
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:397
pcl::octree::OctreePointCloudSearch::AlignedPointTVector
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
Definition: octree_search.h:72
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::prioBranchQueueEntry
prioBranchQueueEntry(OctreeNode *_node, OctreeKey &_key, float _point_distance)
Constructor for initializing priority queue entry.
Definition: octree_search.h:269
pcl::octree::OctreePointCloudSearch::getKNearestNeighborRecursive
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.
Definition: octree_search.hpp:205
pcl::octree::OctreePointCloud< PointT, LeafTWrap, BranchTWrap >::PointCloudConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: octree_pointcloud.h:91
pcl::octree::OctreePointCloud
Octree pointcloud class
Definition: octree_pointcloud.h:72
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::node
const OctreeNode * node
Pointer to octree node.
Definition: octree_search.h:284
pcl::octree::OctreePointCloudSearch::prioPointQueueEntry::operator<
bool operator<(const prioPointQueueEntry &rhs) const
Operator< for comparing priority queue entries with each other.
Definition: octree_search.h:321
pcl::octree::OctreePointCloudSearch::boxSearchRecursive
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.
Definition: octree_search.hpp:485
pcl::octree::OctreePointCloudSearch::voxelSearch
bool voxelSearch(const PointT &point, std::vector< int > &point_idx_data)
Search for neighbors within a voxel at given point.
Definition: octree_search.hpp:46
pcl::octree::OctreePointCloudSearch::nearestKSearch
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.
Definition: octree_search.h:112
pcl::octree::OctreePointCloudSearch::getNeighborsWithinRadiusRecursive
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.
Definition: octree_search.hpp:309
pcl::octree::OctreePointCloudSearch::getIntersectedVoxelCenters
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).
Definition: octree_search.hpp:558
pcl::octree::OctreeKey
Octree key class
Definition: octree_key.h:50
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: projection_matrix.h:52
pcl::octree::OctreePointCloudSearch::radiusSearch
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.
Definition: octree_search.h:182
pcl::octree::OctreePointCloudSearch::approxNearestSearch
void approxNearestSearch(const PointCloud &cloud, int query_index, int &result_index, float &sqr_distance)
Search for approx.
Definition: octree_search.h:149
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:623
pcl::octree::OctreeNode
Abstract octree node class
Definition: octree_nodes.h:67
pcl::octree::OctreePointCloud< PointT, LeafContainerT, BranchContainerT >::min_y_
double min_y_
Definition: octree_pointcloud.h:527
pcl::octree::OctreePointCloudSearch::prioPointQueueEntry
Priority queue entry for point candidates
Definition: octree_search.h:298
pcl::octree::OctreeLeafNode
Abstract octree leaf class
Definition: octree_nodes.h:96
pcl::octree::OctreePointCloudSearch::prioPointQueueEntry::point_idx_
int point_idx_
Index representing a point in the dataset given by setInputCloud.
Definition: octree_search.h:327
pcl::octree::OctreePointCloudSearch::prioPointQueueEntry::point_distance_
float point_distance_
Distance to query point.
Definition: octree_search.h:330
pcl::octree::OctreePointCloud< PointT, LeafTWrap, BranchTWrap >::IndicesConstPtr
shared_ptr< const std::vector< int > > IndicesConstPtr
Definition: octree_pointcloud.h:87
pcl::octree::OctreeBase< LeafTWrap, BranchTWrap >
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::key
OctreeKey key
Octree key.
Definition: octree_search.h:290
pcl::octree::OctreePointCloudSearch::OctreePointCloudSearch
OctreePointCloudSearch(const double resolution)
Constructor.
Definition: octree_search.h:81
pcl::octree::OctreeBranchNode
Abstract octree branch class
Definition: octree_nodes.h:203
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::point_distance
float point_distance
Distance to query point.
Definition: octree_search.h:287
pcl::octree::OctreePointCloudSearch::getNextIntersectedNode
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.
Definition: octree_search.h:572
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry
Priority queue entry for branch nodes
Definition: octree_search.h:255
pcl::octree::OctreePointCloud< PointT, LeafTWrap, BranchTWrap >::PointCloudPtr
typename PointCloud::Ptr PointCloudPtr
Definition: octree_pointcloud.h:90
pcl::octree::OctreePointCloud< PointT, LeafContainerT, BranchContainerT >::min_z_
double min_z_
Definition: octree_pointcloud.h:530
pcl::octree::OctreePointCloud< PointT, LeafContainerT, BranchContainerT >::max_x_
double max_x_
Definition: octree_pointcloud.h:525
pcl::octree::OctreePointCloud< PointT, LeafContainerT, BranchContainerT >::BranchNode
typename OctreeBase< LeafContainerT, BranchContainerT > ::BranchNode BranchNode
Definition: octree_pointcloud.h:78
pcl::octree::OctreePointCloudSearch::approxNearestSearchRecursive
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.
Definition: octree_search.hpp:389
pcl::octree::OctreePointCloudSearch::initIntersectedVoxel
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.
Definition: octree_search.h:459
pcl::octree::OctreePointCloud< PointT, LeafTWrap, BranchTWrap >::ConstPtr
shared_ptr< const OctreePointCloud< PointT, LeafTWrap, BranchTWrap, OctreeBase< LeafTWrap, BranchTWrap > > > ConstPtr
Definition: octree_pointcloud.h:99
pcl::octree::OctreePointCloud< PointT, LeafTWrap, BranchTWrap >::Ptr
shared_ptr< OctreePointCloud< PointT, LeafTWrap, BranchTWrap, OctreeBase< LeafTWrap, BranchTWrap > > > Ptr
Definition: octree_pointcloud.h:98
pcl::octree::OctreePointCloud< PointT, LeafContainerT, BranchContainerT >::max_z_
double max_z_
Definition: octree_pointcloud.h:531
pcl::octree::OctreePointCloud< PointT, LeafContainerT, BranchContainerT >::max_y_
double max_y_
Definition: octree_pointcloud.h:528
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:415
pcl::octree::OctreePointCloudSearch::pointSquaredDist
float pointSquaredDist(const PointT &point_a, const PointT &point_b) const
Helper function to calculate the squared distance between two points.
Definition: octree_search.hpp:477
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::operator<
bool operator<(const prioBranchQueueEntry rhs) const
Operator< for comparing priority queue entries with each other.
Definition: octree_search.h:278
pcl::octree::OctreePointCloudSearch::prioPointQueueEntry::prioPointQueueEntry
prioPointQueueEntry(unsigned int &point_idx, float point_distance)
Constructor for initializing priority queue entry.
Definition: octree_search.h:312
pcl::octree::OctreePointCloudSearch::getIntersectedVoxelCentersRecursive
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: octree_search.hpp:606
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:416
pcl::octree::OctreePointCloud< PointT, LeafTWrap, BranchTWrap >::IndicesPtr
shared_ptr< std::vector< int > > IndicesPtr
Definition: octree_pointcloud.h:86
pcl::octree::OctreePointCloudSearch::getFirstIntersectedNode
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.
Definition: octree_search.h:513
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::prioBranchQueueEntry
prioBranchQueueEntry()
Empty constructor
Definition: octree_search.h:259
pcl::octree::OctreePointCloudSearch
Octree pointcloud search class
Definition: octree_search.h:56
pcl::octree::OctreePointCloud< PointT, LeafTWrap, BranchTWrap >::AlignedPointTVector
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
Definition: octree_pointcloud.h:102
pcl::octree::OctreePointCloudSearch::boxSearch
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 ...
Definition: octree_search.hpp:187
pcl::octree::OctreePointCloud< PointT, LeafContainerT, BranchContainerT >::LeafNode
typename OctreeBase< LeafContainerT, BranchContainerT > ::LeafNode LeafNode
Definition: octree_pointcloud.h:77
pcl::shared_ptr
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: pcl_macros.h:90
pcl::octree::OctreePointCloud< PointT, LeafContainerT, BranchContainerT >::min_x_
double min_x_
Definition: octree_pointcloud.h:524