Point Cloud Library (PCL)  1.7.1
octree_search.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, 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 #ifndef PCL_OCTREE_SEARCH_IMPL_H_
40 #define PCL_OCTREE_SEARCH_IMPL_H_
41 
42 #include <pcl/point_cloud.h>
43 #include <pcl/point_types.h>
44 
45 #include <pcl/common/common.h>
46 #include <assert.h>
47 
48 using namespace std;
49 
50 //////////////////////////////////////////////////////////////////////////////////////////////
51 template<typename PointT, typename LeafContainerT, typename BranchContainerT> bool
53  std::vector<int>& point_idx_data)
54 {
55  assert (isFinite (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
56  OctreeKey key;
57  bool b_success = false;
58 
59  // generate key
60  this->genOctreeKeyforPoint (point, key);
61 
62  LeafContainerT* leaf = this->findLeaf (key);
63 
64  if (leaf)
65  {
66  (*leaf).getPointIndices (point_idx_data);
67  b_success = true;
68  }
69 
70  return (b_success);
71 }
72 
73 //////////////////////////////////////////////////////////////////////////////////////////////
74 template<typename PointT, typename LeafContainerT, typename BranchContainerT> bool
76  std::vector<int>& point_idx_data)
77 {
78  const PointT search_point = this->getPointByIndex (index);
79  return (this->voxelSearch (search_point, point_idx_data));
80 }
81 
82 //////////////////////////////////////////////////////////////////////////////////////////////
83 template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
85  std::vector<int> &k_indices,
86  std::vector<float> &k_sqr_distances)
87 {
88  assert(this->leaf_count_>0);
89  assert (isFinite (p_q) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
90 
91  k_indices.clear ();
92  k_sqr_distances.clear ();
93 
94  if (k < 1)
95  return 0;
96 
97  unsigned int i;
98  unsigned int result_count;
99 
100  prioPointQueueEntry point_entry;
101  std::vector<prioPointQueueEntry> point_candidates;
102 
103  OctreeKey key;
104  key.x = key.y = key.z = 0;
105 
106  // initalize smallest point distance in search with high value
107  double smallest_dist = numeric_limits<double>::max ();
108 
109  getKNearestNeighborRecursive (p_q, k, this->root_node_, key, 1, smallest_dist, point_candidates);
110 
111  result_count = static_cast<unsigned int> (point_candidates.size ());
112 
113  k_indices.resize (result_count);
114  k_sqr_distances.resize (result_count);
115 
116  for (i = 0; i < result_count; ++i)
117  {
118  k_indices [i] = point_candidates [i].point_idx_;
119  k_sqr_distances [i] = point_candidates [i].point_distance_;
120  }
121 
122  return static_cast<int> (k_indices.size ());
123 }
124 
125 //////////////////////////////////////////////////////////////////////////////////////////////
126 template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
128  std::vector<int> &k_indices,
129  std::vector<float> &k_sqr_distances)
130 {
131  const PointT search_point = this->getPointByIndex (index);
132  return (nearestKSearch (search_point, k, k_indices, k_sqr_distances));
133 }
134 
135 //////////////////////////////////////////////////////////////////////////////////////////////
136 template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
138  int &result_index,
139  float &sqr_distance)
140 {
141  assert(this->leaf_count_>0);
142  assert (isFinite (p_q) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
143 
144  OctreeKey key;
145  key.x = key.y = key.z = 0;
146 
147  approxNearestSearchRecursive (p_q, this->root_node_, key, 1, result_index, sqr_distance);
148 
149  return;
150 }
151 
152 //////////////////////////////////////////////////////////////////////////////////////////////
153 template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
155  float &sqr_distance)
156 {
157  const PointT search_point = this->getPointByIndex (query_index);
158 
159  return (approxNearestSearch (search_point, result_index, sqr_distance));
160 }
161 
162 //////////////////////////////////////////////////////////////////////////////////////////////
163 template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
165  std::vector<int> &k_indices,
166  std::vector<float> &k_sqr_distances,
167  unsigned int max_nn) const
168 {
169  assert (isFinite (p_q) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
170  OctreeKey key;
171  key.x = key.y = key.z = 0;
172 
173  k_indices.clear ();
174  k_sqr_distances.clear ();
175 
176  getNeighborsWithinRadiusRecursive (p_q, radius * radius, this->root_node_, key, 1, k_indices, k_sqr_distances,
177  max_nn);
178 
179  return (static_cast<int> (k_indices.size ()));
180 }
181 
182 //////////////////////////////////////////////////////////////////////////////////////////////
183 template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
185  std::vector<int> &k_indices,
186  std::vector<float> &k_sqr_distances,
187  unsigned int max_nn) const
188 {
189  const PointT search_point = this->getPointByIndex (index);
190 
191  return (radiusSearch (search_point, radius, k_indices, k_sqr_distances, max_nn));
192 }
193 
194 //////////////////////////////////////////////////////////////////////////////////////////////
195 template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
197  const Eigen::Vector3f &max_pt,
198  std::vector<int> &k_indices) const
199 {
200 
201  OctreeKey key;
202  key.x = key.y = key.z = 0;
203 
204  k_indices.clear ();
205 
206  boxSearchRecursive (min_pt, max_pt, this->root_node_, key, 1, k_indices);
207 
208  return (static_cast<int> (k_indices.size ()));
209 
210 }
211 
212 //////////////////////////////////////////////////////////////////////////////////////////////
213 template<typename PointT, typename LeafContainerT, typename BranchContainerT> double
215  const PointT & point, unsigned int K, const BranchNode* node, const OctreeKey& key, unsigned int tree_depth,
216  const double squared_search_radius, std::vector<prioPointQueueEntry>& point_candidates) const
217 {
218  std::vector<prioBranchQueueEntry> search_heap;
219  search_heap.resize (8);
220 
221  unsigned char child_idx;
222 
223  OctreeKey new_key;
224 
225  double smallest_squared_dist = squared_search_radius;
226 
227  // get spatial voxel information
228  double voxelSquaredDiameter = this->getVoxelSquaredDiameter (tree_depth);
229 
230  // iterate over all children
231  for (child_idx = 0; child_idx < 8; child_idx++)
232  {
233  if (this->branchHasChild (*node, child_idx))
234  {
235  PointT voxel_center;
236 
237  search_heap[child_idx].key.x = (key.x << 1) + (!!(child_idx & (1 << 2)));
238  search_heap[child_idx].key.y = (key.y << 1) + (!!(child_idx & (1 << 1)));
239  search_heap[child_idx].key.z = (key.z << 1) + (!!(child_idx & (1 << 0)));
240 
241  // generate voxel center point for voxel at key
242  this->genVoxelCenterFromOctreeKey (search_heap[child_idx].key, tree_depth, voxel_center);
243 
244  // generate new priority queue element
245  search_heap[child_idx].node = this->getBranchChildPtr (*node, child_idx);
246  search_heap[child_idx].point_distance = pointSquaredDist (voxel_center, point);
247  }
248  else
249  {
250  search_heap[child_idx].point_distance = numeric_limits<float>::infinity ();
251  }
252  }
253 
254  std::sort (search_heap.begin (), search_heap.end ());
255 
256  // iterate over all children in priority queue
257  // check if the distance to search candidate is smaller than the best point distance (smallest_squared_dist)
258  while ((!search_heap.empty ()) && (search_heap.back ().point_distance <
259  smallest_squared_dist + voxelSquaredDiameter / 4.0 + sqrt (smallest_squared_dist * voxelSquaredDiameter) - this->epsilon_))
260  {
261  const OctreeNode* child_node;
262 
263  // read from priority queue element
264  child_node = search_heap.back ().node;
265  new_key = search_heap.back ().key;
266 
267  if (tree_depth < this->octree_depth_)
268  {
269  // we have not reached maximum tree depth
270  smallest_squared_dist = getKNearestNeighborRecursive (point, K, static_cast<const BranchNode*> (child_node), new_key, tree_depth + 1,
271  smallest_squared_dist, point_candidates);
272  }
273  else
274  {
275  // we reached leaf node level
276 
277  float squared_dist;
278  size_t i;
279  vector<int> decoded_point_vector;
280 
281  const LeafNode* child_leaf = static_cast<const LeafNode*> (child_node);
282 
283  // decode leaf node into decoded_point_vector
284  (*child_leaf)->getPointIndices (decoded_point_vector);
285 
286  // Linearly iterate over all decoded (unsorted) points
287  for (i = 0; i < decoded_point_vector.size (); i++)
288  {
289 
290  const PointT& candidate_point = this->getPointByIndex (decoded_point_vector[i]);
291 
292  // calculate point distance to search point
293  squared_dist = pointSquaredDist (candidate_point, point);
294 
295  // check if a closer match is found
296  if (squared_dist < smallest_squared_dist)
297  {
298  prioPointQueueEntry point_entry;
299 
300  point_entry.point_distance_ = squared_dist;
301  point_entry.point_idx_ = decoded_point_vector[i];
302  point_candidates.push_back (point_entry);
303  }
304  }
305 
306  std::sort (point_candidates.begin (), point_candidates.end ());
307 
308  if (point_candidates.size () > K)
309  point_candidates.resize (K);
310 
311  if (point_candidates.size () == K)
312  smallest_squared_dist = point_candidates.back ().point_distance_;
313  }
314  // pop element from priority queue
315  search_heap.pop_back ();
316  }
317 
318  return (smallest_squared_dist);
319 }
320 
321 //////////////////////////////////////////////////////////////////////////////////////////////
322 template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
324  const PointT & point, const double radiusSquared, const BranchNode* node, const OctreeKey& key,
325  unsigned int tree_depth, std::vector<int>& k_indices, std::vector<float>& k_sqr_distances,
326  unsigned int max_nn) const
327 {
328  // child iterator
329  unsigned char child_idx;
330 
331  // get spatial voxel information
332  double voxel_squared_diameter = this->getVoxelSquaredDiameter (tree_depth);
333 
334  // iterate over all children
335  for (child_idx = 0; child_idx < 8; child_idx++)
336  {
337  if (!this->branchHasChild (*node, child_idx))
338  continue;
339 
340  const OctreeNode* child_node;
341  child_node = this->getBranchChildPtr (*node, child_idx);
342 
343  OctreeKey new_key;
344  PointT voxel_center;
345  float squared_dist;
346 
347  // generate new key for current branch voxel
348  new_key.x = (key.x << 1) + (!!(child_idx & (1 << 2)));
349  new_key.y = (key.y << 1) + (!!(child_idx & (1 << 1)));
350  new_key.z = (key.z << 1) + (!!(child_idx & (1 << 0)));
351 
352  // generate voxel center point for voxel at key
353  this->genVoxelCenterFromOctreeKey (new_key, tree_depth, voxel_center);
354 
355  // calculate distance to search point
356  squared_dist = pointSquaredDist (static_cast<const PointT&> (voxel_center), point);
357 
358  // if distance is smaller than search radius
359  if (squared_dist + this->epsilon_
360  <= voxel_squared_diameter / 4.0 + radiusSquared + sqrt (voxel_squared_diameter * radiusSquared))
361  {
362 
363  if (tree_depth < this->octree_depth_)
364  {
365  // we have not reached maximum tree depth
366  getNeighborsWithinRadiusRecursive (point, radiusSquared, static_cast<const BranchNode*> (child_node), new_key, tree_depth + 1,
367  k_indices, k_sqr_distances, max_nn);
368  if (max_nn != 0 && k_indices.size () == static_cast<unsigned int> (max_nn))
369  return;
370  }
371  else
372  {
373  // we reached leaf node level
374 
375  size_t i;
376  const LeafNode* child_leaf = static_cast<const LeafNode*> (child_node);
377  vector<int> decoded_point_vector;
378 
379  // decode leaf node into decoded_point_vector
380  (*child_leaf)->getPointIndices (decoded_point_vector);
381 
382  // Linearly iterate over all decoded (unsorted) points
383  for (i = 0; i < decoded_point_vector.size (); i++)
384  {
385  const PointT& candidate_point = this->getPointByIndex (decoded_point_vector[i]);
386 
387  // calculate point distance to search point
388  squared_dist = pointSquaredDist (candidate_point, point);
389 
390  // check if a match is found
391  if (squared_dist > radiusSquared)
392  continue;
393 
394  // add point to result vector
395  k_indices.push_back (decoded_point_vector[i]);
396  k_sqr_distances.push_back (squared_dist);
397 
398  if (max_nn != 0 && k_indices.size () == static_cast<unsigned int> (max_nn))
399  return;
400  }
401  }
402  }
403  }
404 }
405 
406 //////////////////////////////////////////////////////////////////////////////////////////////
407 template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
409  const BranchNode* node,
410  const OctreeKey& key,
411  unsigned int tree_depth,
412  int& result_index,
413  float& sqr_distance)
414 {
415  unsigned char child_idx;
416  unsigned char min_child_idx;
417  double min_voxel_center_distance;
418 
419  OctreeKey minChildKey;
420  OctreeKey new_key;
421 
422  const OctreeNode* child_node;
423 
424  // set minimum voxel distance to maximum value
425  min_voxel_center_distance = numeric_limits<double>::max ();
426 
427  min_child_idx = 0xFF;
428 
429  // iterate over all children
430  for (child_idx = 0; child_idx < 8; child_idx++)
431  {
432  if (!this->branchHasChild (*node, child_idx))
433  continue;
434 
435  PointT voxel_center;
436  double voxelPointDist;
437 
438  new_key.x = (key.x << 1) + (!!(child_idx & (1 << 2)));
439  new_key.y = (key.y << 1) + (!!(child_idx & (1 << 1)));
440  new_key.z = (key.z << 1) + (!!(child_idx & (1 << 0)));
441 
442  // generate voxel center point for voxel at key
443  this->genVoxelCenterFromOctreeKey (new_key, tree_depth, voxel_center);
444 
445  voxelPointDist = pointSquaredDist (voxel_center, point);
446 
447  // search for child voxel with shortest distance to search point
448  if (voxelPointDist >= min_voxel_center_distance)
449  continue;
450 
451  min_voxel_center_distance = voxelPointDist;
452  min_child_idx = child_idx;
453  minChildKey = new_key;
454  }
455 
456  // make sure we found at least one branch child
457  assert(min_child_idx<8);
458 
459  child_node = this->getBranchChildPtr (*node, min_child_idx);
460 
461  if (tree_depth < this->octree_depth_)
462  {
463  // we have not reached maximum tree depth
464  approxNearestSearchRecursive (point, static_cast<const BranchNode*> (child_node), minChildKey, tree_depth + 1, result_index,
465  sqr_distance);
466  }
467  else
468  {
469  // we reached leaf node level
470 
471  double squared_dist;
472  double smallest_squared_dist;
473  size_t i;
474  vector<int> decoded_point_vector;
475 
476  const LeafNode* child_leaf = static_cast<const LeafNode*> (child_node);
477 
478  smallest_squared_dist = numeric_limits<double>::max ();
479 
480  // decode leaf node into decoded_point_vector
481  (**child_leaf).getPointIndices (decoded_point_vector);
482 
483  // Linearly iterate over all decoded (unsorted) points
484  for (i = 0; i < decoded_point_vector.size (); i++)
485  {
486  const PointT& candidate_point = this->getPointByIndex (decoded_point_vector[i]);
487 
488  // calculate point distance to search point
489  squared_dist = pointSquaredDist (candidate_point, point);
490 
491  // check if a closer match is found
492  if (squared_dist >= smallest_squared_dist)
493  continue;
494 
495  result_index = decoded_point_vector[i];
496  smallest_squared_dist = squared_dist;
497  sqr_distance = static_cast<float> (squared_dist);
498  }
499  }
500 }
501 
502 //////////////////////////////////////////////////////////////////////////////////////////////
503 template<typename PointT, typename LeafContainerT, typename BranchContainerT> float
505  const PointT & point_b) const
506 {
507  return (point_a.getVector3fMap () - point_b.getVector3fMap ()).squaredNorm ();
508 }
509 
510 //////////////////////////////////////////////////////////////////////////////////////////////
511 template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
513  const Eigen::Vector3f &max_pt,
514  const BranchNode* node,
515  const OctreeKey& key,
516  unsigned int tree_depth,
517  std::vector<int>& k_indices) const
518 {
519  // child iterator
520  unsigned char child_idx;
521 
522  // iterate over all children
523  for (child_idx = 0; child_idx < 8; child_idx++)
524  {
525 
526  const OctreeNode* child_node;
527  child_node = this->getBranchChildPtr (*node, child_idx);
528 
529  if (!child_node)
530  continue;
531 
532  OctreeKey new_key;
533  // generate new key for current branch voxel
534  new_key.x = (key.x << 1) + (!!(child_idx & (1 << 2)));
535  new_key.y = (key.y << 1) + (!!(child_idx & (1 << 1)));
536  new_key.z = (key.z << 1) + (!!(child_idx & (1 << 0)));
537 
538  // voxel corners
539  Eigen::Vector3f lower_voxel_corner;
540  Eigen::Vector3f upper_voxel_corner;
541  // get voxel coordinates
542  this->genVoxelBoundsFromOctreeKey (new_key, tree_depth, lower_voxel_corner, upper_voxel_corner);
543 
544  // test if search region overlap with voxel space
545 
546  if ( !( (lower_voxel_corner (0) > max_pt (0)) || (min_pt (0) > upper_voxel_corner(0)) ||
547  (lower_voxel_corner (1) > max_pt (1)) || (min_pt (1) > upper_voxel_corner(1)) ||
548  (lower_voxel_corner (2) > max_pt (2)) || (min_pt (2) > upper_voxel_corner(2)) ) )
549  {
550 
551  if (tree_depth < this->octree_depth_)
552  {
553  // we have not reached maximum tree depth
554  boxSearchRecursive (min_pt, max_pt, static_cast<const BranchNode*> (child_node), new_key, tree_depth + 1, k_indices);
555  }
556  else
557  {
558  // we reached leaf node level
559  size_t i;
560  vector<int> decoded_point_vector;
561  bool bInBox;
562 
563  const LeafNode* child_leaf = static_cast<const LeafNode*> (child_node);
564 
565  // decode leaf node into decoded_point_vector
566  (**child_leaf).getPointIndices (decoded_point_vector);
567 
568  // Linearly iterate over all decoded (unsorted) points
569  for (i = 0; i < decoded_point_vector.size (); i++)
570  {
571  const PointT& candidate_point = this->getPointByIndex (decoded_point_vector[i]);
572 
573  // check if point falls within search box
574  bInBox = ( (candidate_point.x >= min_pt (0)) && (candidate_point.x <= max_pt (0)) &&
575  (candidate_point.y >= min_pt (1)) && (candidate_point.y <= max_pt (1)) &&
576  (candidate_point.z >= min_pt (2)) && (candidate_point.z <= max_pt (2)) );
577 
578  if (bInBox)
579  // add to result vector
580  k_indices.push_back (decoded_point_vector[i]);
581  }
582  }
583  }
584  }
585 }
586 
587 //////////////////////////////////////////////////////////////////////////////////////////////
588 template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
590  Eigen::Vector3f origin, Eigen::Vector3f direction, AlignedPointTVector &voxel_center_list,
591  int max_voxel_count) const
592 {
593  OctreeKey key;
594  key.x = key.y = key.z = 0;
595 
596  voxel_center_list.clear ();
597 
598  // Voxel child_idx remapping
599  unsigned char a = 0;
600 
601  double min_x, min_y, min_z, max_x, max_y, max_z;
602 
603  initIntersectedVoxel (origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);
604 
605  if (max (max (min_x, min_y), min_z) < min (min (max_x, max_y), max_z))
606  return getIntersectedVoxelCentersRecursive (min_x, min_y, min_z, max_x, max_y, max_z, a, this->root_node_, key,
607  voxel_center_list, max_voxel_count);
608 
609  return (0);
610 }
611 
612 //////////////////////////////////////////////////////////////////////////////////////////////
613 template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
615  Eigen::Vector3f origin, Eigen::Vector3f direction, std::vector<int> &k_indices,
616  int max_voxel_count) const
617 {
618  OctreeKey key;
619  key.x = key.y = key.z = 0;
620 
621  k_indices.clear ();
622 
623  // Voxel child_idx remapping
624  unsigned char a = 0;
625  double min_x, min_y, min_z, max_x, max_y, max_z;
626 
627  initIntersectedVoxel (origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);
628 
629  if (max (max (min_x, min_y), min_z) < min (min (max_x, max_y), max_z))
630  return getIntersectedVoxelIndicesRecursive (min_x, min_y, min_z, max_x, max_y, max_z, a, this->root_node_, key,
631  k_indices, max_voxel_count);
632  return (0);
633 }
634 
635 //////////////////////////////////////////////////////////////////////////////////////////////
636 template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
638  double min_x, double min_y, double min_z, double max_x, double max_y, double max_z, unsigned char a,
639  const OctreeNode* node, const OctreeKey& key, AlignedPointTVector &voxel_center_list, int max_voxel_count) const
640 {
641  if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0)
642  return (0);
643 
644  // If leaf node, get voxel center and increment intersection count
645  if (node->getNodeType () == LEAF_NODE)
646  {
647  PointT newPoint;
648 
649  this->genLeafNodeCenterFromOctreeKey (key, newPoint);
650 
651  voxel_center_list.push_back (newPoint);
652 
653  return (1);
654  }
655 
656  // Voxel intersection count for branches children
657  int voxel_count = 0;
658 
659  // Voxel mid lines
660  double mid_x = 0.5 * (min_x + max_x);
661  double mid_y = 0.5 * (min_y + max_y);
662  double mid_z = 0.5 * (min_z + max_z);
663 
664  // First voxel node ray will intersect
665  int curr_node = getFirstIntersectedNode (min_x, min_y, min_z, mid_x, mid_y, mid_z);
666 
667  // Child index, node and key
668  unsigned char child_idx;
669  const OctreeNode *child_node;
670  OctreeKey child_key;
671 
672  do
673  {
674  if (curr_node != 0)
675  child_idx = static_cast<unsigned char> (curr_node ^ a);
676  else
677  child_idx = a;
678 
679  // child_node == 0 if child_node doesn't exist
680  child_node = this->getBranchChildPtr (static_cast<const BranchNode&> (*node), child_idx);
681 
682  // Generate new key for current branch voxel
683  child_key.x = (key.x << 1) | (!!(child_idx & (1 << 2)));
684  child_key.y = (key.y << 1) | (!!(child_idx & (1 << 1)));
685  child_key.z = (key.z << 1) | (!!(child_idx & (1 << 0)));
686 
687  // Recursively call each intersected child node, selecting the next
688  // node intersected by the ray. Children that do not intersect will
689  // not be traversed.
690 
691  switch (curr_node)
692  {
693  case 0:
694  if (child_node)
695  voxel_count += getIntersectedVoxelCentersRecursive (min_x, min_y, min_z, mid_x, mid_y, mid_z, a, child_node,
696  child_key, voxel_center_list, max_voxel_count);
697  curr_node = getNextIntersectedNode (mid_x, mid_y, mid_z, 4, 2, 1);
698  break;
699 
700  case 1:
701  if (child_node)
702  voxel_count += getIntersectedVoxelCentersRecursive (min_x, min_y, mid_z, mid_x, mid_y, max_z, a, child_node,
703  child_key, voxel_center_list, max_voxel_count);
704  curr_node = getNextIntersectedNode (mid_x, mid_y, max_z, 5, 3, 8);
705  break;
706 
707  case 2:
708  if (child_node)
709  voxel_count += getIntersectedVoxelCentersRecursive (min_x, mid_y, min_z, mid_x, max_y, mid_z, a, child_node,
710  child_key, voxel_center_list, max_voxel_count);
711  curr_node = getNextIntersectedNode (mid_x, max_y, mid_z, 6, 8, 3);
712  break;
713 
714  case 3:
715  if (child_node)
716  voxel_count += getIntersectedVoxelCentersRecursive (min_x, mid_y, mid_z, mid_x, max_y, max_z, a, child_node,
717  child_key, voxel_center_list, max_voxel_count);
718  curr_node = getNextIntersectedNode (mid_x, max_y, max_z, 7, 8, 8);
719  break;
720 
721  case 4:
722  if (child_node)
723  voxel_count += getIntersectedVoxelCentersRecursive (mid_x, min_y, min_z, max_x, mid_y, mid_z, a, child_node,
724  child_key, voxel_center_list, max_voxel_count);
725  curr_node = getNextIntersectedNode (max_x, mid_y, mid_z, 8, 6, 5);
726  break;
727 
728  case 5:
729  if (child_node)
730  voxel_count += getIntersectedVoxelCentersRecursive (mid_x, min_y, mid_z, max_x, mid_y, max_z, a, child_node,
731  child_key, voxel_center_list, max_voxel_count);
732  curr_node = getNextIntersectedNode (max_x, mid_y, max_z, 8, 7, 8);
733  break;
734 
735  case 6:
736  if (child_node)
737  voxel_count += getIntersectedVoxelCentersRecursive (mid_x, mid_y, min_z, max_x, max_y, mid_z, a, child_node,
738  child_key, voxel_center_list, max_voxel_count);
739  curr_node = getNextIntersectedNode (max_x, max_y, mid_z, 8, 8, 7);
740  break;
741 
742  case 7:
743  if (child_node)
744  voxel_count += getIntersectedVoxelCentersRecursive (mid_x, mid_y, mid_z, max_x, max_y, max_z, a, child_node,
745  child_key, voxel_center_list, max_voxel_count);
746  curr_node = 8;
747  break;
748  }
749  } while ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count));
750  return (voxel_count);
751 }
752 
753 //////////////////////////////////////////////////////////////////////////////////////////////
754 template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
756  double min_x, double min_y, double min_z, double max_x, double max_y, double max_z, unsigned char a,
757  const OctreeNode* node, const OctreeKey& key, std::vector<int> &k_indices, int max_voxel_count) const
758 {
759  if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0)
760  return (0);
761 
762  // If leaf node, get voxel center and increment intersection count
763  if (node->getNodeType () == LEAF_NODE)
764  {
765  const LeafNode* leaf = static_cast<const LeafNode*> (node);
766 
767  // decode leaf node into k_indices
768  (*leaf)->getPointIndices (k_indices);
769 
770  return (1);
771  }
772 
773  // Voxel intersection count for branches children
774  int voxel_count = 0;
775 
776  // Voxel mid lines
777  double mid_x = 0.5 * (min_x + max_x);
778  double mid_y = 0.5 * (min_y + max_y);
779  double mid_z = 0.5 * (min_z + max_z);
780 
781  // First voxel node ray will intersect
782  int curr_node = getFirstIntersectedNode (min_x, min_y, min_z, mid_x, mid_y, mid_z);
783 
784  // Child index, node and key
785  unsigned char child_idx;
786  const OctreeNode *child_node;
787  OctreeKey child_key;
788  do
789  {
790  if (curr_node != 0)
791  child_idx = static_cast<unsigned char> (curr_node ^ a);
792  else
793  child_idx = a;
794 
795  // child_node == 0 if child_node doesn't exist
796  child_node = this->getBranchChildPtr (static_cast<const BranchNode&> (*node), child_idx);
797  // Generate new key for current branch voxel
798  child_key.x = (key.x << 1) | (!!(child_idx & (1 << 2)));
799  child_key.y = (key.y << 1) | (!!(child_idx & (1 << 1)));
800  child_key.z = (key.z << 1) | (!!(child_idx & (1 << 0)));
801 
802  // Recursively call each intersected child node, selecting the next
803  // node intersected by the ray. Children that do not intersect will
804  // not be traversed.
805  switch (curr_node)
806  {
807  case 0:
808  if (child_node)
809  voxel_count += getIntersectedVoxelIndicesRecursive (min_x, min_y, min_z, mid_x, mid_y, mid_z, a, child_node,
810  child_key, k_indices, max_voxel_count);
811  curr_node = getNextIntersectedNode (mid_x, mid_y, mid_z, 4, 2, 1);
812  break;
813 
814  case 1:
815  if (child_node)
816  voxel_count += getIntersectedVoxelIndicesRecursive (min_x, min_y, mid_z, mid_x, mid_y, max_z, a, child_node,
817  child_key, k_indices, max_voxel_count);
818  curr_node = getNextIntersectedNode (mid_x, mid_y, max_z, 5, 3, 8);
819  break;
820 
821  case 2:
822  if (child_node)
823  voxel_count += getIntersectedVoxelIndicesRecursive (min_x, mid_y, min_z, mid_x, max_y, mid_z, a, child_node,
824  child_key, k_indices, max_voxel_count);
825  curr_node = getNextIntersectedNode (mid_x, max_y, mid_z, 6, 8, 3);
826  break;
827 
828  case 3:
829  if (child_node)
830  voxel_count += getIntersectedVoxelIndicesRecursive (min_x, mid_y, mid_z, mid_x, max_y, max_z, a, child_node,
831  child_key, k_indices, max_voxel_count);
832  curr_node = getNextIntersectedNode (mid_x, max_y, max_z, 7, 8, 8);
833  break;
834 
835  case 4:
836  if (child_node)
837  voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, min_y, min_z, max_x, mid_y, mid_z, a, child_node,
838  child_key, k_indices, max_voxel_count);
839  curr_node = getNextIntersectedNode (max_x, mid_y, mid_z, 8, 6, 5);
840  break;
841 
842  case 5:
843  if (child_node)
844  voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, min_y, mid_z, max_x, mid_y, max_z, a, child_node,
845  child_key, k_indices, max_voxel_count);
846  curr_node = getNextIntersectedNode (max_x, mid_y, max_z, 8, 7, 8);
847  break;
848 
849  case 6:
850  if (child_node)
851  voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, mid_y, min_z, max_x, max_y, mid_z, a, child_node,
852  child_key, k_indices, max_voxel_count);
853  curr_node = getNextIntersectedNode (max_x, max_y, mid_z, 8, 8, 7);
854  break;
855 
856  case 7:
857  if (child_node)
858  voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, mid_y, mid_z, max_x, max_y, max_z, a, child_node,
859  child_key, k_indices, max_voxel_count);
860  curr_node = 8;
861  break;
862  }
863  } while ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count));
864 
865  return (voxel_count);
866 }
867 
868 #endif // PCL_OCTREE_SEARCH_IMPL_H_