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