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