Point Cloud Library (PCL)  1.12.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/octree/octree_pointcloud.h>
42 #include <pcl/point_cloud.h>
43 
44 namespace pcl {
45 namespace octree {
46 
47 /** \brief @b Octree pointcloud search class
48  * \note This class provides several methods for spatial neighbor search based on octree
49  * structure
50  * \tparam PointT type of point used in pointcloud
51  * \ingroup octree
52  * \author Julius Kammerl (julius@kammerl.de)
53  */
54 template <typename PointT,
55  typename LeafContainerT = OctreeContainerPointIndices,
56  typename BranchContainerT = OctreeContainerEmpty>
58 : public OctreePointCloud<PointT, LeafContainerT, BranchContainerT> {
59 public:
60  // public typedefs
61  using IndicesPtr = shared_ptr<Indices>;
62  using IndicesConstPtr = shared_ptr<const Indices>;
63 
65  using PointCloudPtr = typename PointCloud::Ptr;
67 
68  // Boost shared pointers
69  using Ptr =
70  shared_ptr<OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>>;
71  using ConstPtr = shared_ptr<
73 
74  // Eigen aligned allocator
75  using AlignedPointTVector = std::vector<PointT, Eigen::aligned_allocator<PointT>>;
76 
78  using LeafNode = typename OctreeT::LeafNode;
79  using BranchNode = typename OctreeT::BranchNode;
80 
81  /** \brief Constructor.
82  * \param[in] resolution octree resolution at lowest octree level
83  */
84  OctreePointCloudSearch(const double resolution)
85  : OctreePointCloud<PointT, LeafContainerT, BranchContainerT>(resolution)
86  {}
87 
88  /** \brief Search for neighbors within a voxel at given point
89  * \param[in] point point addressing a leaf node voxel
90  * \param[out] point_idx_data the resultant indices of the neighboring voxel points
91  * \return "true" if leaf node exist; "false" otherwise
92  */
93  bool
94  voxelSearch(const PointT& point, Indices& point_idx_data);
95 
96  /** \brief Search for neighbors within a voxel at given point referenced by a point
97  * index
98  * \param[in] index the index in input cloud defining the query point
99  * \param[out] point_idx_data the resultant indices of the neighboring voxel points
100  * \return "true" if leaf node exist; "false" otherwise
101  */
102  bool
103  voxelSearch(uindex_t index, Indices& point_idx_data);
104 
105  /** \brief Search for k-nearest neighbors at the query point.
106  * \param[in] cloud the point cloud data
107  * \param[in] index the index in \a cloud representing the query point
108  * \param[in] k the number of neighbors to search for
109  * \param[out] k_indices the resultant indices of the neighboring points (must be
110  * resized to \a k a priori!)
111  * \param[out] k_sqr_distances the resultant squared distances to the neighboring
112  * points (must be resized to \a k a priori!)
113  * \return number of neighbors found
114  */
115  inline uindex_t
117  uindex_t index,
118  uindex_t k,
119  Indices& k_indices,
120  std::vector<float>& k_sqr_distances)
121  {
122  return (nearestKSearch(cloud[index], k, k_indices, k_sqr_distances));
123  }
124 
125  /** \brief Search for k-nearest neighbors at given query point.
126  * \param[in] p_q the given query point
127  * \param[in] k the number of neighbors to search for
128  * \param[out] k_indices the resultant indices of the neighboring points (must be
129  * resized to k a priori!)
130  * \param[out] k_sqr_distances the resultant squared distances to the neighboring
131  * points (must be resized to k a priori!)
132  * \return number of neighbors found
133  */
134  uindex_t
135  nearestKSearch(const PointT& p_q,
136  uindex_t k,
137  Indices& k_indices,
138  std::vector<float>& k_sqr_distances);
139 
140  /** \brief Search for k-nearest neighbors at query point
141  * \param[in] index index representing the query point in the dataset given by \a
142  * setInputCloud. If indices were given in setInputCloud, index will be the position
143  * in the indices vector.
144  * \param[in] k the number of neighbors to search for
145  * \param[out] k_indices the resultant indices of the neighboring points (must be
146  * resized to \a k a priori!)
147  * \param[out] k_sqr_distances the resultant squared distances to the neighboring
148  * points (must be resized to \a k a priori!)
149  * \return number of neighbors found
150  */
151  uindex_t
152  nearestKSearch(uindex_t index,
153  uindex_t k,
154  Indices& k_indices,
155  std::vector<float>& k_sqr_distances);
156 
157  /** \brief Search for approx. nearest neighbor at the query point.
158  * \param[in] cloud the point cloud data
159  * \param[in] query_index the index in \a cloud representing the query point
160  * \param[out] result_index the resultant index of the neighbor point
161  * \param[out] sqr_distance the resultant squared distance to the neighboring point
162  * \return number of neighbors found
163  */
164  inline void
166  uindex_t query_index,
167  index_t& result_index,
168  float& sqr_distance)
169  {
170  return (approxNearestSearch(cloud[query_index], result_index, sqr_distance));
171  }
172 
173  /** \brief Search for approx. nearest neighbor at the query point.
174  * \param[in] p_q the given query point
175  * \param[out] result_index the resultant index of the neighbor point
176  * \param[out] sqr_distance the resultant squared distance to the neighboring point
177  */
178  void
179  approxNearestSearch(const PointT& p_q, index_t& result_index, float& sqr_distance);
180 
181  /** \brief Search for approx. nearest neighbor at the query point.
182  * \param[in] query_index index representing the query point in the dataset given by
183  * \a setInputCloud. If indices were given in setInputCloud, index will be the
184  * position in the indices vector.
185  * \param[out] result_index the resultant index of the neighbor point
186  * \param[out] sqr_distance the resultant squared distance to the neighboring point
187  * \return number of neighbors found
188  */
189  void
190  approxNearestSearch(uindex_t query_index, index_t& result_index, float& sqr_distance);
191 
192  /** \brief Search for all neighbors of query point that are within a given radius.
193  * \param[in] cloud the point cloud data
194  * \param[in] index the index in \a cloud representing the query point
195  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
196  * \param[out] k_indices the resultant indices of the neighboring points
197  * \param[out] k_sqr_distances the resultant squared distances to the neighboring
198  * 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  uindex_t
203  radiusSearch(const PointCloud& cloud,
204  uindex_t index,
205  double radius,
206  Indices& k_indices,
207  std::vector<float>& k_sqr_distances,
208  index_t max_nn = 0)
209  {
210  return (radiusSearch(cloud[index], radius, k_indices, k_sqr_distances, max_nn));
211  }
212 
213  /** \brief Search for all neighbors of query point that are within a given radius.
214  * \param[in] p_q the given query point
215  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
216  * \param[out] k_indices the resultant indices of the neighboring points
217  * \param[out] k_sqr_distances the resultant squared distances to the neighboring
218  * points
219  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
220  * \return number of neighbors found in radius
221  */
222  uindex_t
223  radiusSearch(const PointT& p_q,
224  const double radius,
225  Indices& k_indices,
226  std::vector<float>& k_sqr_distances,
227  uindex_t max_nn = 0) const;
228 
229  /** \brief Search for all neighbors of query point that are within a given radius.
230  * \param[in] index index representing the query point in the dataset given by \a
231  * setInputCloud. If indices were given in setInputCloud, index will be the position
232  * in the indices vector
233  * \param[in] radius radius of the sphere bounding all of p_q's neighbors
234  * \param[out] k_indices the resultant indices of the neighboring points
235  * \param[out] k_sqr_distances the resultant squared distances to the neighboring
236  * points
237  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
238  * \return number of neighbors found in radius
239  */
240  uindex_t
241  radiusSearch(uindex_t index,
242  const double radius,
243  Indices& k_indices,
244  std::vector<float>& k_sqr_distances,
245  uindex_t max_nn = 0) const;
246 
247  /** \brief Get a PointT vector of centers of all voxels that intersected by a ray
248  * (origin, direction).
249  * \param[in] origin ray origin
250  * \param[in] direction ray direction vector
251  * \param[out] voxel_center_list results are written to this vector of PointT elements
252  * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
253  * disable)
254  * \return number of intersected voxels
255  */
256  uindex_t
257  getIntersectedVoxelCenters(Eigen::Vector3f origin,
258  Eigen::Vector3f direction,
259  AlignedPointTVector& voxel_center_list,
260  uindex_t max_voxel_count = 0) const;
261 
262  /** \brief Get indices of all voxels that are intersected by a ray (origin,
263  * direction).
264  * \param[in] origin ray origin \param[in] direction ray direction vector
265  * \param[out] k_indices resulting point indices from intersected voxels
266  * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
267  * disable)
268  * \return number of intersected voxels
269  */
270  uindex_t
271  getIntersectedVoxelIndices(Eigen::Vector3f origin,
272  Eigen::Vector3f direction,
273  Indices& k_indices,
274  uindex_t max_voxel_count = 0) const;
275 
276  /** \brief Search for points within rectangular search area
277  * Points exactly on the edges of the search rectangle are included.
278  * \param[in] min_pt lower corner of search area
279  * \param[in] max_pt upper corner of search area
280  * \param[out] k_indices the resultant point indices
281  * \return number of points found within search area
282  */
283  uindex_t
284  boxSearch(const Eigen::Vector3f& min_pt,
285  const Eigen::Vector3f& max_pt,
286  Indices& k_indices) const;
287 
288 protected:
289  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
290  // Octree-based search routines & helpers
291  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
292  /** \brief @b Priority queue entry for branch nodes
293  * \note This class defines priority queue entries for the nearest neighbor search.
294  * \author Julius Kammerl (julius@kammerl.de)
295  */
297  public:
298  /** \brief Empty constructor */
300 
301  /** \brief Constructor for initializing priority queue entry.
302  * \param _node pointer to octree node
303  * \param _key octree key addressing voxel in octree structure
304  * \param[in] _point_distance distance of query point to voxel center
305  */
306  prioBranchQueueEntry(OctreeNode* _node, OctreeKey& _key, float _point_distance)
307  : node(_node), point_distance(_point_distance), key(_key)
308  {}
309 
310  /** \brief Operator< for comparing priority queue entries with each other.
311  * \param[in] rhs the priority queue to compare this against
312  */
313  bool
314  operator<(const prioBranchQueueEntry rhs) const
315  {
316  return (this->point_distance > rhs.point_distance);
317  }
318 
319  /** \brief Pointer to octree node. */
320  const OctreeNode* node;
321 
322  /** \brief Distance to query point. */
324 
325  /** \brief Octree key. */
327  };
328 
329  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
330  /** \brief @b Priority queue entry for point candidates
331  * \note This class defines priority queue entries for the nearest neighbor point
332  * candidates.
333  * \author Julius Kammerl (julius@kammerl.de)
334  */
336  public:
337  /** \brief Empty constructor */
339 
340  /** \brief Constructor for initializing priority queue entry.
341  * \param[in] point_idx index for a dataset point given by \a setInputCloud
342  * \param[in] point_distance distance of query point to voxel center
343  */
344  prioPointQueueEntry(uindex_t point_idx, float point_distance)
345  : point_idx_(point_idx), point_distance_(point_distance)
346  {}
347 
348  /** \brief Operator< for comparing priority queue entries with each other.
349  * \param[in] rhs priority queue to compare this against
350  */
351  bool
352  operator<(const prioPointQueueEntry& rhs) const
353  {
354  return (this->point_distance_ < rhs.point_distance_);
355  }
356 
357  /** \brief Index representing a point in the dataset given by \a setInputCloud. */
359 
360  /** \brief Distance to query point. */
362  };
363 
364  /** \brief Helper function to calculate the squared distance between two points
365  * \param[in] point_a point A
366  * \param[in] point_b point B
367  * \return squared distance between point A and point B
368  */
369  float
370  pointSquaredDist(const PointT& point_a, const PointT& point_b) const;
371 
372  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
373  // Recursive search routine methods
374  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
375 
376  /** \brief Recursive search method that explores the octree and finds neighbors within
377  * a given radius
378  * \param[in] point query point \param[in] radiusSquared squared search radius
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] k_indices vector of indices found to be neighbors of query point
383  * \param[out] k_sqr_distances squared distances of neighbors to query point
384  * \param[in] max_nn maximum of neighbors to be found
385  */
386  void
388  const double radiusSquared,
389  const BranchNode* node,
390  const OctreeKey& key,
391  uindex_t tree_depth,
392  Indices& k_indices,
393  std::vector<float>& k_sqr_distances,
394  uindex_t max_nn) const;
395 
396  /** \brief Recursive search method that explores the octree and finds the K nearest
397  * neighbors
398  * \param[in] point query point
399  * \param[in] K amount of nearest neighbors to be found
400  * \param[in] node current octree node to be explored
401  * \param[in] key octree key addressing a leaf node.
402  * \param[in] tree_depth current depth/level in the octree
403  * \param[in] squared_search_radius squared search radius distance
404  * \param[out] point_candidates priority queue of nearest neigbor point candidates
405  * \return squared search radius based on current point candidate set found
406  */
407  double
409  const PointT& point,
410  uindex_t K,
411  const BranchNode* node,
412  const OctreeKey& key,
413  uindex_t tree_depth,
414  const double squared_search_radius,
415  std::vector<prioPointQueueEntry>& point_candidates) const;
416 
417  /** \brief Recursive search method that explores the octree and finds the approximate
418  * nearest neighbor
419  * \param[in] point query point
420  * \param[in] node current octree node to be explored
421  * \param[in] key octree key addressing a leaf node.
422  * \param[in] tree_depth current depth/level in the octree
423  * \param[out] result_index result index is written to this reference
424  * \param[out] sqr_distance squared distance to search
425  */
426  void
428  const BranchNode* node,
429  const OctreeKey& key,
430  uindex_t tree_depth,
431  index_t& result_index,
432  float& sqr_distance);
433 
434  /** \brief Recursively search the tree for all intersected leaf nodes and return a
435  * vector of voxel centers. This algorithm is based off the paper An Efficient
436  * Parametric Algorithm for Octree Traversal:
437  * http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf
438  * \param[in] min_x octree nodes X coordinate of lower bounding box corner
439  * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
440  * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
441  * \param[in] max_x octree nodes X coordinate of upper bounding box corner
442  * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
443  * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
444  * \param[in] a
445  * \param[in] node current octree node to be explored
446  * \param[in] key octree key addressing a leaf node.
447  * \param[out] voxel_center_list results are written to this vector of PointT elements
448  * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
449  * disable)
450  * \return number of voxels found
451  */
452  uindex_t
454  double min_y,
455  double min_z,
456  double max_x,
457  double max_y,
458  double max_z,
459  unsigned char a,
460  const OctreeNode* node,
461  const OctreeKey& key,
462  AlignedPointTVector& voxel_center_list,
463  uindex_t max_voxel_count) const;
464 
465  /** \brief Recursive search method that explores the octree and finds points within a
466  * rectangular search area
467  * \param[in] min_pt lower corner of search area
468  * \param[in] max_pt upper corner of search area
469  * \param[in] node current octree node to be explored
470  * \param[in] key octree key addressing a leaf node.
471  * \param[in] tree_depth current depth/level in the octree
472  * \param[out] k_indices the resultant point indices
473  */
474  void
475  boxSearchRecursive(const Eigen::Vector3f& min_pt,
476  const Eigen::Vector3f& max_pt,
477  const BranchNode* node,
478  const OctreeKey& key,
479  uindex_t tree_depth,
480  Indices& k_indices) const;
481 
482  /** \brief Recursively search the tree for all intersected leaf nodes and return a
483  * vector of indices. This algorithm is based off the paper An Efficient Parametric
484  * Algorithm for Octree Traversal: http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf
485  * \param[in] min_x octree nodes X coordinate of lower bounding box corner
486  * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
487  * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
488  * \param[in] max_x octree nodes X coordinate of upper bounding box corner
489  * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
490  * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
491  * \param[in] a
492  * \param[in] node current octree node to be explored
493  * \param[in] key octree key addressing a leaf node.
494  * \param[out] k_indices resulting indices
495  * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
496  * disable)
497  * \return number of voxels found
498  */
499  uindex_t
501  double min_y,
502  double min_z,
503  double max_x,
504  double max_y,
505  double max_z,
506  unsigned char a,
507  const OctreeNode* node,
508  const OctreeKey& key,
509  Indices& k_indices,
510  uindex_t max_voxel_count) const;
511 
512  /** \brief Initialize raytracing algorithm
513  * \param origin
514  * \param direction
515  * \param[in] min_x octree nodes X coordinate of lower bounding box corner
516  * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
517  * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
518  * \param[in] max_x octree nodes X coordinate of upper bounding box corner
519  * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
520  * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
521  * \param a
522  */
523  inline void
524  initIntersectedVoxel(Eigen::Vector3f& origin,
525  Eigen::Vector3f& direction,
526  double& min_x,
527  double& min_y,
528  double& min_z,
529  double& max_x,
530  double& max_y,
531  double& max_z,
532  unsigned char& a) const
533  {
534  // Account for division by zero when direction vector is 0.0
535  const float epsilon = 1e-10f;
536  if (direction.x() == 0.0)
537  direction.x() = epsilon;
538  if (direction.y() == 0.0)
539  direction.y() = epsilon;
540  if (direction.z() == 0.0)
541  direction.z() = epsilon;
542 
543  // Voxel childIdx remapping
544  a = 0;
545 
546  // Handle negative axis direction vector
547  if (direction.x() < 0.0) {
548  origin.x() = static_cast<float>(this->min_x_) + static_cast<float>(this->max_x_) -
549  origin.x();
550  direction.x() = -direction.x();
551  a |= 4;
552  }
553  if (direction.y() < 0.0) {
554  origin.y() = static_cast<float>(this->min_y_) + static_cast<float>(this->max_y_) -
555  origin.y();
556  direction.y() = -direction.y();
557  a |= 2;
558  }
559  if (direction.z() < 0.0) {
560  origin.z() = static_cast<float>(this->min_z_) + static_cast<float>(this->max_z_) -
561  origin.z();
562  direction.z() = -direction.z();
563  a |= 1;
564  }
565  min_x = (this->min_x_ - origin.x()) / direction.x();
566  max_x = (this->max_x_ - origin.x()) / direction.x();
567  min_y = (this->min_y_ - origin.y()) / direction.y();
568  max_y = (this->max_y_ - origin.y()) / direction.y();
569  min_z = (this->min_z_ - origin.z()) / direction.z();
570  max_z = (this->max_z_ - origin.z()) / direction.z();
571  }
572 
573  /** \brief Find first child node ray will enter
574  * \param[in] min_x octree nodes X coordinate of lower bounding box corner
575  * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
576  * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
577  * \param[in] mid_x octree nodes X coordinate of bounding box mid line
578  * \param[in] mid_y octree nodes Y coordinate of bounding box mid line
579  * \param[in] mid_z octree nodes Z coordinate of bounding box mid line
580  * \return the first child node ray will enter
581  */
582  inline int
584  double min_y,
585  double min_z,
586  double mid_x,
587  double mid_y,
588  double mid_z) const
589  {
590  int currNode = 0;
591 
592  if (min_x > min_y) {
593  if (min_x > min_z) {
594  // max(min_x, min_y, min_z) is min_x. Entry plane is YZ.
595  if (mid_y < min_x)
596  currNode |= 2;
597  if (mid_z < min_x)
598  currNode |= 1;
599  }
600  else {
601  // max(min_x, min_y, min_z) is min_z. Entry plane is XY.
602  if (mid_x < min_z)
603  currNode |= 4;
604  if (mid_y < min_z)
605  currNode |= 2;
606  }
607  }
608  else {
609  if (min_y > min_z) {
610  // max(min_x, min_y, min_z) is min_y. Entry plane is XZ.
611  if (mid_x < min_y)
612  currNode |= 4;
613  if (mid_z < min_y)
614  currNode |= 1;
615  }
616  else {
617  // max(min_x, min_y, min_z) is min_z. Entry plane is XY.
618  if (mid_x < min_z)
619  currNode |= 4;
620  if (mid_y < min_z)
621  currNode |= 2;
622  }
623  }
624 
625  return currNode;
626  }
627 
628  /** \brief Get the next visited node given the current node upper
629  * bounding box corner. This function accepts three float values, and
630  * three int values. The function returns the ith integer where the
631  * ith float value is the minimum of the three float values.
632  * \param[in] x current nodes X coordinate of upper bounding box corner
633  * \param[in] y current nodes Y coordinate of upper bounding box corner
634  * \param[in] z current nodes Z coordinate of upper bounding box corner
635  * \param[in] a next node if exit Plane YZ
636  * \param[in] b next node if exit Plane XZ
637  * \param[in] c next node if exit Plane XY
638  * \return the next child node ray will enter or 8 if exiting
639  */
640  inline int
641  getNextIntersectedNode(double x, double y, double z, int a, int b, int c) const
642  {
643  if (x < y) {
644  if (x < z)
645  return a;
646  return c;
647  }
648  if (y < z)
649  return b;
650  return c;
651  }
652 };
653 } // namespace octree
654 } // namespace pcl
655 
656 #ifdef PCL_NO_PRECOMPILE
657 #include <pcl/octree/impl/octree_search.hpp>
658 #endif
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
Octree key class
Definition: octree_key.h:54
Abstract octree node class
Definition: octree_nodes.h:58
Octree pointcloud class
typename OctreeT::LeafNode LeafNode
typename OctreeT::BranchNode BranchNode
Priority queue entry for branch nodes
bool operator<(const prioBranchQueueEntry rhs) const
Operator< for comparing priority queue entries with each other.
OctreeKey key
Octree key.
const OctreeNode * node
Pointer to octree node.
prioBranchQueueEntry(OctreeNode *_node, OctreeKey &_key, float _point_distance)
Constructor for initializing priority queue entry.
prioBranchQueueEntry()
Empty constructor
float point_distance
Distance to query point.
Priority queue entry for point candidates
prioPointQueueEntry(uindex_t point_idx, float point_distance)
Constructor for initializing priority queue entry.
prioPointQueueEntry()
Empty constructor
uindex_t point_idx_
Index representing a point in the dataset given by setInputCloud.
bool operator<(const prioPointQueueEntry &rhs) const
Operator< for comparing priority queue entries with each other.
float point_distance_
Distance to query point.
Octree pointcloud search class
Definition: octree_search.h:58
typename OctreeT::LeafNode LeafNode
Definition: octree_search.h:78
double getKNearestNeighborRecursive(const PointT &point, uindex_t K, const BranchNode *node, const OctreeKey &key, uindex_t 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.
uindex_t getIntersectedVoxelIndices(Eigen::Vector3f origin, Eigen::Vector3f direction, Indices &k_indices, uindex_t max_voxel_count=0) const
Get indices of all voxels that are intersected by a ray (origin, direction).
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.
uindex_t 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, Indices &k_indices, uindex_t max_voxel_count) const
Recursively search the tree for all intersected leaf nodes and return a vector of indices.
uindex_t getIntersectedVoxelCenters(Eigen::Vector3f origin, Eigen::Vector3f direction, AlignedPointTVector &voxel_center_list, uindex_t max_voxel_count=0) const
Get a PointT vector of centers of all voxels that intersected by a ray (origin, direction).
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: octree_search.h:66
bool voxelSearch(const PointT &point, Indices &point_idx_data)
Search for neighbors within a voxel at given point.
shared_ptr< const OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT > > ConstPtr
Definition: octree_search.h:72
shared_ptr< Indices > IndicesPtr
Definition: octree_search.h:61
shared_ptr< OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT > > Ptr
Definition: octree_search.h:70
uindex_t boxSearch(const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, Indices &k_indices) const
Search for points within rectangular search area Points exactly on the edges of the search rectangle ...
uindex_t 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, uindex_t max_voxel_count) const
Recursively search the tree for all intersected leaf nodes and return a vector of voxel centers.
void approxNearestSearchRecursive(const PointT &point, const BranchNode *node, const OctreeKey &key, uindex_t tree_depth, index_t &result_index, float &sqr_distance)
Recursive search method that explores the octree and finds the approximate nearest neighbor.
uindex_t nearestKSearch(const PointCloud &cloud, uindex_t index, uindex_t k, Indices &k_indices, std::vector< float > &k_sqr_distances)
Search for k-nearest neighbors at the query point.
void boxSearchRecursive(const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, const BranchNode *node, const OctreeKey &key, uindex_t tree_depth, Indices &k_indices) const
Recursive search method that explores the octree and finds points within a rectangular search area.
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.
float pointSquaredDist(const PointT &point_a, const PointT &point_b) const
Helper function to calculate the squared distance between two points.
void getNeighborsWithinRadiusRecursive(const PointT &point, const double radiusSquared, const BranchNode *node, const OctreeKey &key, uindex_t tree_depth, Indices &k_indices, std::vector< float > &k_sqr_distances, uindex_t max_nn) const
Recursive search method that explores the octree and finds neighbors within a given radius.
shared_ptr< const Indices > IndicesConstPtr
Definition: octree_search.h:62
typename OctreeT::BranchNode BranchNode
Definition: octree_search.h:79
void approxNearestSearch(const PointCloud &cloud, uindex_t query_index, index_t &result_index, float &sqr_distance)
Search for approx.
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.
uindex_t radiusSearch(const PointCloud &cloud, uindex_t index, double radius, Indices &k_indices, std::vector< float > &k_sqr_distances, index_t max_nn=0)
Search for all neighbors of query point that are within a given radius.
OctreePointCloudSearch(const double resolution)
Constructor.
Definition: octree_search.h:84
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
Definition: octree_search.h:75
typename PointCloud::Ptr PointCloudPtr
Definition: octree_search.h:65
@ K
Definition: norms.h:54
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
Definition: types.h:120
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition: types.h:112
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
A point structure representing Euclidean xyz coordinates, and the RGB color.