Point Cloud Library (PCL)  1.14.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  Indices decoded_point_vector;
402 
403  // decode leaf node into decoded_point_vector
404  (*child_leaf)->getPointIndices(decoded_point_vector);
405 
406  // Linearly iterate over all decoded (unsorted) points
407  for (const auto& index : decoded_point_vector) {
408  const PointT& candidate_point = this->getPointByIndex(index);
409 
410  // calculate point distance to search point
411  squared_dist = pointSquaredDist(candidate_point, point);
412 
413  // check if a match is found
414  if (squared_dist > radiusSquared)
415  continue;
416 
417  // add point to result vector
418  k_indices.push_back(index);
419  k_sqr_distances.push_back(squared_dist);
420 
421  if (max_nn != 0 && k_indices.size() == max_nn)
422  return;
423  }
424  }
425  }
426  }
427 }
428 
429 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
430 void
431 OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
432  approxNearestSearchRecursive(const PointT& point,
433  const BranchNode* node,
434  const OctreeKey& key,
435  uindex_t tree_depth,
436  index_t& result_index,
437  float& sqr_distance)
438 {
439  OctreeKey minChildKey;
440  OctreeKey new_key;
441 
442  const OctreeNode* child_node;
443 
444  // set minimum voxel distance to maximum value
445  double min_voxel_center_distance = std::numeric_limits<double>::max();
446 
447  unsigned char min_child_idx = 0xFF;
448 
449  // iterate over all children
450  for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
451  if (!this->branchHasChild(*node, child_idx))
452  continue;
453 
454  PointT voxel_center;
455  double voxelPointDist;
456 
457  new_key.x = (key.x << 1) + (!!(child_idx & (1 << 2)));
458  new_key.y = (key.y << 1) + (!!(child_idx & (1 << 1)));
459  new_key.z = (key.z << 1) + (!!(child_idx & (1 << 0)));
460 
461  // generate voxel center point for voxel at key
462  this->genVoxelCenterFromOctreeKey(new_key, tree_depth, voxel_center);
463 
464  voxelPointDist = pointSquaredDist(voxel_center, point);
465 
466  // search for child voxel with shortest distance to search point
467  if (voxelPointDist >= min_voxel_center_distance)
468  continue;
469 
470  min_voxel_center_distance = voxelPointDist;
471  min_child_idx = child_idx;
472  minChildKey = new_key;
473  }
474 
475  // make sure we found at least one branch child
476  assert(min_child_idx < 8);
477 
478  child_node = this->getBranchChildPtr(*node, min_child_idx);
479 
480  if (child_node->getNodeType() == BRANCH_NODE) {
481  // we have not reached maximum tree depth
482  approxNearestSearchRecursive(point,
483  static_cast<const BranchNode*>(child_node),
484  minChildKey,
485  tree_depth + 1,
486  result_index,
487  sqr_distance);
488  }
489  else {
490  // we reached leaf node level
491  Indices decoded_point_vector;
492 
493  const auto* child_leaf = static_cast<const LeafNode*>(child_node);
494 
495  float smallest_squared_dist = std::numeric_limits<float>::max();
496 
497  // decode leaf node into decoded_point_vector
498  (**child_leaf).getPointIndices(decoded_point_vector);
499 
500  // Linearly iterate over all decoded (unsorted) points
501  for (const auto& index : decoded_point_vector) {
502  const PointT& candidate_point = this->getPointByIndex(index);
503 
504  // calculate point distance to search point
505  float squared_dist = pointSquaredDist(candidate_point, point);
506 
507  // check if a closer match is found
508  if (squared_dist >= smallest_squared_dist)
509  continue;
510 
511  result_index = index;
512  smallest_squared_dist = squared_dist;
513  sqr_distance = squared_dist;
514  }
515  }
516 }
517 
518 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
519 float
520 OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::pointSquaredDist(
521  const PointT& point_a, const PointT& point_b) const
522 {
523  return (point_a.getVector3fMap() - point_b.getVector3fMap()).squaredNorm();
524 }
525 
526 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
527 void
528 OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::boxSearchRecursive(
529  const Eigen::Vector3f& min_pt,
530  const Eigen::Vector3f& max_pt,
531  const BranchNode* node,
532  const OctreeKey& key,
533  uindex_t tree_depth,
534  Indices& k_indices) const
535 {
536  // iterate over all children
537  for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
538 
539  const OctreeNode* child_node;
540  child_node = this->getBranchChildPtr(*node, child_idx);
541 
542  if (!child_node)
543  continue;
544 
545  OctreeKey new_key;
546  // generate new key for current branch voxel
547  new_key.x = (key.x << 1) + (!!(child_idx & (1 << 2)));
548  new_key.y = (key.y << 1) + (!!(child_idx & (1 << 1)));
549  new_key.z = (key.z << 1) + (!!(child_idx & (1 << 0)));
550 
551  // voxel corners
552  Eigen::Vector3f lower_voxel_corner;
553  Eigen::Vector3f upper_voxel_corner;
554  // get voxel coordinates
555  this->genVoxelBoundsFromOctreeKey(
556  new_key, tree_depth, lower_voxel_corner, upper_voxel_corner);
557 
558  // test if search region overlap with voxel space
559 
560  if (!((lower_voxel_corner(0) > max_pt(0)) || (min_pt(0) > upper_voxel_corner(0)) ||
561  (lower_voxel_corner(1) > max_pt(1)) || (min_pt(1) > upper_voxel_corner(1)) ||
562  (lower_voxel_corner(2) > max_pt(2)) || (min_pt(2) > upper_voxel_corner(2)))) {
563 
564  if (child_node->getNodeType() == BRANCH_NODE) {
565  // we have not reached maximum tree depth
566  boxSearchRecursive(min_pt,
567  max_pt,
568  static_cast<const BranchNode*>(child_node),
569  new_key,
570  tree_depth + 1,
571  k_indices);
572  }
573  else {
574  // we reached leaf node level
575  Indices decoded_point_vector;
576 
577  const auto* child_leaf = static_cast<const LeafNode*>(child_node);
578 
579  // decode leaf node into decoded_point_vector
580  (**child_leaf).getPointIndices(decoded_point_vector);
581 
582  // Linearly iterate over all decoded (unsorted) points
583  for (const auto& index : decoded_point_vector) {
584  const PointT& candidate_point = this->getPointByIndex(index);
585 
586  // check if point falls within search box
587  bool bInBox =
588  ((candidate_point.x >= min_pt(0)) && (candidate_point.x <= max_pt(0)) &&
589  (candidate_point.y >= min_pt(1)) && (candidate_point.y <= max_pt(1)) &&
590  (candidate_point.z >= min_pt(2)) && (candidate_point.z <= max_pt(2)));
591 
592  if (bInBox)
593  // add to result vector
594  k_indices.push_back(index);
595  }
596  }
597  }
598  }
599 }
600 
601 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
602 uindex_t
603 OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
604  getIntersectedVoxelCenters(Eigen::Vector3f origin,
605  Eigen::Vector3f direction,
606  AlignedPointTVector& voxel_center_list,
607  uindex_t max_voxel_count) const
608 {
609  OctreeKey key;
610  key.x = key.y = key.z = 0;
611 
612  voxel_center_list.clear();
613 
614  // Voxel child_idx remapping
615  unsigned char a = 0;
616 
617  double min_x, min_y, min_z, max_x, max_y, max_z;
618 
619  initIntersectedVoxel(origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);
620 
621  if (std::max(std::max(min_x, min_y), min_z) < std::min(std::min(max_x, max_y), max_z))
622  return getIntersectedVoxelCentersRecursive(min_x,
623  min_y,
624  min_z,
625  max_x,
626  max_y,
627  max_z,
628  a,
629  this->root_node_,
630  key,
631  voxel_center_list,
632  max_voxel_count);
633 
634  return (0);
635 }
636 
637 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
638 uindex_t
639 OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
640  getIntersectedVoxelIndices(Eigen::Vector3f origin,
641  Eigen::Vector3f direction,
642  Indices& k_indices,
643  uindex_t max_voxel_count) const
644 {
645  OctreeKey key;
646  key.x = key.y = key.z = 0;
647 
648  k_indices.clear();
649 
650  // Voxel child_idx remapping
651  unsigned char a = 0;
652  double min_x, min_y, min_z, max_x, max_y, max_z;
653 
654  initIntersectedVoxel(origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);
655 
656  if (std::max(std::max(min_x, min_y), min_z) < std::min(std::min(max_x, max_y), max_z))
657  return getIntersectedVoxelIndicesRecursive(min_x,
658  min_y,
659  min_z,
660  max_x,
661  max_y,
662  max_z,
663  a,
664  this->root_node_,
665  key,
666  k_indices,
667  max_voxel_count);
668  return (0);
669 }
670 
671 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
672 uindex_t
673 OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
674  getIntersectedVoxelCentersRecursive(double min_x,
675  double min_y,
676  double min_z,
677  double max_x,
678  double max_y,
679  double max_z,
680  unsigned char a,
681  const OctreeNode* node,
682  const OctreeKey& key,
683  AlignedPointTVector& voxel_center_list,
684  uindex_t max_voxel_count) const
685 {
686  if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0)
687  return (0);
688 
689  // If leaf node, get voxel center and increment intersection count
690  if (node->getNodeType() == LEAF_NODE) {
691  PointT newPoint;
692 
693  this->genLeafNodeCenterFromOctreeKey(key, newPoint);
694 
695  voxel_center_list.push_back(newPoint);
696 
697  return (1);
698  }
699 
700  // Voxel intersection count for branches children
701  uindex_t voxel_count = 0;
702 
703  // Voxel mid lines
704  double mid_x = 0.5 * (min_x + max_x);
705  double mid_y = 0.5 * (min_y + max_y);
706  double mid_z = 0.5 * (min_z + max_z);
707 
708  // First voxel node ray will intersect
709  auto curr_node = getFirstIntersectedNode(min_x, min_y, min_z, mid_x, mid_y, mid_z);
710 
711  // Child index, node and key
712  unsigned char child_idx;
713  OctreeKey child_key;
714 
715  do {
716  if (curr_node != 0)
717  child_idx = static_cast<unsigned char>(curr_node ^ a);
718  else
719  child_idx = a;
720 
721  // child_node == 0 if child_node doesn't exist
722  const OctreeNode* child_node =
723  this->getBranchChildPtr(static_cast<const BranchNode&>(*node), child_idx);
724 
725  // Generate new key for current branch voxel
726  child_key.x = (key.x << 1) | (!!(child_idx & (1 << 2)));
727  child_key.y = (key.y << 1) | (!!(child_idx & (1 << 1)));
728  child_key.z = (key.z << 1) | (!!(child_idx & (1 << 0)));
729 
730  // Recursively call each intersected child node, selecting the next
731  // node intersected by the ray. Children that do not intersect will
732  // not be traversed.
733 
734  switch (curr_node) {
735  case 0:
736  if (child_node)
737  voxel_count += getIntersectedVoxelCentersRecursive(min_x,
738  min_y,
739  min_z,
740  mid_x,
741  mid_y,
742  mid_z,
743  a,
744  child_node,
745  child_key,
746  voxel_center_list,
747  max_voxel_count);
748  curr_node = getNextIntersectedNode(mid_x, mid_y, mid_z, 4, 2, 1);
749  break;
750 
751  case 1:
752  if (child_node)
753  voxel_count += getIntersectedVoxelCentersRecursive(min_x,
754  min_y,
755  mid_z,
756  mid_x,
757  mid_y,
758  max_z,
759  a,
760  child_node,
761  child_key,
762  voxel_center_list,
763  max_voxel_count);
764  curr_node = getNextIntersectedNode(mid_x, mid_y, max_z, 5, 3, 8);
765  break;
766 
767  case 2:
768  if (child_node)
769  voxel_count += getIntersectedVoxelCentersRecursive(min_x,
770  mid_y,
771  min_z,
772  mid_x,
773  max_y,
774  mid_z,
775  a,
776  child_node,
777  child_key,
778  voxel_center_list,
779  max_voxel_count);
780  curr_node = getNextIntersectedNode(mid_x, max_y, mid_z, 6, 8, 3);
781  break;
782 
783  case 3:
784  if (child_node)
785  voxel_count += getIntersectedVoxelCentersRecursive(min_x,
786  mid_y,
787  mid_z,
788  mid_x,
789  max_y,
790  max_z,
791  a,
792  child_node,
793  child_key,
794  voxel_center_list,
795  max_voxel_count);
796  curr_node = getNextIntersectedNode(mid_x, max_y, max_z, 7, 8, 8);
797  break;
798 
799  case 4:
800  if (child_node)
801  voxel_count += getIntersectedVoxelCentersRecursive(mid_x,
802  min_y,
803  min_z,
804  max_x,
805  mid_y,
806  mid_z,
807  a,
808  child_node,
809  child_key,
810  voxel_center_list,
811  max_voxel_count);
812  curr_node = getNextIntersectedNode(max_x, mid_y, mid_z, 8, 6, 5);
813  break;
814 
815  case 5:
816  if (child_node)
817  voxel_count += getIntersectedVoxelCentersRecursive(mid_x,
818  min_y,
819  mid_z,
820  max_x,
821  mid_y,
822  max_z,
823  a,
824  child_node,
825  child_key,
826  voxel_center_list,
827  max_voxel_count);
828  curr_node = getNextIntersectedNode(max_x, mid_y, max_z, 8, 7, 8);
829  break;
830 
831  case 6:
832  if (child_node)
833  voxel_count += getIntersectedVoxelCentersRecursive(mid_x,
834  mid_y,
835  min_z,
836  max_x,
837  max_y,
838  mid_z,
839  a,
840  child_node,
841  child_key,
842  voxel_center_list,
843  max_voxel_count);
844  curr_node = getNextIntersectedNode(max_x, max_y, mid_z, 8, 8, 7);
845  break;
846 
847  case 7:
848  if (child_node)
849  voxel_count += getIntersectedVoxelCentersRecursive(mid_x,
850  mid_y,
851  mid_z,
852  max_x,
853  max_y,
854  max_z,
855  a,
856  child_node,
857  child_key,
858  voxel_center_list,
859  max_voxel_count);
860  curr_node = 8;
861  break;
862  }
863  } while ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count));
864  return (voxel_count);
865 }
866 
867 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
868 uindex_t
869 OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
870  getIntersectedVoxelIndicesRecursive(double min_x,
871  double min_y,
872  double min_z,
873  double max_x,
874  double max_y,
875  double max_z,
876  unsigned char a,
877  const OctreeNode* node,
878  const OctreeKey& key,
879  Indices& k_indices,
880  uindex_t max_voxel_count) const
881 {
882  if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0)
883  return (0);
884 
885  // If leaf node, get voxel center and increment intersection count
886  if (node->getNodeType() == LEAF_NODE) {
887  const auto* leaf = static_cast<const LeafNode*>(node);
888 
889  // decode leaf node into k_indices
890  (*leaf)->getPointIndices(k_indices);
891 
892  return (1);
893  }
894 
895  // Voxel intersection count for branches children
896  uindex_t voxel_count = 0;
897 
898  // Voxel mid lines
899  double mid_x = 0.5 * (min_x + max_x);
900  double mid_y = 0.5 * (min_y + max_y);
901  double mid_z = 0.5 * (min_z + max_z);
902 
903  // First voxel node ray will intersect
904  auto curr_node = getFirstIntersectedNode(min_x, min_y, min_z, mid_x, mid_y, mid_z);
905 
906  // Child index, node and key
907  unsigned char child_idx;
908  OctreeKey child_key;
909  do {
910  if (curr_node != 0)
911  child_idx = static_cast<unsigned char>(curr_node ^ a);
912  else
913  child_idx = a;
914 
915  // child_node == 0 if child_node doesn't exist
916  const OctreeNode* child_node =
917  this->getBranchChildPtr(static_cast<const BranchNode&>(*node), child_idx);
918  // Generate new key for current branch voxel
919  child_key.x = (key.x << 1) | (!!(child_idx & (1 << 2)));
920  child_key.y = (key.y << 1) | (!!(child_idx & (1 << 1)));
921  child_key.z = (key.z << 1) | (!!(child_idx & (1 << 0)));
922 
923  // Recursively call each intersected child node, selecting the next
924  // node intersected by the ray. Children that do not intersect will
925  // not be traversed.
926  switch (curr_node) {
927  case 0:
928  if (child_node)
929  voxel_count += getIntersectedVoxelIndicesRecursive(min_x,
930  min_y,
931  min_z,
932  mid_x,
933  mid_y,
934  mid_z,
935  a,
936  child_node,
937  child_key,
938  k_indices,
939  max_voxel_count);
940  curr_node = getNextIntersectedNode(mid_x, mid_y, mid_z, 4, 2, 1);
941  break;
942 
943  case 1:
944  if (child_node)
945  voxel_count += getIntersectedVoxelIndicesRecursive(min_x,
946  min_y,
947  mid_z,
948  mid_x,
949  mid_y,
950  max_z,
951  a,
952  child_node,
953  child_key,
954  k_indices,
955  max_voxel_count);
956  curr_node = getNextIntersectedNode(mid_x, mid_y, max_z, 5, 3, 8);
957  break;
958 
959  case 2:
960  if (child_node)
961  voxel_count += getIntersectedVoxelIndicesRecursive(min_x,
962  mid_y,
963  min_z,
964  mid_x,
965  max_y,
966  mid_z,
967  a,
968  child_node,
969  child_key,
970  k_indices,
971  max_voxel_count);
972  curr_node = getNextIntersectedNode(mid_x, max_y, mid_z, 6, 8, 3);
973  break;
974 
975  case 3:
976  if (child_node)
977  voxel_count += getIntersectedVoxelIndicesRecursive(min_x,
978  mid_y,
979  mid_z,
980  mid_x,
981  max_y,
982  max_z,
983  a,
984  child_node,
985  child_key,
986  k_indices,
987  max_voxel_count);
988  curr_node = getNextIntersectedNode(mid_x, max_y, max_z, 7, 8, 8);
989  break;
990 
991  case 4:
992  if (child_node)
993  voxel_count += getIntersectedVoxelIndicesRecursive(mid_x,
994  min_y,
995  min_z,
996  max_x,
997  mid_y,
998  mid_z,
999  a,
1000  child_node,
1001  child_key,
1002  k_indices,
1003  max_voxel_count);
1004  curr_node = getNextIntersectedNode(max_x, mid_y, mid_z, 8, 6, 5);
1005  break;
1006 
1007  case 5:
1008  if (child_node)
1009  voxel_count += getIntersectedVoxelIndicesRecursive(mid_x,
1010  min_y,
1011  mid_z,
1012  max_x,
1013  mid_y,
1014  max_z,
1015  a,
1016  child_node,
1017  child_key,
1018  k_indices,
1019  max_voxel_count);
1020  curr_node = getNextIntersectedNode(max_x, mid_y, max_z, 8, 7, 8);
1021  break;
1022 
1023  case 6:
1024  if (child_node)
1025  voxel_count += getIntersectedVoxelIndicesRecursive(mid_x,
1026  mid_y,
1027  min_z,
1028  max_x,
1029  max_y,
1030  mid_z,
1031  a,
1032  child_node,
1033  child_key,
1034  k_indices,
1035  max_voxel_count);
1036  curr_node = getNextIntersectedNode(max_x, max_y, mid_z, 8, 8, 7);
1037  break;
1038 
1039  case 7:
1040  if (child_node)
1041  voxel_count += getIntersectedVoxelIndicesRecursive(mid_x,
1042  mid_y,
1043  mid_z,
1044  max_x,
1045  max_y,
1046  max_z,
1047  a,
1048  child_node,
1049  child_key,
1050  k_indices,
1051  max_voxel_count);
1052  curr_node = 8;
1053  break;
1054  }
1055  } while ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count));
1056 
1057  return (voxel_count);
1058 }
1059 
1060 } // namespace octree
1061 } // namespace pcl
1062 
1063 #define PCL_INSTANTIATE_OctreePointCloudSearch(T) \
1064  template class PCL_EXPORTS pcl::octree::OctreePointCloudSearch<T>;
1065 
1066 #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:640
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:870
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:604
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:674
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:432
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:528
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:520
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