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