Point Cloud Library (PCL)  1.10.0-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
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
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.
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:415
Priority queue entry for branch nodes
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:397
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.
void approxNearestSearch(const PointCloud &cloud, int query_index, int &result_index, float &sqr_distance)
Search for approx.
shared_ptr< const OctreePointCloud< PointT, LeafTWrap, BranchTWrap, OctreeBase< LeafTWrap, BranchTWrap > > > ConstPtr
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
prioPointQueueEntry(unsigned int &point_idx, float point_distance)
Constructor for initializing priority queue entry.
int getFirstIntersectedNode(double min_x, double min_y, double min_z, double mid_x, double mid_y, double mid_z) const
Find first child node ray will enter.
int getNextIntersectedNode(double x, double y, double z, int a, int b, int c) const
Get the next visited node given the current node upper bounding box corner.
int getIntersectedVoxelIndices(Eigen::Vector3f origin, Eigen::Vector3f direction, std::vector< int > &k_indices, int max_voxel_count=0) const
Get indices of all voxels that are intersected by a ray (origin, direction).
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...
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.
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...
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:416
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.
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: pcl_macros.h:90
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.
shared_ptr< OctreePointCloud< PointT, LeafTWrap, BranchTWrap, OctreeBase< LeafTWrap, BranchTWrap > > > Ptr