Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
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
44namespace pcl {
45
46namespace octree {
47
48template <typename PointT, typename LeafContainerT, typename BranchContainerT>
49bool
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
71template <typename PointT, typename LeafContainerT, typename BranchContainerT>
72bool
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
80template <typename PointT, typename LeafContainerT, typename BranchContainerT>
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();
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;
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 const auto result_count = static_cast<uindex_t>(point_candidates.size());
111
112 k_indices.resize(result_count);
113 k_sqr_distances.resize(result_count);
114
115 for (uindex_t 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 k_indices.size();
121}
122
123template <typename PointT, typename LeafContainerT, typename BranchContainerT>
126 uindex_t index, uindex_t k, Indices& 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
132template <typename PointT, typename LeafContainerT, typename BranchContainerT>
133void
135 const PointT& p_q, index_t& 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
150template <typename PointT, typename LeafContainerT, typename BranchContainerT>
151void
153 uindex_t query_index, index_t& 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
160template <typename PointT, typename LeafContainerT, typename BranchContainerT>
163 const PointT& p_q,
164 const double radius,
165 Indices& k_indices,
166 std::vector<float>& k_sqr_distances,
167 uindex_t 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 k_indices.size();
187}
188
189template <typename PointT, typename LeafContainerT, typename BranchContainerT>
192 uindex_t index,
193 const double radius,
194 Indices& k_indices,
195 std::vector<float>& k_sqr_distances,
196 uindex_t 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
203template <typename PointT, typename LeafContainerT, typename BranchContainerT>
206 const Eigen::Vector3f& min_pt,
207 const Eigen::Vector3f& max_pt,
208 Indices& 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 k_indices.size();
219}
220
221template <typename PointT, typename LeafContainerT, typename BranchContainerT>
222double
225 const PointT& point,
226 uindex_t K,
227 const BranchNode* node,
228 const OctreeKey& key,
229 uindex_t 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 (child_node->getNodeType() == BRANCH_NODE) {
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 Indices decoded_point_vector;
294
295 const auto* 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 auto& 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
333template <typename PointT, typename LeafContainerT, typename BranchContainerT>
334void
337 const double radiusSquared,
338 const BranchNode* node,
339 const OctreeKey& key,
340 uindex_t tree_depth,
341 Indices& k_indices,
342 std::vector<float>& k_sqr_distances,
343 uindex_t 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 (child_node->getNodeType() == BRANCH_NODE) {
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() == max_nn)
387 return;
388 }
389 else {
390 // we reached leaf node level
391 const auto* child_leaf = static_cast<const LeafNode*>(child_node);
392 Indices 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 auto& 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() == max_nn)
413 return;
414 }
415 }
416 }
417 }
418}
419
420template <typename PointT, typename LeafContainerT, typename BranchContainerT>
421void
424 const BranchNode* node,
425 const OctreeKey& key,
426 uindex_t tree_depth,
427 index_t& result_index,
428 float& sqr_distance)
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 (child_node->getNodeType() == BRANCH_NODE) {
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 Indices decoded_point_vector;
483
484 const auto* child_leaf = static_cast<const LeafNode*>(child_node);
485
486 float smallest_squared_dist = std::numeric_limits<float>::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 auto& index : decoded_point_vector) {
493 const PointT& candidate_point = this->getPointByIndex(index);
494
495 // calculate point distance to search point
496 float 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 = squared_dist;
505 }
506 }
507}
508
509template <typename PointT, typename LeafContainerT, typename BranchContainerT>
510float
512 const PointT& point_a, const PointT& point_b) const
513{
514 return (point_a.getVector3fMap() - point_b.getVector3fMap()).squaredNorm();
515}
516
517template <typename PointT, typename LeafContainerT, typename BranchContainerT>
518void
520 const Eigen::Vector3f& min_pt,
521 const Eigen::Vector3f& max_pt,
522 const BranchNode* node,
523 const OctreeKey& key,
524 uindex_t tree_depth,
525 Indices& 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 (child_node->getNodeType() == BRANCH_NODE) {
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 Indices decoded_point_vector;
567
568 const auto* 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 auto& 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
592template <typename PointT, typename LeafContainerT, typename BranchContainerT>
595 getIntersectedVoxelCenters(Eigen::Vector3f origin,
596 Eigen::Vector3f direction,
597 AlignedPointTVector& voxel_center_list,
598 uindex_t 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
628template <typename PointT, typename LeafContainerT, typename BranchContainerT>
631 getIntersectedVoxelIndices(Eigen::Vector3f origin,
632 Eigen::Vector3f direction,
633 Indices& k_indices,
634 uindex_t 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
662template <typename PointT, typename LeafContainerT, typename BranchContainerT>
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 uindex_t 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 uindex_t 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 auto 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
858template <typename PointT, typename LeafContainerT, typename BranchContainerT>
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 Indices& k_indices,
871 uindex_t 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 auto* 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 uindex_t 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 auto 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_
Octree key class
Definition octree_key.h:54
Abstract octree node class
virtual node_type_t getNodeType() const =0
Pure virtual method for retrieving the type of octree node (branch or leaf)
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.
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).
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.
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).
bool voxelSearch(const PointT &point, Indices &point_idx_data)
Search for neighbors within a voxel at given point.
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 ...
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.
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.
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.
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.
float pointSquaredDist(const PointT &point_a, const PointT &point_b) const
Helper function to calculate the squared distance between two points.
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.
typename OctreeT::BranchNode BranchNode
void approxNearestSearch(const PointCloud &cloud, uindex_t query_index, index_t &result_index, float &sqr_distance)
Search for approx.
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.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
@ K
Definition norms.h:54
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
Definition types.h:120
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
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
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
A point structure representing Euclidean xyz coordinates, and the RGB color.