Point Cloud Library (PCL)  1.13.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/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
265  * \param[in] direction ray direction vector
266  * \param[out] k_indices resulting point indices from intersected voxels
267  * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
268  * disable)
269  * \return number of intersected voxels
270  */
271  uindex_t
272  getIntersectedVoxelIndices(Eigen::Vector3f origin,
273  Eigen::Vector3f direction,
274  Indices& k_indices,
275  uindex_t max_voxel_count = 0) const;
276 
277  /** \brief Search for points within rectangular search area
278  * Points exactly on the edges of the search rectangle are included.
279  * \param[in] min_pt lower corner of search area
280  * \param[in] max_pt upper corner of search area
281  * \param[out] k_indices the resultant point indices
282  * \return number of points found within search area
283  */
284  uindex_t
285  boxSearch(const Eigen::Vector3f& min_pt,
286  const Eigen::Vector3f& max_pt,
287  Indices& k_indices) const;
288 
289 protected:
290  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
291  // Octree-based search routines & helpers
292  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
293  /** \brief @b Priority queue entry for branch nodes
294  * \note This class defines priority queue entries for the nearest neighbor search.
295  * \author Julius Kammerl (julius@kammerl.de)
296  */
298  public:
299  /** \brief Empty constructor */
301 
302  /** \brief Constructor for initializing priority queue entry.
303  * \param _node pointer to octree node
304  * \param _key octree key addressing voxel in octree structure
305  * \param[in] _point_distance distance of query point to voxel center
306  */
307  prioBranchQueueEntry(OctreeNode* _node, OctreeKey& _key, float _point_distance)
308  : node(_node), point_distance(_point_distance), key(_key)
309  {}
310 
311  /** \brief Operator< for comparing priority queue entries with each other.
312  * \param[in] rhs the priority queue to compare this against
313  */
314  bool
315  operator<(const prioBranchQueueEntry rhs) const
316  {
317  return (this->point_distance > rhs.point_distance);
318  }
319 
320  /** \brief Pointer to octree node. */
321  const OctreeNode* node;
322 
323  /** \brief Distance to query point. */
325 
326  /** \brief Octree key. */
328  };
329 
330  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
331  /** \brief @b Priority queue entry for point candidates
332  * \note This class defines priority queue entries for the nearest neighbor point
333  * candidates.
334  * \author Julius Kammerl (julius@kammerl.de)
335  */
337  public:
338  /** \brief Empty constructor */
340 
341  /** \brief Constructor for initializing priority queue entry.
342  * \param[in] point_idx index for a dataset point given by \a setInputCloud
343  * \param[in] point_distance distance of query point to voxel center
344  */
345  prioPointQueueEntry(uindex_t point_idx, float point_distance)
346  : point_idx_(point_idx), point_distance_(point_distance)
347  {}
348 
349  /** \brief Operator< for comparing priority queue entries with each other.
350  * \param[in] rhs priority queue to compare this against
351  */
352  bool
353  operator<(const prioPointQueueEntry& rhs) const
354  {
355  return (this->point_distance_ < rhs.point_distance_);
356  }
357 
358  /** \brief Index representing a point in the dataset given by \a setInputCloud. */
360 
361  /** \brief Distance to query point. */
363  };
364 
365  /** \brief Helper function to calculate the squared distance between two points
366  * \param[in] point_a point A
367  * \param[in] point_b point B
368  * \return squared distance between point A and point B
369  */
370  float
371  pointSquaredDist(const PointT& point_a, const PointT& point_b) const;
372 
373  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
374  // Recursive search routine methods
375  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
376 
377  /** \brief Recursive search method that explores the octree and finds neighbors within
378  * a given radius
379  * \param[in] point query point
380  * \param[in] radiusSquared squared search radius
381  * \param[in] node current octree node to be explored
382  * \param[in] key octree key addressing a leaf node.
383  * \param[in] tree_depth current depth/level in the octree
384  * \param[out] k_indices vector of indices found to be neighbors of query point
385  * \param[out] k_sqr_distances squared distances of neighbors to query point
386  * \param[in] max_nn maximum of neighbors to be found
387  */
388  void
390  const double radiusSquared,
391  const BranchNode* node,
392  const OctreeKey& key,
393  uindex_t tree_depth,
394  Indices& k_indices,
395  std::vector<float>& k_sqr_distances,
396  uindex_t max_nn) const;
397 
398  /** \brief Recursive search method that explores the octree and finds the K nearest
399  * neighbors
400  * \param[in] point query point
401  * \param[in] K amount of nearest neighbors to be found
402  * \param[in] node current octree node to be explored
403  * \param[in] key octree key addressing a leaf node.
404  * \param[in] tree_depth current depth/level in the octree
405  * \param[in] squared_search_radius squared search radius distance
406  * \param[out] point_candidates priority queue of nearest neighbor point candidates
407  * \return squared search radius based on current point candidate set found
408  */
409  double
411  const PointT& point,
412  uindex_t K,
413  const BranchNode* node,
414  const OctreeKey& key,
415  uindex_t tree_depth,
416  const double squared_search_radius,
417  std::vector<prioPointQueueEntry>& point_candidates) const;
418 
419  /** \brief Recursive search method that explores the octree and finds the approximate
420  * nearest neighbor
421  * \param[in] point query point
422  * \param[in] node current octree node to be explored
423  * \param[in] key octree key addressing a leaf node.
424  * \param[in] tree_depth current depth/level in the octree
425  * \param[out] result_index result index is written to this reference
426  * \param[out] sqr_distance squared distance to search
427  */
428  void
430  const BranchNode* node,
431  const OctreeKey& key,
432  uindex_t tree_depth,
433  index_t& result_index,
434  float& sqr_distance);
435 
436  /** \brief Recursively search the tree for all intersected leaf nodes and return a
437  * vector of voxel centers. This algorithm is based off the paper An Efficient
438  * Parametric Algorithm for Octree Traversal:
439  * http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf
440  * \param[in] min_x octree nodes X coordinate of lower bounding box corner
441  * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
442  * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
443  * \param[in] max_x octree nodes X coordinate of upper bounding box corner
444  * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
445  * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
446  * \param[in] a number used for voxel child index remapping
447  * \param[in] node current octree node to be explored
448  * \param[in] key octree key addressing a leaf node.
449  * \param[out] voxel_center_list results are written to this vector of PointT elements
450  * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
451  * disable)
452  * \return number of voxels found
453  */
454  uindex_t
456  double min_y,
457  double min_z,
458  double max_x,
459  double max_y,
460  double max_z,
461  unsigned char a,
462  const OctreeNode* node,
463  const OctreeKey& key,
464  AlignedPointTVector& voxel_center_list,
465  uindex_t max_voxel_count) const;
466 
467  /** \brief Recursive search method that explores the octree and finds points within a
468  * rectangular search area
469  * \param[in] min_pt lower corner of search area
470  * \param[in] max_pt upper corner of search area
471  * \param[in] node current octree node to be explored
472  * \param[in] key octree key addressing a leaf node.
473  * \param[in] tree_depth current depth/level in the octree
474  * \param[out] k_indices the resultant point indices
475  */
476  void
477  boxSearchRecursive(const Eigen::Vector3f& min_pt,
478  const Eigen::Vector3f& max_pt,
479  const BranchNode* node,
480  const OctreeKey& key,
481  uindex_t tree_depth,
482  Indices& k_indices) const;
483 
484  /** \brief Recursively search the tree for all intersected leaf nodes and return a
485  * vector of indices. This algorithm is based off the paper An Efficient Parametric
486  * Algorithm for Octree Traversal: http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf
487  * \param[in] min_x octree nodes X coordinate of lower bounding box corner
488  * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
489  * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
490  * \param[in] max_x octree nodes X coordinate of upper bounding box corner
491  * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
492  * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
493  * \param[in] a number used for voxel child index remapping
494  * \param[in] node current octree node to be explored
495  * \param[in] key octree key addressing a leaf node.
496  * \param[out] k_indices resulting indices
497  * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
498  * disable)
499  * \return number of voxels found
500  */
501  uindex_t
503  double min_y,
504  double min_z,
505  double max_x,
506  double max_y,
507  double max_z,
508  unsigned char a,
509  const OctreeNode* node,
510  const OctreeKey& key,
511  Indices& k_indices,
512  uindex_t max_voxel_count) const;
513 
514  /** \brief Initialize raytracing algorithm
515  * \param[in] origin ray origin
516  * \param[in] direction ray direction vector
517  * \param[out] min_x octree nodes X coordinate of lower bounding box corner
518  * \param[out] min_y octree nodes Y coordinate of lower bounding box corner
519  * \param[out] min_z octree nodes Z coordinate of lower bounding box corner
520  * \param[out] max_x octree nodes X coordinate of upper bounding box corner
521  * \param[out] max_y octree nodes Y coordinate of upper bounding box corner
522  * \param[out] max_z octree nodes Z coordinate of upper bounding box corner
523  * \param[out] a number used for voxel child index remapping
524  */
525  inline void
526  initIntersectedVoxel(Eigen::Vector3f& origin,
527  Eigen::Vector3f& direction,
528  double& min_x,
529  double& min_y,
530  double& min_z,
531  double& max_x,
532  double& max_y,
533  double& max_z,
534  unsigned char& a) const
535  {
536  // Account for division by zero when direction vector is 0.0
537  constexpr float epsilon = 1e-10f;
538  if (direction.x() == 0.0)
539  direction.x() = epsilon;
540  if (direction.y() == 0.0)
541  direction.y() = epsilon;
542  if (direction.z() == 0.0)
543  direction.z() = epsilon;
544 
545  // Voxel childIdx remapping
546  a = 0;
547 
548  // Handle negative axis direction vector
549  if (direction.x() < 0.0) {
550  origin.x() = static_cast<float>(this->min_x_) + static_cast<float>(this->max_x_) -
551  origin.x();
552  direction.x() = -direction.x();
553  a |= 4;
554  }
555  if (direction.y() < 0.0) {
556  origin.y() = static_cast<float>(this->min_y_) + static_cast<float>(this->max_y_) -
557  origin.y();
558  direction.y() = -direction.y();
559  a |= 2;
560  }
561  if (direction.z() < 0.0) {
562  origin.z() = static_cast<float>(this->min_z_) + static_cast<float>(this->max_z_) -
563  origin.z();
564  direction.z() = -direction.z();
565  a |= 1;
566  }
567  min_x = (this->min_x_ - origin.x()) / direction.x();
568  max_x = (this->max_x_ - origin.x()) / direction.x();
569  min_y = (this->min_y_ - origin.y()) / direction.y();
570  max_y = (this->max_y_ - origin.y()) / direction.y();
571  min_z = (this->min_z_ - origin.z()) / direction.z();
572  max_z = (this->max_z_ - origin.z()) / direction.z();
573  }
574 
575  /** \brief Find first child node ray will enter
576  * \param[in] min_x octree nodes X coordinate of lower bounding box corner
577  * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
578  * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
579  * \param[in] mid_x octree nodes X coordinate of bounding box mid line
580  * \param[in] mid_y octree nodes Y coordinate of bounding box mid line
581  * \param[in] mid_z octree nodes Z coordinate of bounding box mid line
582  * \return the first child node ray will enter
583  */
584  inline int
586  double min_y,
587  double min_z,
588  double mid_x,
589  double mid_y,
590  double mid_z) const
591  {
592  int currNode = 0;
593 
594  if (min_x > min_y) {
595  if (min_x > min_z) {
596  // max(min_x, min_y, min_z) is min_x. Entry plane is YZ.
597  if (mid_y < min_x)
598  currNode |= 2;
599  if (mid_z < min_x)
600  currNode |= 1;
601  }
602  else {
603  // max(min_x, min_y, min_z) is min_z. Entry plane is XY.
604  if (mid_x < min_z)
605  currNode |= 4;
606  if (mid_y < min_z)
607  currNode |= 2;
608  }
609  }
610  else {
611  if (min_y > min_z) {
612  // max(min_x, min_y, min_z) is min_y. Entry plane is XZ.
613  if (mid_x < min_y)
614  currNode |= 4;
615  if (mid_z < min_y)
616  currNode |= 1;
617  }
618  else {
619  // max(min_x, min_y, min_z) is min_z. Entry plane is XY.
620  if (mid_x < min_z)
621  currNode |= 4;
622  if (mid_y < min_z)
623  currNode |= 2;
624  }
625  }
626 
627  return currNode;
628  }
629 
630  /** \brief Get the next visited node given the current node upper
631  * bounding box corner. This function accepts three float values, and
632  * three int values. The function returns the ith integer where the
633  * ith float value is the minimum of the three float values.
634  * \param[in] x current nodes X coordinate of upper bounding box corner
635  * \param[in] y current nodes Y coordinate of upper bounding box corner
636  * \param[in] z current nodes Z coordinate of upper bounding box corner
637  * \param[in] a next node if exit Plane YZ
638  * \param[in] b next node if exit Plane XZ
639  * \param[in] c next node if exit Plane XY
640  * \return the next child node ray will enter or 8 if exiting
641  */
642  inline int
643  getNextIntersectedNode(double x, double y, double z, int a, int b, int c) const
644  {
645  if (x < y) {
646  if (x < z)
647  return a;
648  return c;
649  }
650  if (y < z)
651  return b;
652  return c;
653  }
654 };
655 } // namespace octree
656 } // namespace pcl
657 
658 #ifdef PCL_NO_PRECOMPILE
659 #include <pcl/octree/impl/octree_search.hpp>
660 #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:59
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.