Point Cloud Library (PCL)  1.10.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 <cassert>
43 
44 //////////////////////////////////////////////////////////////////////////////////////////////
45 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
46 bool
48  voxelSearch(const PointT& point, std::vector<int>& point_idx_data)
49 {
50  assert(isFinite(point) &&
51  "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
52  OctreeKey key;
53  bool b_success = false;
54 
55  // generate key
56  this->genOctreeKeyforPoint(point, key);
57 
58  LeafContainerT* leaf = this->findLeaf(key);
59 
60  if (leaf) {
61  (*leaf).getPointIndices(point_idx_data);
62  b_success = true;
63  }
64 
65  return (b_success);
66 }
67 
68 //////////////////////////////////////////////////////////////////////////////////////////////
69 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
70 bool
72  voxelSearch(const int index, std::vector<int>& point_idx_data)
73 {
74  const PointT search_point = this->getPointByIndex(index);
75  return (this->voxelSearch(search_point, point_idx_data));
76 }
77 
78 //////////////////////////////////////////////////////////////////////////////////////////////
79 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
80 int
83  int k,
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) &&
89  "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
90 
91  k_indices.clear();
92  k_sqr_distances.clear();
93 
94  if (k < 1)
95  return 0;
96 
97  prioPointQueueEntry point_entry;
98  std::vector<prioPointQueueEntry> point_candidates;
99 
100  OctreeKey key;
101  key.x = key.y = key.z = 0;
102 
103  // initialize smallest point distance in search with high value
104  double smallest_dist = std::numeric_limits<double>::max();
105 
106  getKNearestNeighborRecursive(
107  p_q, k, this->root_node_, key, 1, smallest_dist, point_candidates);
108 
109  unsigned int result_count = static_cast<unsigned int>(point_candidates.size());
110 
111  k_indices.resize(result_count);
112  k_sqr_distances.resize(result_count);
113 
114  for (unsigned int i = 0; i < result_count; ++i) {
115  k_indices[i] = point_candidates[i].point_idx_;
116  k_sqr_distances[i] = point_candidates[i].point_distance_;
117  }
118 
119  return static_cast<int>(k_indices.size());
120 }
121 
122 //////////////////////////////////////////////////////////////////////////////////////////////
123 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
124 int
126  nearestKSearch(int index,
127  int k,
128  std::vector<int>& k_indices,
129  std::vector<float>& k_sqr_distances)
130 {
131  const PointT search_point = this->getPointByIndex(index);
132  return (nearestKSearch(search_point, k, k_indices, k_sqr_distances));
133 }
134 
135 //////////////////////////////////////////////////////////////////////////////////////////////
136 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
137 void
139  approxNearestSearch(const PointT& p_q, int& result_index, float& sqr_distance)
140 {
141  assert(this->leaf_count_ > 0);
142  assert(isFinite(p_q) &&
143  "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
144 
145  OctreeKey key;
146  key.x = key.y = key.z = 0;
147 
148  approxNearestSearchRecursive(
149  p_q, this->root_node_, key, 1, result_index, sqr_distance);
150 
151  return;
152 }
153 
154 //////////////////////////////////////////////////////////////////////////////////////////////
155 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
156 void
158  approxNearestSearch(int query_index, int& result_index, float& sqr_distance)
159 {
160  const PointT search_point = this->getPointByIndex(query_index);
161 
162  return (approxNearestSearch(search_point, result_index, sqr_distance));
163 }
164 
165 //////////////////////////////////////////////////////////////////////////////////////////////
166 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
167 int
170  const double radius,
171  std::vector<int>& k_indices,
172  std::vector<float>& k_sqr_distances,
173  unsigned int max_nn) const
174 {
175  assert(isFinite(p_q) &&
176  "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
177  OctreeKey key;
178  key.x = key.y = key.z = 0;
179 
180  k_indices.clear();
181  k_sqr_distances.clear();
182 
183  getNeighborsWithinRadiusRecursive(p_q,
184  radius * radius,
185  this->root_node_,
186  key,
187  1,
188  k_indices,
189  k_sqr_distances,
190  max_nn);
191 
192  return (static_cast<int>(k_indices.size()));
193 }
194 
195 //////////////////////////////////////////////////////////////////////////////////////////////
196 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
197 int
199  radiusSearch(int index,
200  const double radius,
201  std::vector<int>& k_indices,
202  std::vector<float>& k_sqr_distances,
203  unsigned int max_nn) const
204 {
205  const PointT search_point = this->getPointByIndex(index);
206 
207  return (radiusSearch(search_point, radius, k_indices, k_sqr_distances, max_nn));
208 }
209 
210 //////////////////////////////////////////////////////////////////////////////////////////////
211 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
212 int
214  boxSearch(const Eigen::Vector3f& min_pt,
215  const Eigen::Vector3f& max_pt,
216  std::vector<int>& k_indices) const
217 {
218 
219  OctreeKey key;
220  key.x = key.y = key.z = 0;
221 
222  k_indices.clear();
223 
224  boxSearchRecursive(min_pt, max_pt, this->root_node_, key, 1, k_indices);
225 
226  return (static_cast<int>(k_indices.size()));
227 }
228 
229 //////////////////////////////////////////////////////////////////////////////////////////////
230 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
231 double
234  const PointT& point,
235  unsigned int K,
236  const BranchNode* node,
237  const OctreeKey& key,
238  unsigned int tree_depth,
239  const double squared_search_radius,
240  std::vector<prioPointQueueEntry>& point_candidates) const
241 {
242  std::vector<prioBranchQueueEntry> search_heap;
243  search_heap.resize(8);
244 
245  OctreeKey new_key;
246 
247  double smallest_squared_dist = squared_search_radius;
248 
249  // get spatial voxel information
250  double voxelSquaredDiameter = this->getVoxelSquaredDiameter(tree_depth);
251 
252  // iterate over all children
253  for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
254  if (this->branchHasChild(*node, child_idx)) {
255  PointT voxel_center;
256 
257  search_heap[child_idx].key.x = (key.x << 1) + (!!(child_idx & (1 << 2)));
258  search_heap[child_idx].key.y = (key.y << 1) + (!!(child_idx & (1 << 1)));
259  search_heap[child_idx].key.z = (key.z << 1) + (!!(child_idx & (1 << 0)));
260 
261  // generate voxel center point for voxel at key
262  this->genVoxelCenterFromOctreeKey(
263  search_heap[child_idx].key, tree_depth, voxel_center);
264 
265  // generate new priority queue element
266  search_heap[child_idx].node = this->getBranchChildPtr(*node, child_idx);
267  search_heap[child_idx].point_distance = pointSquaredDist(voxel_center, point);
268  }
269  else {
270  search_heap[child_idx].point_distance = std::numeric_limits<float>::infinity();
271  }
272  }
273 
274  std::sort(search_heap.begin(), search_heap.end());
275 
276  // iterate over all children in priority queue
277  // check if the distance to search candidate is smaller than the best point distance
278  // (smallest_squared_dist)
279  while ((!search_heap.empty()) &&
280  (search_heap.back().point_distance <
281  smallest_squared_dist + voxelSquaredDiameter / 4.0 +
282  sqrt(smallest_squared_dist * voxelSquaredDiameter) - this->epsilon_)) {
283  const OctreeNode* child_node;
284 
285  // read from priority queue element
286  child_node = search_heap.back().node;
287  new_key = search_heap.back().key;
288 
289  if (tree_depth < this->octree_depth_) {
290  // we have not reached maximum tree depth
291  smallest_squared_dist =
292  getKNearestNeighborRecursive(point,
293  K,
294  static_cast<const BranchNode*>(child_node),
295  new_key,
296  tree_depth + 1,
297  smallest_squared_dist,
298  point_candidates);
299  }
300  else {
301  // we reached leaf node level
302  std::vector<int> decoded_point_vector;
303 
304  const LeafNode* child_leaf = static_cast<const LeafNode*>(child_node);
305 
306  // decode leaf node into decoded_point_vector
307  (*child_leaf)->getPointIndices(decoded_point_vector);
308 
309  // Linearly iterate over all decoded (unsorted) points
310  for (const int& point_index : decoded_point_vector) {
311 
312  const PointT& candidate_point = this->getPointByIndex(point_index);
313 
314  // calculate point distance to search point
315  float squared_dist = pointSquaredDist(candidate_point, point);
316 
317  // check if a closer match is found
318  if (squared_dist < smallest_squared_dist) {
319  prioPointQueueEntry point_entry;
320 
321  point_entry.point_distance_ = squared_dist;
322  point_entry.point_idx_ = point_index;
323  point_candidates.push_back(point_entry);
324  }
325  }
326 
327  std::sort(point_candidates.begin(), point_candidates.end());
328 
329  if (point_candidates.size() > K)
330  point_candidates.resize(K);
331 
332  if (point_candidates.size() == K)
333  smallest_squared_dist = point_candidates.back().point_distance_;
334  }
335  // pop element from priority queue
336  search_heap.pop_back();
337  }
338 
339  return (smallest_squared_dist);
340 }
341 
342 //////////////////////////////////////////////////////////////////////////////////////////////
343 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
344 void
347  const double radiusSquared,
348  const BranchNode* node,
349  const OctreeKey& key,
350  unsigned int tree_depth,
351  std::vector<int>& k_indices,
352  std::vector<float>& k_sqr_distances,
353  unsigned int max_nn) const
354 {
355  // get spatial voxel information
356  double voxel_squared_diameter = this->getVoxelSquaredDiameter(tree_depth);
357 
358  // iterate over all children
359  for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
360  if (!this->branchHasChild(*node, child_idx))
361  continue;
362 
363  const OctreeNode* child_node;
364  child_node = this->getBranchChildPtr(*node, child_idx);
365 
366  OctreeKey new_key;
367  PointT voxel_center;
368  float squared_dist;
369 
370  // generate new key for current branch voxel
371  new_key.x = (key.x << 1) + (!!(child_idx & (1 << 2)));
372  new_key.y = (key.y << 1) + (!!(child_idx & (1 << 1)));
373  new_key.z = (key.z << 1) + (!!(child_idx & (1 << 0)));
374 
375  // generate voxel center point for voxel at key
376  this->genVoxelCenterFromOctreeKey(new_key, tree_depth, voxel_center);
377 
378  // calculate distance to search point
379  squared_dist = pointSquaredDist(static_cast<const PointT&>(voxel_center), point);
380 
381  // if distance is smaller than search radius
382  if (squared_dist + this->epsilon_ <=
383  voxel_squared_diameter / 4.0 + radiusSquared +
384  sqrt(voxel_squared_diameter * radiusSquared)) {
385 
386  if (tree_depth < this->octree_depth_) {
387  // we have not reached maximum tree depth
389  radiusSquared,
390  static_cast<const BranchNode*>(child_node),
391  new_key,
392  tree_depth + 1,
393  k_indices,
394  k_sqr_distances,
395  max_nn);
396  if (max_nn != 0 && k_indices.size() == static_cast<unsigned int>(max_nn))
397  return;
398  }
399  else {
400  // we reached leaf node level
401  const LeafNode* child_leaf = static_cast<const LeafNode*>(child_node);
402  std::vector<int> decoded_point_vector;
403 
404  // decode leaf node into decoded_point_vector
405  (*child_leaf)->getPointIndices(decoded_point_vector);
406 
407  // Linearly iterate over all decoded (unsorted) points
408  for (const int& index : decoded_point_vector) {
409  const PointT& candidate_point = this->getPointByIndex(index);
410 
411  // calculate point distance to search point
412  squared_dist = pointSquaredDist(candidate_point, point);
413 
414  // check if a match is found
415  if (squared_dist > radiusSquared)
416  continue;
417 
418  // add point to result vector
419  k_indices.push_back(index);
420  k_sqr_distances.push_back(squared_dist);
421 
422  if (max_nn != 0 && k_indices.size() == static_cast<unsigned int>(max_nn))
423  return;
424  }
425  }
426  }
427  }
428 }
429 
430 //////////////////////////////////////////////////////////////////////////////////////////////
431 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
432 void
435  const BranchNode* node,
436  const OctreeKey& key,
437  unsigned int tree_depth,
438  int& result_index,
439  float& sqr_distance)
440 {
441  OctreeKey minChildKey;
442  OctreeKey new_key;
443 
444  const OctreeNode* child_node;
445 
446  // set minimum voxel distance to maximum value
447  double min_voxel_center_distance = std::numeric_limits<double>::max();
448 
449  unsigned char min_child_idx = 0xFF;
450 
451  // iterate over all children
452  for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
453  if (!this->branchHasChild(*node, child_idx))
454  continue;
455 
456  PointT voxel_center;
457  double voxelPointDist;
458 
459  new_key.x = (key.x << 1) + (!!(child_idx & (1 << 2)));
460  new_key.y = (key.y << 1) + (!!(child_idx & (1 << 1)));
461  new_key.z = (key.z << 1) + (!!(child_idx & (1 << 0)));
462 
463  // generate voxel center point for voxel at key
464  this->genVoxelCenterFromOctreeKey(new_key, tree_depth, voxel_center);
465 
466  voxelPointDist = pointSquaredDist(voxel_center, point);
467 
468  // search for child voxel with shortest distance to search point
469  if (voxelPointDist >= min_voxel_center_distance)
470  continue;
471 
472  min_voxel_center_distance = voxelPointDist;
473  min_child_idx = child_idx;
474  minChildKey = new_key;
475  }
476 
477  // make sure we found at least one branch child
478  assert(min_child_idx < 8);
479 
480  child_node = this->getBranchChildPtr(*node, min_child_idx);
481 
482  if (tree_depth < this->octree_depth_) {
483  // we have not reached maximum tree depth
484  approxNearestSearchRecursive(point,
485  static_cast<const BranchNode*>(child_node),
486  minChildKey,
487  tree_depth + 1,
488  result_index,
489  sqr_distance);
490  }
491  else {
492  // we reached leaf node level
493  std::vector<int> decoded_point_vector;
494 
495  const LeafNode* child_leaf = static_cast<const LeafNode*>(child_node);
496 
497  double smallest_squared_dist = std::numeric_limits<double>::max();
498 
499  // decode leaf node into decoded_point_vector
500  (**child_leaf).getPointIndices(decoded_point_vector);
501 
502  // Linearly iterate over all decoded (unsorted) points
503  for (const int& index : decoded_point_vector) {
504  const PointT& candidate_point = this->getPointByIndex(index);
505 
506  // calculate point distance to search point
507  double squared_dist = pointSquaredDist(candidate_point, point);
508 
509  // check if a closer match is found
510  if (squared_dist >= smallest_squared_dist)
511  continue;
512 
513  result_index = index;
514  smallest_squared_dist = squared_dist;
515  sqr_distance = static_cast<float>(squared_dist);
516  }
517  }
518 }
519 
520 //////////////////////////////////////////////////////////////////////////////////////////////
521 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
522 float
524  pointSquaredDist(const PointT& point_a, const PointT& point_b) const
525 {
526  return (point_a.getVector3fMap() - point_b.getVector3fMap()).squaredNorm();
527 }
528 
529 //////////////////////////////////////////////////////////////////////////////////////////////
530 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
531 void
533  boxSearchRecursive(const Eigen::Vector3f& min_pt,
534  const Eigen::Vector3f& max_pt,
535  const BranchNode* node,
536  const OctreeKey& key,
537  unsigned int tree_depth,
538  std::vector<int>& k_indices) const
539 {
540  // iterate over all children
541  for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
542 
543  const OctreeNode* child_node;
544  child_node = this->getBranchChildPtr(*node, child_idx);
545 
546  if (!child_node)
547  continue;
548 
549  OctreeKey new_key;
550  // generate new key for current branch voxel
551  new_key.x = (key.x << 1) + (!!(child_idx & (1 << 2)));
552  new_key.y = (key.y << 1) + (!!(child_idx & (1 << 1)));
553  new_key.z = (key.z << 1) + (!!(child_idx & (1 << 0)));
554 
555  // voxel corners
556  Eigen::Vector3f lower_voxel_corner;
557  Eigen::Vector3f upper_voxel_corner;
558  // get voxel coordinates
559  this->genVoxelBoundsFromOctreeKey(
560  new_key, tree_depth, lower_voxel_corner, upper_voxel_corner);
561 
562  // test if search region overlap with voxel space
563 
564  if (!((lower_voxel_corner(0) > max_pt(0)) || (min_pt(0) > upper_voxel_corner(0)) ||
565  (lower_voxel_corner(1) > max_pt(1)) || (min_pt(1) > upper_voxel_corner(1)) ||
566  (lower_voxel_corner(2) > max_pt(2)) || (min_pt(2) > upper_voxel_corner(2)))) {
567 
568  if (tree_depth < this->octree_depth_) {
569  // we have not reached maximum tree depth
570  boxSearchRecursive(min_pt,
571  max_pt,
572  static_cast<const BranchNode*>(child_node),
573  new_key,
574  tree_depth + 1,
575  k_indices);
576  }
577  else {
578  // we reached leaf node level
579  std::vector<int> decoded_point_vector;
580 
581  const LeafNode* child_leaf = static_cast<const LeafNode*>(child_node);
582 
583  // decode leaf node into decoded_point_vector
584  (**child_leaf).getPointIndices(decoded_point_vector);
585 
586  // Linearly iterate over all decoded (unsorted) points
587  for (const int& index : decoded_point_vector) {
588  const PointT& candidate_point = this->getPointByIndex(index);
589 
590  // check if point falls within search box
591  bool bInBox =
592  ((candidate_point.x >= min_pt(0)) && (candidate_point.x <= max_pt(0)) &&
593  (candidate_point.y >= min_pt(1)) && (candidate_point.y <= max_pt(1)) &&
594  (candidate_point.z >= min_pt(2)) && (candidate_point.z <= max_pt(2)));
595 
596  if (bInBox)
597  // add to result vector
598  k_indices.push_back(index);
599  }
600  }
601  }
602  }
603 }
604 
605 //////////////////////////////////////////////////////////////////////////////////////////////
606 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
607 int
609  getIntersectedVoxelCenters(Eigen::Vector3f origin,
610  Eigen::Vector3f direction,
611  AlignedPointTVector& voxel_center_list,
612  int max_voxel_count) const
613 {
614  OctreeKey key;
615  key.x = key.y = key.z = 0;
616 
617  voxel_center_list.clear();
618 
619  // Voxel child_idx remapping
620  unsigned char a = 0;
621 
622  double min_x, min_y, min_z, max_x, max_y, max_z;
623 
624  initIntersectedVoxel(origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);
625 
626  if (std::max(std::max(min_x, min_y), min_z) < std::min(std::min(max_x, max_y), max_z))
627  return getIntersectedVoxelCentersRecursive(min_x,
628  min_y,
629  min_z,
630  max_x,
631  max_y,
632  max_z,
633  a,
634  this->root_node_,
635  key,
636  voxel_center_list,
637  max_voxel_count);
638 
639  return (0);
640 }
641 
642 //////////////////////////////////////////////////////////////////////////////////////////////
643 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
644 int
646  getIntersectedVoxelIndices(Eigen::Vector3f origin,
647  Eigen::Vector3f direction,
648  std::vector<int>& k_indices,
649  int max_voxel_count) const
650 {
651  OctreeKey key;
652  key.x = key.y = key.z = 0;
653 
654  k_indices.clear();
655 
656  // Voxel child_idx remapping
657  unsigned char a = 0;
658  double min_x, min_y, min_z, max_x, max_y, max_z;
659 
660  initIntersectedVoxel(origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);
661 
662  if (std::max(std::max(min_x, min_y), min_z) < std::min(std::min(max_x, max_y), max_z))
663  return getIntersectedVoxelIndicesRecursive(min_x,
664  min_y,
665  min_z,
666  max_x,
667  max_y,
668  max_z,
669  a,
670  this->root_node_,
671  key,
672  k_indices,
673  max_voxel_count);
674  return (0);
675 }
676 
677 //////////////////////////////////////////////////////////////////////////////////////////////
678 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
679 int
682  double min_y,
683  double min_z,
684  double max_x,
685  double max_y,
686  double max_z,
687  unsigned char a,
688  const OctreeNode* node,
689  const OctreeKey& key,
690  AlignedPointTVector& voxel_center_list,
691  int max_voxel_count) const
692 {
693  if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0)
694  return (0);
695 
696  // If leaf node, get voxel center and increment intersection count
697  if (node->getNodeType() == LEAF_NODE) {
698  PointT newPoint;
699 
700  this->genLeafNodeCenterFromOctreeKey(key, newPoint);
701 
702  voxel_center_list.push_back(newPoint);
703 
704  return (1);
705  }
706 
707  // Voxel intersection count for branches children
708  int voxel_count = 0;
709 
710  // Voxel mid lines
711  double mid_x = 0.5 * (min_x + max_x);
712  double mid_y = 0.5 * (min_y + max_y);
713  double mid_z = 0.5 * (min_z + max_z);
714 
715  // First voxel node ray will intersect
716  int curr_node = getFirstIntersectedNode(min_x, min_y, min_z, mid_x, mid_y, mid_z);
717 
718  // Child index, node and key
719  unsigned char child_idx;
720  OctreeKey child_key;
721 
722  do {
723  if (curr_node != 0)
724  child_idx = static_cast<unsigned char>(curr_node ^ a);
725  else
726  child_idx = a;
727 
728  // child_node == 0 if child_node doesn't exist
729  const OctreeNode* child_node =
730  this->getBranchChildPtr(static_cast<const BranchNode&>(*node), child_idx);
731 
732  // Generate new key for current branch voxel
733  child_key.x = (key.x << 1) | (!!(child_idx & (1 << 2)));
734  child_key.y = (key.y << 1) | (!!(child_idx & (1 << 1)));
735  child_key.z = (key.z << 1) | (!!(child_idx & (1 << 0)));
736 
737  // Recursively call each intersected child node, selecting the next
738  // node intersected by the ray. Children that do not intersect will
739  // not be traversed.
740 
741  switch (curr_node) {
742  case 0:
743  if (child_node)
744  voxel_count += getIntersectedVoxelCentersRecursive(min_x,
745  min_y,
746  min_z,
747  mid_x,
748  mid_y,
749  mid_z,
750  a,
751  child_node,
752  child_key,
753  voxel_center_list,
754  max_voxel_count);
755  curr_node = getNextIntersectedNode(mid_x, mid_y, mid_z, 4, 2, 1);
756  break;
757 
758  case 1:
759  if (child_node)
760  voxel_count += getIntersectedVoxelCentersRecursive(min_x,
761  min_y,
762  mid_z,
763  mid_x,
764  mid_y,
765  max_z,
766  a,
767  child_node,
768  child_key,
769  voxel_center_list,
770  max_voxel_count);
771  curr_node = getNextIntersectedNode(mid_x, mid_y, max_z, 5, 3, 8);
772  break;
773 
774  case 2:
775  if (child_node)
776  voxel_count += getIntersectedVoxelCentersRecursive(min_x,
777  mid_y,
778  min_z,
779  mid_x,
780  max_y,
781  mid_z,
782  a,
783  child_node,
784  child_key,
785  voxel_center_list,
786  max_voxel_count);
787  curr_node = getNextIntersectedNode(mid_x, max_y, mid_z, 6, 8, 3);
788  break;
789 
790  case 3:
791  if (child_node)
792  voxel_count += getIntersectedVoxelCentersRecursive(min_x,
793  mid_y,
794  mid_z,
795  mid_x,
796  max_y,
797  max_z,
798  a,
799  child_node,
800  child_key,
801  voxel_center_list,
802  max_voxel_count);
803  curr_node = getNextIntersectedNode(mid_x, max_y, max_z, 7, 8, 8);
804  break;
805 
806  case 4:
807  if (child_node)
808  voxel_count += getIntersectedVoxelCentersRecursive(mid_x,
809  min_y,
810  min_z,
811  max_x,
812  mid_y,
813  mid_z,
814  a,
815  child_node,
816  child_key,
817  voxel_center_list,
818  max_voxel_count);
819  curr_node = getNextIntersectedNode(max_x, mid_y, mid_z, 8, 6, 5);
820  break;
821 
822  case 5:
823  if (child_node)
824  voxel_count += getIntersectedVoxelCentersRecursive(mid_x,
825  min_y,
826  mid_z,
827  max_x,
828  mid_y,
829  max_z,
830  a,
831  child_node,
832  child_key,
833  voxel_center_list,
834  max_voxel_count);
835  curr_node = getNextIntersectedNode(max_x, mid_y, max_z, 8, 7, 8);
836  break;
837 
838  case 6:
839  if (child_node)
840  voxel_count += getIntersectedVoxelCentersRecursive(mid_x,
841  mid_y,
842  min_z,
843  max_x,
844  max_y,
845  mid_z,
846  a,
847  child_node,
848  child_key,
849  voxel_center_list,
850  max_voxel_count);
851  curr_node = getNextIntersectedNode(max_x, max_y, mid_z, 8, 8, 7);
852  break;
853 
854  case 7:
855  if (child_node)
856  voxel_count += getIntersectedVoxelCentersRecursive(mid_x,
857  mid_y,
858  mid_z,
859  max_x,
860  max_y,
861  max_z,
862  a,
863  child_node,
864  child_key,
865  voxel_center_list,
866  max_voxel_count);
867  curr_node = 8;
868  break;
869  }
870  } while ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count));
871  return (voxel_count);
872 }
873 
874 //////////////////////////////////////////////////////////////////////////////////////////////
875 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
876 int
879  double min_y,
880  double min_z,
881  double max_x,
882  double max_y,
883  double max_z,
884  unsigned char a,
885  const OctreeNode* node,
886  const OctreeKey& key,
887  std::vector<int>& k_indices,
888  int max_voxel_count) const
889 {
890  if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0)
891  return (0);
892 
893  // If leaf node, get voxel center and increment intersection count
894  if (node->getNodeType() == LEAF_NODE) {
895  const LeafNode* leaf = static_cast<const LeafNode*>(node);
896 
897  // decode leaf node into k_indices
898  (*leaf)->getPointIndices(k_indices);
899 
900  return (1);
901  }
902 
903  // Voxel intersection count for branches children
904  int voxel_count = 0;
905 
906  // Voxel mid lines
907  double mid_x = 0.5 * (min_x + max_x);
908  double mid_y = 0.5 * (min_y + max_y);
909  double mid_z = 0.5 * (min_z + max_z);
910 
911  // First voxel node ray will intersect
912  int curr_node = getFirstIntersectedNode(min_x, min_y, min_z, mid_x, mid_y, mid_z);
913 
914  // Child index, node and key
915  unsigned char child_idx;
916  OctreeKey child_key;
917  do {
918  if (curr_node != 0)
919  child_idx = static_cast<unsigned char>(curr_node ^ a);
920  else
921  child_idx = a;
922 
923  // child_node == 0 if child_node doesn't exist
924  const OctreeNode* child_node =
925  this->getBranchChildPtr(static_cast<const BranchNode&>(*node), child_idx);
926  // Generate new key for current branch voxel
927  child_key.x = (key.x << 1) | (!!(child_idx & (1 << 2)));
928  child_key.y = (key.y << 1) | (!!(child_idx & (1 << 1)));
929  child_key.z = (key.z << 1) | (!!(child_idx & (1 << 0)));
930 
931  // Recursively call each intersected child node, selecting the next
932  // node intersected by the ray. Children that do not intersect will
933  // not be traversed.
934  switch (curr_node) {
935  case 0:
936  if (child_node)
937  voxel_count += getIntersectedVoxelIndicesRecursive(min_x,
938  min_y,
939  min_z,
940  mid_x,
941  mid_y,
942  mid_z,
943  a,
944  child_node,
945  child_key,
946  k_indices,
947  max_voxel_count);
948  curr_node = getNextIntersectedNode(mid_x, mid_y, mid_z, 4, 2, 1);
949  break;
950 
951  case 1:
952  if (child_node)
953  voxel_count += getIntersectedVoxelIndicesRecursive(min_x,
954  min_y,
955  mid_z,
956  mid_x,
957  mid_y,
958  max_z,
959  a,
960  child_node,
961  child_key,
962  k_indices,
963  max_voxel_count);
964  curr_node = getNextIntersectedNode(mid_x, mid_y, max_z, 5, 3, 8);
965  break;
966 
967  case 2:
968  if (child_node)
969  voxel_count += getIntersectedVoxelIndicesRecursive(min_x,
970  mid_y,
971  min_z,
972  mid_x,
973  max_y,
974  mid_z,
975  a,
976  child_node,
977  child_key,
978  k_indices,
979  max_voxel_count);
980  curr_node = getNextIntersectedNode(mid_x, max_y, mid_z, 6, 8, 3);
981  break;
982 
983  case 3:
984  if (child_node)
985  voxel_count += getIntersectedVoxelIndicesRecursive(min_x,
986  mid_y,
987  mid_z,
988  mid_x,
989  max_y,
990  max_z,
991  a,
992  child_node,
993  child_key,
994  k_indices,
995  max_voxel_count);
996  curr_node = getNextIntersectedNode(mid_x, max_y, max_z, 7, 8, 8);
997  break;
998 
999  case 4:
1000  if (child_node)
1001  voxel_count += getIntersectedVoxelIndicesRecursive(mid_x,
1002  min_y,
1003  min_z,
1004  max_x,
1005  mid_y,
1006  mid_z,
1007  a,
1008  child_node,
1009  child_key,
1010  k_indices,
1011  max_voxel_count);
1012  curr_node = getNextIntersectedNode(max_x, mid_y, mid_z, 8, 6, 5);
1013  break;
1014 
1015  case 5:
1016  if (child_node)
1017  voxel_count += getIntersectedVoxelIndicesRecursive(mid_x,
1018  min_y,
1019  mid_z,
1020  max_x,
1021  mid_y,
1022  max_z,
1023  a,
1024  child_node,
1025  child_key,
1026  k_indices,
1027  max_voxel_count);
1028  curr_node = getNextIntersectedNode(max_x, mid_y, max_z, 8, 7, 8);
1029  break;
1030 
1031  case 6:
1032  if (child_node)
1033  voxel_count += getIntersectedVoxelIndicesRecursive(mid_x,
1034  mid_y,
1035  min_z,
1036  max_x,
1037  max_y,
1038  mid_z,
1039  a,
1040  child_node,
1041  child_key,
1042  k_indices,
1043  max_voxel_count);
1044  curr_node = getNextIntersectedNode(max_x, max_y, mid_z, 8, 8, 7);
1045  break;
1046 
1047  case 7:
1048  if (child_node)
1049  voxel_count += getIntersectedVoxelIndicesRecursive(mid_x,
1050  mid_y,
1051  mid_z,
1052  max_x,
1053  max_y,
1054  max_z,
1055  a,
1056  child_node,
1057  child_key,
1058  k_indices,
1059  max_voxel_count);
1060  curr_node = 8;
1061  break;
1062  }
1063  } while ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count));
1064 
1065  return (voxel_count);
1066 }
1067 
1068 #define PCL_INSTANTIATE_OctreePointCloudSearch(T) \
1069  template class PCL_EXPORTS pcl::octree::OctreePointCloudSearch<T>;
1070 
1071 #endif // PCL_OCTREE_SEARCH_IMPL_H_
pcl::K
Definition: norms.h:54
pcl::octree::OctreePointCloudSearch::getIntersectedVoxelIndicesRecursive
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.
Definition: octree_search.hpp:878
pcl::octree::OctreePointCloudSearch::getIntersectedVoxelIndices
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).
Definition: octree_search.hpp:646
pcl::isFinite
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:55
pcl::octree::OctreeNode::getNodeType
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)
pcl::octree::OctreePointCloudSearch::getKNearestNeighborRecursive
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.
Definition: octree_search.hpp:233
pcl::octree::OctreePointCloudSearch::boxSearchRecursive
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.
Definition: octree_search.hpp:533
pcl::octree::OctreePointCloudSearch::voxelSearch
bool voxelSearch(const PointT &point, std::vector< int > &point_idx_data)
Search for neighbors within a voxel at given point.
Definition: octree_search.hpp:48
pcl::octree::OctreePointCloudSearch::nearestKSearch
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.
Definition: octree_search.h:116
pcl::octree::OctreePointCloudSearch::getNeighborsWithinRadiusRecursive
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.
Definition: octree_search.hpp:346
pcl::octree::OctreePointCloudSearch::getIntersectedVoxelCenters
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).
Definition: octree_search.hpp:609
pcl::octree::OctreeKey
Octree key class
Definition: octree_key.h:51
pcl::octree::OctreePointCloudSearch::radiusSearch
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.
Definition: octree_search.h:203
pcl::octree::OctreePointCloudSearch::approxNearestSearch
void approxNearestSearch(const PointCloud &cloud, int query_index, int &result_index, float &sqr_distance)
Search for approx.
Definition: octree_search.h:165
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:620
pcl::octree::OctreeNode
Abstract octree node class
Definition: octree_nodes.h:62
pcl::octree::LEAF_NODE
Definition: octree_nodes.h:55
pcl::octree::OctreeLeafNode
Abstract octree leaf class
Definition: octree_nodes.h:84
pcl::octree::OctreeBranchNode
Abstract octree branch class
Definition: octree_nodes.h:167
pcl::octree::OctreeKey::z
std::uint32_t z
Definition: octree_key.h:150
pcl::octree::OctreePointCloudSearch::approxNearestSearchRecursive
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.
Definition: octree_search.hpp:434
pcl::octree::OctreePointCloudSearch::pointSquaredDist
float pointSquaredDist(const PointT &point_a, const PointT &point_b) const
Helper function to calculate the squared distance between two points.
Definition: octree_search.hpp:524
pcl::octree::OctreePointCloudSearch::getIntersectedVoxelCentersRecursive
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: octree_search.hpp:681
pcl::octree::OctreePointCloud< PointT, LeafTWrap, BranchTWrap >::AlignedPointTVector
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
Definition: octree_pointcloud.h:108
pcl::octree::OctreePointCloudSearch::boxSearch
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 ...
Definition: octree_search.hpp:214
pcl::octree::OctreeKey::x
std::uint32_t x
Definition: octree_key.h:148
pcl::octree::OctreeKey::y
std::uint32_t y
Definition: octree_key.h:149