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