Point Cloud Library (PCL)  1.10.1
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 namespace octree {
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
63 
65  using PointCloudPtr = typename PointCloud::Ptr;
67 
68  // Boost shared pointers
69  using Ptr =
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, std::vector<int>& 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(const int index, std::vector<int>& 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 int
117  int index,
118  int k,
119  std::vector<int>& 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  int
135  nearestKSearch(const PointT& p_q,
136  int k,
137  std::vector<int>& 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  int
152  nearestKSearch(int index,
153  int k,
154  std::vector<int>& 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  int query_index,
167  int& result_index,
168  float& sqr_distance)
169  {
170  return (approxNearestSearch(cloud.points[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, int& 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(int query_index, int& 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  int
203  radiusSearch(const PointCloud& cloud,
204  int index,
205  double radius,
206  std::vector<int>& k_indices,
207  std::vector<float>& k_sqr_distances,
208  unsigned int max_nn = 0)
209  {
210  return (
211  radiusSearch(cloud.points[index], radius, k_indices, k_sqr_distances, max_nn));
212  }
213 
214  /** \brief Search for all neighbors of query point that are within a given radius.
215  * \param[in] p_q the given query point
216  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
217  * \param[out] k_indices the resultant indices of the neighboring points
218  * \param[out] k_sqr_distances the resultant squared distances to the neighboring
219  * points
220  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
221  * \return number of neighbors found in radius
222  */
223  int
224  radiusSearch(const PointT& p_q,
225  const double radius,
226  std::vector<int>& k_indices,
227  std::vector<float>& k_sqr_distances,
228  unsigned int max_nn = 0) const;
229 
230  /** \brief Search for all neighbors of query point that are within a given radius.
231  * \param[in] index index representing the query point in the dataset given by \a
232  * setInputCloud. If indices were given in setInputCloud, index will be the position
233  * in the indices vector
234  * \param[in] radius radius of the sphere bounding all of p_q's neighbors
235  * \param[out] k_indices the resultant indices of the neighboring points
236  * \param[out] k_sqr_distances the resultant squared distances to the neighboring
237  * points
238  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
239  * \return number of neighbors found in radius
240  */
241  int
242  radiusSearch(int index,
243  const double radius,
244  std::vector<int>& k_indices,
245  std::vector<float>& k_sqr_distances,
246  unsigned int max_nn = 0) const;
247 
248  /** \brief Get a PointT vector of centers of all voxels that intersected by a ray
249  * (origin, direction).
250  * \param[in] origin ray origin
251  * \param[in] direction ray direction vector
252  * \param[out] voxel_center_list results are written to this vector of PointT elements
253  * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
254  * disable)
255  * \return number of intersected voxels
256  */
257  int
258  getIntersectedVoxelCenters(Eigen::Vector3f origin,
259  Eigen::Vector3f direction,
260  AlignedPointTVector& voxel_center_list,
261  int max_voxel_count = 0) const;
262 
263  /** \brief Get indices of all voxels that are intersected by a ray (origin,
264  * direction).
265  * \param[in] origin ray origin \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  int
272  getIntersectedVoxelIndices(Eigen::Vector3f origin,
273  Eigen::Vector3f direction,
274  std::vector<int>& k_indices,
275  int 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  int
285  boxSearch(const Eigen::Vector3f& min_pt,
286  const Eigen::Vector3f& max_pt,
287  std::vector<int>& 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
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 an index representing a point in the dataset given by \a
343  * setInputCloud \param[in] point_distance distance of query point to voxel center
344  */
345  prioPointQueueEntry(unsigned int& 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 \param[in] radiusSquared squared search radius
380  * \param[in] node current octree node to be explored
381  * \param[in] key octree key addressing a leaf node.
382  * \param[in] tree_depth current depth/level in the octree
383  * \param[out] k_indices vector of indices found to be neighbors of query point
384  * \param[out] k_sqr_distances squared distances of neighbors to query point
385  * \param[in] max_nn maximum of neighbors to be found
386  */
387  void
389  const double radiusSquared,
390  const BranchNode* node,
391  const OctreeKey& key,
392  unsigned int tree_depth,
393  std::vector<int>& k_indices,
394  std::vector<float>& k_sqr_distances,
395  unsigned int max_nn) const;
396 
397  /** \brief Recursive search method that explores the octree and finds the K nearest
398  * neighbors
399  * \param[in] point query point
400  * \param[in] K amount of nearest neighbors to be found
401  * \param[in] node current octree node to be explored
402  * \param[in] key octree key addressing a leaf node.
403  * \param[in] tree_depth current depth/level in the octree
404  * \param[in] squared_search_radius squared search radius distance
405  * \param[out] point_candidates priority queue of nearest neigbor point candidates
406  * \return squared search radius based on current point candidate set found
407  */
408  double
410  const PointT& point,
411  unsigned int K,
412  const BranchNode* node,
413  const OctreeKey& key,
414  unsigned int tree_depth,
415  const double squared_search_radius,
416  std::vector<prioPointQueueEntry>& point_candidates) const;
417 
418  /** \brief Recursive search method that explores the octree and finds the approximate
419  * nearest neighbor
420  * \param[in] point query point
421  * \param[in] node current octree node to be explored
422  * \param[in] key octree key addressing a leaf node.
423  * \param[in] tree_depth current depth/level in the octree
424  * \param[out] result_index result index is written to this reference
425  * \param[out] sqr_distance squared distance to search
426  */
427  void
429  const BranchNode* node,
430  const OctreeKey& key,
431  unsigned int tree_depth,
432  int& result_index,
433  float& sqr_distance);
434 
435  /** \brief Recursively search the tree for all intersected leaf nodes and return a
436  * vector of voxel centers. This algorithm is based off the paper An Efficient
437  * Parametric Algorithm for Octree Traversal:
438  * http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf
439  * \param[in] min_x octree nodes X coordinate of lower bounding box corner
440  * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
441  * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
442  * \param[in] max_x octree nodes X coordinate of upper bounding box corner
443  * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
444  * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
445  * \param[in] a
446  * \param[in] node current octree node to be explored
447  * \param[in] key octree key addressing a leaf node.
448  * \param[out] voxel_center_list results are written to this vector of PointT elements
449  * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
450  * disable)
451  * \return number of voxels found
452  */
453  int
455  double min_y,
456  double min_z,
457  double max_x,
458  double max_y,
459  double max_z,
460  unsigned char a,
461  const OctreeNode* node,
462  const OctreeKey& key,
463  AlignedPointTVector& voxel_center_list,
464  int max_voxel_count) const;
465 
466  /** \brief Recursive search method that explores the octree and finds points within a
467  * rectangular search area
468  * \param[in] min_pt lower corner of search area
469  * \param[in] max_pt upper corner of search area
470  * \param[in] node current octree node to be explored
471  * \param[in] key octree key addressing a leaf node.
472  * \param[in] tree_depth current depth/level in the octree
473  * \param[out] k_indices the resultant point indices
474  */
475  void
476  boxSearchRecursive(const Eigen::Vector3f& min_pt,
477  const Eigen::Vector3f& max_pt,
478  const BranchNode* node,
479  const OctreeKey& key,
480  unsigned int tree_depth,
481  std::vector<int>& k_indices) const;
482 
483  /** \brief Recursively search the tree for all intersected leaf nodes and return a
484  * vector of indices. This algorithm is based off the paper An Efficient Parametric
485  * Algorithm for Octree Traversal: http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf
486  * \param[in] min_x octree nodes X coordinate of lower bounding box corner
487  * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
488  * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
489  * \param[in] max_x octree nodes X coordinate of upper bounding box corner
490  * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
491  * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
492  * \param[in] a
493  * \param[in] node current octree node to be explored
494  * \param[in] key octree key addressing a leaf node.
495  * \param[out] k_indices resulting indices
496  * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
497  * disable)
498  * \return number of voxels found
499  */
500  int
502  double min_y,
503  double min_z,
504  double max_x,
505  double max_y,
506  double max_z,
507  unsigned char a,
508  const OctreeNode* node,
509  const OctreeKey& key,
510  std::vector<int>& k_indices,
511  int max_voxel_count) const;
512 
513  /** \brief Initialize raytracing algorithm
514  * \param origin
515  * \param direction
516  * \param[in] min_x octree nodes X coordinate of lower bounding box corner
517  * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
518  * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
519  * \param[in] max_x octree nodes X coordinate of upper bounding box corner
520  * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
521  * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
522  * \param a
523  */
524  inline void
525  initIntersectedVoxel(Eigen::Vector3f& origin,
526  Eigen::Vector3f& direction,
527  double& min_x,
528  double& min_y,
529  double& min_z,
530  double& max_x,
531  double& max_y,
532  double& max_z,
533  unsigned char& a) const
534  {
535  // Account for division by zero when direction vector is 0.0
536  const float epsilon = 1e-10f;
537  if (direction.x() == 0.0)
538  direction.x() = epsilon;
539  if (direction.y() == 0.0)
540  direction.y() = epsilon;
541  if (direction.z() == 0.0)
542  direction.z() = epsilon;
543 
544  // Voxel childIdx remapping
545  a = 0;
546 
547  // Handle negative axis direction vector
548  if (direction.x() < 0.0) {
549  origin.x() = static_cast<float>(this->min_x_) + static_cast<float>(this->max_x_) -
550  origin.x();
551  direction.x() = -direction.x();
552  a |= 4;
553  }
554  if (direction.y() < 0.0) {
555  origin.y() = static_cast<float>(this->min_y_) + static_cast<float>(this->max_y_) -
556  origin.y();
557  direction.y() = -direction.y();
558  a |= 2;
559  }
560  if (direction.z() < 0.0) {
561  origin.z() = static_cast<float>(this->min_z_) + static_cast<float>(this->max_z_) -
562  origin.z();
563  direction.z() = -direction.z();
564  a |= 1;
565  }
566  min_x = (this->min_x_ - origin.x()) / direction.x();
567  max_x = (this->max_x_ - origin.x()) / direction.x();
568  min_y = (this->min_y_ - origin.y()) / direction.y();
569  max_y = (this->max_y_ - origin.y()) / direction.y();
570  min_z = (this->min_z_ - origin.z()) / direction.z();
571  max_z = (this->max_z_ - origin.z()) / direction.z();
572  }
573 
574  /** \brief Find first child node ray will enter
575  * \param[in] min_x octree nodes X coordinate of lower bounding box corner
576  * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
577  * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
578  * \param[in] mid_x octree nodes X coordinate of bounding box mid line
579  * \param[in] mid_y octree nodes Y coordinate of bounding box mid line
580  * \param[in] mid_z octree nodes Z coordinate of bounding box mid line
581  * \return the first child node ray will enter
582  */
583  inline int
585  double min_y,
586  double min_z,
587  double mid_x,
588  double mid_y,
589  double mid_z) const
590  {
591  int currNode = 0;
592 
593  if (min_x > min_y) {
594  if (min_x > min_z) {
595  // max(min_x, min_y, min_z) is min_x. Entry plane is YZ.
596  if (mid_y < min_x)
597  currNode |= 2;
598  if (mid_z < min_x)
599  currNode |= 1;
600  }
601  else {
602  // max(min_x, min_y, min_z) is min_z. Entry plane is XY.
603  if (mid_x < min_z)
604  currNode |= 4;
605  if (mid_y < min_z)
606  currNode |= 2;
607  }
608  }
609  else {
610  if (min_y > min_z) {
611  // max(min_x, min_y, min_z) is min_y. Entry plane is XZ.
612  if (mid_x < min_y)
613  currNode |= 4;
614  if (mid_z < min_y)
615  currNode |= 1;
616  }
617  else {
618  // max(min_x, min_y, min_z) is min_z. Entry plane is XY.
619  if (mid_x < min_z)
620  currNode |= 4;
621  if (mid_y < min_z)
622  currNode |= 2;
623  }
624  }
625 
626  return currNode;
627  }
628 
629  /** \brief Get the next visited node given the current node upper
630  * bounding box corner. This function accepts three float values, and
631  * three int values. The function returns the ith integer where the
632  * ith float value is the minimum of the three float values.
633  * \param[in] x current nodes X coordinate of upper bounding box corner
634  * \param[in] y current nodes Y coordinate of upper bounding box corner
635  * \param[in] z current nodes Z coordinate of upper bounding box corner
636  * \param[in] a next node if exit Plane YZ
637  * \param[in] b next node if exit Plane XZ
638  * \param[in] c next node if exit Plane XY
639  * \return the next child node ray will enter or 8 if exiting
640  */
641  inline int
642  getNextIntersectedNode(double x, double y, double z, int a, int b, int c) const
643  {
644  if (x < y) {
645  if (x < z)
646  return a;
647  return c;
648  }
649  if (y < z)
650  return b;
651  return c;
652  }
653 };
654 } // namespace octree
655 } // namespace pcl
656 
657 #ifdef PCL_NO_PRECOMPILE
658 #include <pcl/octree/impl/octree_search.hpp>
659 #endif
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
pcl::K
@ K
Definition: norms.h:54
pcl::octree::OctreePointCloudSearch::prioPointQueueEntry::prioPointQueueEntry
prioPointQueueEntry()
Empty constructor
Definition: octree_search.h:339
pcl::octree::OctreePointCloudSearch::getIntersectedVoxelIndicesRecursive
int getIntersectedVoxelIndicesRecursive(double min_x, double min_y, double min_z, double max_x, double max_y, double max_z, unsigned char a, const OctreeNode *node, const OctreeKey &key, std::vector< int > &k_indices, int max_voxel_count) const
Recursively search the tree for all intersected leaf nodes and return a vector of indices.
Definition: octree_search.hpp:878
pcl::octree::OctreePointCloudSearch::getIntersectedVoxelIndices
int getIntersectedVoxelIndices(Eigen::Vector3f origin, Eigen::Vector3f direction, std::vector< int > &k_indices, int max_voxel_count=0) const
Get indices of all voxels that are intersected by a ray (origin, direction).
Definition: octree_search.hpp:646
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:397
pcl::octree::OctreePointCloudSearch::AlignedPointTVector
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
Definition: octree_search.h:75
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::prioBranchQueueEntry
prioBranchQueueEntry(OctreeNode *_node, OctreeKey &_key, float _point_distance)
Constructor for initializing priority queue entry.
Definition: octree_search.h:307
pcl::octree::OctreePointCloudSearch::getKNearestNeighborRecursive
double getKNearestNeighborRecursive(const PointT &point, unsigned int K, const BranchNode *node, const OctreeKey &key, unsigned int tree_depth, const double squared_search_radius, std::vector< prioPointQueueEntry > &point_candidates) const
Recursive search method that explores the octree and finds the K nearest neighbors.
Definition: octree_search.hpp:233
pcl::octree::OctreePointCloud< PointT, pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty >::PointCloudConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: octree_pointcloud.h:91
pcl::octree::OctreePointCloud
Octree pointcloud class
Definition: octree_pointcloud.h:73
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::node
const OctreeNode * node
Pointer to octree node.
Definition: octree_search.h:321
pcl::octree::OctreePointCloudSearch::prioPointQueueEntry::operator<
bool operator<(const prioPointQueueEntry &rhs) const
Operator< for comparing priority queue entries with each other.
Definition: octree_search.h:353
pcl::octree::OctreePointCloudSearch::boxSearchRecursive
void boxSearchRecursive(const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, const BranchNode *node, const OctreeKey &key, unsigned int tree_depth, std::vector< int > &k_indices) const
Recursive search method that explores the octree and finds points within a rectangular search area.
Definition: octree_search.hpp:533
pcl::octree::OctreePointCloudSearch::voxelSearch
bool voxelSearch(const PointT &point, std::vector< int > &point_idx_data)
Search for neighbors within a voxel at given point.
Definition: octree_search.hpp:48
pcl::octree::OctreePointCloudSearch::nearestKSearch
int nearestKSearch(const PointCloud &cloud, int index, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances)
Search for k-nearest neighbors at the query point.
Definition: octree_search.h:116
pcl::octree::OctreePointCloudSearch::getNeighborsWithinRadiusRecursive
void getNeighborsWithinRadiusRecursive(const PointT &point, const double radiusSquared, const BranchNode *node, const OctreeKey &key, unsigned int tree_depth, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn) const
Recursive search method that explores the octree and finds neighbors within a given radius.
Definition: octree_search.hpp:346
pcl::octree::OctreePointCloudSearch::getIntersectedVoxelCenters
int getIntersectedVoxelCenters(Eigen::Vector3f origin, Eigen::Vector3f direction, AlignedPointTVector &voxel_center_list, int max_voxel_count=0) const
Get a PointT vector of centers of all voxels that intersected by a ray (origin, direction).
Definition: octree_search.hpp:609
pcl::octree::OctreeKey
Octree key class
Definition: octree_key.h:51
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: projection_matrix.h:52
pcl::octree::OctreePointCloudSearch::radiusSearch
int radiusSearch(const PointCloud &cloud, int index, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0)
Search for all neighbors of query point that are within a given radius.
Definition: octree_search.h:203
pcl::octree::OctreePointCloudSearch::approxNearestSearch
void approxNearestSearch(const PointCloud &cloud, int query_index, int &result_index, float &sqr_distance)
Search for approx.
Definition: octree_search.h:165
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:620
pcl::octree::OctreeNode
Abstract octree node class
Definition: octree_nodes.h:62
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty >::min_y_
double min_y_
Definition: octree_pointcloud.h:588
pcl::octree::OctreePointCloudSearch::prioPointQueueEntry
Priority queue entry for point candidates
Definition: octree_search.h:336
pcl::octree::OctreeLeafNode
Abstract octree leaf class
Definition: octree_nodes.h:84
pcl::octree::OctreePointCloudSearch::prioPointQueueEntry::point_idx_
int point_idx_
Index representing a point in the dataset given by setInputCloud.
Definition: octree_search.h:359
pcl::octree::OctreePointCloudSearch::prioPointQueueEntry::point_distance_
float point_distance_
Distance to query point.
Definition: octree_search.h:362
pcl::octree::OctreePointCloud< PointT, pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty >::ConstPtr
shared_ptr< const OctreePointCloud< PointT, pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty, OctreeBase< pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty > > > ConstPtr
Definition: octree_pointcloud.h:105
pcl::octree::OctreePointCloud< PointT, pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty >::IndicesConstPtr
shared_ptr< const std::vector< int > > IndicesConstPtr
Definition: octree_pointcloud.h:87
pcl::octree::OctreeBase< pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty >
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::key
OctreeKey key
Octree key.
Definition: octree_search.h:327
pcl::octree::OctreePointCloudSearch::OctreePointCloudSearch
OctreePointCloudSearch(const double resolution)
Constructor.
Definition: octree_search.h:84
pcl::octree::OctreeBranchNode
Abstract octree branch class
Definition: octree_nodes.h:167
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::point_distance
float point_distance
Distance to query point.
Definition: octree_search.h:324
pcl::octree::OctreePointCloudSearch::getNextIntersectedNode
int getNextIntersectedNode(double x, double y, double z, int a, int b, int c) const
Get the next visited node given the current node upper bounding box corner.
Definition: octree_search.h:642
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry
Priority queue entry for branch nodes
Definition: octree_search.h:297
pcl::octree::OctreePointCloud< PointT, pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty >::PointCloudPtr
typename PointCloud::Ptr PointCloudPtr
Definition: octree_pointcloud.h:90
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty >::min_z_
double min_z_
Definition: octree_pointcloud.h:591
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty >::max_x_
double max_x_
Definition: octree_pointcloud.h:586
pcl::octree::OctreePointCloud::BranchNode
typename OctreeT::BranchNode BranchNode
Definition: octree_pointcloud.h:78
pcl::octree::OctreePointCloudSearch::approxNearestSearchRecursive
void approxNearestSearchRecursive(const PointT &point, const BranchNode *node, const OctreeKey &key, unsigned int tree_depth, int &result_index, float &sqr_distance)
Recursive search method that explores the octree and finds the approximate nearest neighbor.
Definition: octree_search.hpp:434
pcl::octree::OctreePointCloudSearch::initIntersectedVoxel
void initIntersectedVoxel(Eigen::Vector3f &origin, Eigen::Vector3f &direction, double &min_x, double &min_y, double &min_z, double &max_x, double &max_y, double &max_z, unsigned char &a) const
Initialize raytracing algorithm.
Definition: octree_search.h:525
pcl::octree::OctreePointCloud< PointT, pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty >::Ptr
shared_ptr< OctreePointCloud< PointT, pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty, OctreeBase< pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty > > > Ptr
Definition: octree_pointcloud.h:103
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty >::max_z_
double max_z_
Definition: octree_pointcloud.h:592
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty >::max_y_
double max_y_
Definition: octree_pointcloud.h:589
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:415
pcl::octree::OctreePointCloudSearch::pointSquaredDist
float pointSquaredDist(const PointT &point_a, const PointT &point_b) const
Helper function to calculate the squared distance between two points.
Definition: octree_search.hpp:524
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::operator<
bool operator<(const prioBranchQueueEntry rhs) const
Operator< for comparing priority queue entries with each other.
Definition: octree_search.h:315
pcl::octree::OctreePointCloudSearch::prioPointQueueEntry::prioPointQueueEntry
prioPointQueueEntry(unsigned int &point_idx, float point_distance)
Constructor for initializing priority queue entry.
Definition: octree_search.h:345
pcl::octree::OctreePointCloudSearch::getIntersectedVoxelCentersRecursive
int getIntersectedVoxelCentersRecursive(double min_x, double min_y, double min_z, double max_x, double max_y, double max_z, unsigned char a, const OctreeNode *node, const OctreeKey &key, AlignedPointTVector &voxel_center_list, int max_voxel_count) const
Recursively search the tree for all intersected leaf nodes and return a vector of voxel centers.
Definition: octree_search.hpp:681
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:416
pcl::octree::OctreePointCloud< PointT, pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty >::IndicesPtr
shared_ptr< std::vector< int > > IndicesPtr
Definition: octree_pointcloud.h:86
pcl::octree::OctreePointCloudSearch::getFirstIntersectedNode
int getFirstIntersectedNode(double min_x, double min_y, double min_z, double mid_x, double mid_y, double mid_z) const
Find first child node ray will enter.
Definition: octree_search.h:584
pcl::octree::OctreePointCloudSearch::prioBranchQueueEntry::prioBranchQueueEntry
prioBranchQueueEntry()
Empty constructor
Definition: octree_search.h:300
pcl::octree::OctreePointCloudSearch
Octree pointcloud search class
Definition: octree_search.h:57
pcl::octree::OctreePointCloud< PointT, pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty >::AlignedPointTVector
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
Definition: octree_pointcloud.h:108
pcl::octree::OctreePointCloudSearch::boxSearch
int boxSearch(const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, std::vector< int > &k_indices) const
Search for points within rectangular search area Points exactly on the edges of the search rectangle ...
Definition: octree_search.hpp:214
pcl::octree::OctreePointCloudSearch::BranchNode
typename OctreeT::BranchNode BranchNode
Definition: octree_search.h:79
pcl::octree::OctreePointCloud::LeafNode
typename OctreeT::LeafNode LeafNode
Definition: octree_pointcloud.h:77
pcl::shared_ptr
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: pcl_macros.h:108
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty >::min_x_
double min_x_
Definition: octree_pointcloud.h:585