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