Point Cloud Library (PCL)  1.10.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/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 */
339  prioPointQueueEntry() : point_idx_(0), point_distance_(0) {}
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(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
shared_ptr< const OctreePointCloud< PointT, LeafTWrap, BranchTWrap, OctreeBase< LeafTWrap, BranchTWrap > > > ConstPtr
void boxSearchRecursive(const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, const BranchNode *node, const OctreeKey &key, unsigned int tree_depth, std::vector< int > &k_indices) const
Recursive search method that explores the octree and finds points within a rectangular search area...
Octree pointcloud class
prioBranchQueueEntry(OctreeNode *_node, OctreeKey &_key, float _point_distance)
Constructor for initializing priority queue entry.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
Definition: octree_search.h:75
bool voxelSearch(const PointT &point, std::vector< int > &point_idx_data)
Search for neighbors within a voxel at given point.
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:414
Priority queue entry for branch nodes
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:396
typename OctreeBase< LeafContainerT, BranchContainerT > ::BranchNode BranchNode
float pointSquaredDist(const PointT &point_a, const PointT &point_b) const
Helper function to calculate the squared distance between two points.
Priority queue entry for point candidates
int nearestKSearch(const PointCloud &cloud, int index, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances)
Search for k-nearest neighbors at the query point.
void approxNearestSearch(const PointCloud &cloud, int query_index, int &result_index, float &sqr_distance)
Search for approx.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
prioPointQueueEntry(unsigned int &point_idx, float point_distance)
Constructor for initializing priority queue entry.
int getFirstIntersectedNode(double min_x, double min_y, double min_z, double mid_x, double mid_y, double mid_z) const
Find first child node ray will enter.
int getNextIntersectedNode(double x, double y, double z, int a, int b, int c) const
Get the next visited node given the current node upper bounding box corner.
int getIntersectedVoxelIndices(Eigen::Vector3f origin, Eigen::Vector3f direction, std::vector< int > &k_indices, int max_voxel_count=0) const
Get indices of all voxels that are intersected by a ray (origin, direction).
int point_idx_
Index representing a point in the dataset given by setInputCloud.
double getKNearestNeighborRecursive(const PointT &point, unsigned int K, const BranchNode *node, const OctreeKey &key, unsigned int tree_depth, const double squared_search_radius, std::vector< prioPointQueueEntry > &point_candidates) const
Recursive search method that explores the octree and finds the K nearest neighbors.
const OctreeNode * node
Pointer to octree node.
OctreeKey key
Octree key.
int getIntersectedVoxelCenters(Eigen::Vector3f origin, Eigen::Vector3f direction, AlignedPointTVector &voxel_center_list, int max_voxel_count=0) const
Get a PointT vector of centers of all voxels that intersected by a ray (origin, direction).
void approxNearestSearchRecursive(const PointT &point, const BranchNode *node, const OctreeKey &key, unsigned int tree_depth, int &result_index, float &sqr_distance)
Recursive search method that explores the octree and finds the approximate nearest neighbor...
prioPointQueueEntry()
Empty constructor.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Octree key class
Definition: octree_key.h:52
float point_distance
Distance to query point.
Abstract octree leaf class
Definition: octree_nodes.h:85
int getIntersectedVoxelCentersRecursive(double min_x, double min_y, double min_z, double max_x, double max_y, double max_z, unsigned char a, const OctreeNode *node, const OctreeKey &key, AlignedPointTVector &voxel_center_list, int max_voxel_count) const
Recursively search the tree for all intersected leaf nodes and return a vector of voxel centers...
Definition: norms.h:54
Octree pointcloud search class
Definition: octree_search.h:57
int radiusSearch(const PointCloud &cloud, int index, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0)
Search for all neighbors of query point that are within a given radius.
void getNeighborsWithinRadiusRecursive(const PointT &point, const double radiusSquared, const BranchNode *node, const OctreeKey &key, unsigned int tree_depth, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn) const
Recursive search method that explores the octree and finds neighbors within a given radius...
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:415
bool operator<(const prioBranchQueueEntry rhs) const
Operator< for comparing priority queue entries with each other.
OctreePointCloudSearch(const double resolution)
Constructor.
Definition: octree_search.h:84
typename OctreeBase< LeafContainerT, BranchContainerT > ::LeafNode LeafNode
A point structure representing Euclidean xyz coordinates, and the RGB color.
Abstract octree branch class
Definition: octree_nodes.h:168
float point_distance_
Distance to query point.
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: memory.h:81
Abstract octree node class
Definition: octree_nodes.h:63
void initIntersectedVoxel(Eigen::Vector3f &origin, Eigen::Vector3f &direction, double &min_x, double &min_y, double &min_z, double &max_x, double &max_y, double &max_z, unsigned char &a) const
Initialize raytracing algorithm.
int boxSearch(const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, std::vector< int > &k_indices) const
Search for points within rectangular search area Points exactly on the edges of the search rectangle ...
int getIntersectedVoxelIndicesRecursive(double min_x, double min_y, double min_z, double max_x, double max_y, double max_z, unsigned char a, const OctreeNode *node, const OctreeKey &key, std::vector< int > &k_indices, int max_voxel_count) const
Recursively search the tree for all intersected leaf nodes and return a vector of indices...
prioBranchQueueEntry()
Empty constructor.
shared_ptr< OctreePointCloud< PointT, LeafTWrap, BranchTWrap, OctreeBase< LeafTWrap, BranchTWrap > > > Ptr