Point Cloud Library (PCL) 1.15.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
50OctreePointCloudSearch<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
71template <typename PointT, typename LeafContainerT, typename BranchContainerT>
72bool
73OctreePointCloudSearch<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
80template <typename PointT, typename LeafContainerT, typename BranchContainerT>
81uindex_t
82OctreePointCloudSearch<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
124template <typename PointT, typename LeafContainerT, typename BranchContainerT>
125uindex_t
126OctreePointCloudSearch<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
133template <typename PointT, typename LeafContainerT, typename BranchContainerT>
134void
135OctreePointCloudSearch<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
151template <typename PointT, typename LeafContainerT, typename BranchContainerT>
152void
153OctreePointCloudSearch<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
161template <typename PointT, typename LeafContainerT, typename BranchContainerT>
162uindex_t
163OctreePointCloudSearch<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
190template <typename PointT, typename LeafContainerT, typename BranchContainerT>
191uindex_t
192OctreePointCloudSearch<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
204template <typename PointT, typename LeafContainerT, typename BranchContainerT>
205uindex_t
206OctreePointCloudSearch<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
222template <typename PointT, typename LeafContainerT, typename BranchContainerT>
223double
224OctreePointCloudSearch<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
339template <typename PointT, typename LeafContainerT, typename BranchContainerT>
340void
341OctreePointCloudSearch<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
429template <typename PointT, typename LeafContainerT, typename BranchContainerT>
430void
431OctreePointCloudSearch<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
518template <typename PointT, typename LeafContainerT, typename BranchContainerT>
519float
520OctreePointCloudSearch<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
526template <typename PointT, typename LeafContainerT, typename BranchContainerT>
527void
528OctreePointCloudSearch<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
601template <typename PointT, typename LeafContainerT, typename BranchContainerT>
602uindex_t
603OctreePointCloudSearch<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
637template <typename PointT, typename LeafContainerT, typename BranchContainerT>
638uindex_t
639OctreePointCloudSearch<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
671template <typename PointT, typename LeafContainerT, typename BranchContainerT>
672uindex_t
673OctreePointCloudSearch<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
867template <typename PointT, typename LeafContainerT, typename BranchContainerT>
868uindex_t
869OctreePointCloudSearch<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::OctreeBase::leaf_count_
std::size_t leaf_count_
Amount of leaf nodes.
Definition octree_base.h:78
pcl::octree::OctreeBase::root_node_
BranchNode * root_node_
Pointer to root branch node of octree.
Definition octree_base.h:84
pcl::octree::OctreeBase::branchHasChild
bool branchHasChild(const BranchNode &branch_arg, unsigned char child_idx_arg) const
Check if branch is pointing to a particular child node.
Definition octree_base.h:542
pcl::octree::OctreeBase::findLeaf
LeafContainerT * findLeaf(uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg) const
Find leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
Definition octree_base.hpp:115
pcl::octree::OctreeBase::getBranchChildPtr
OctreeNode * getBranchChildPtr(const BranchNode &branch_arg, unsigned char child_idx_arg) const
Retrieve a child node pointer for child node at child_idx.
Definition octree_base.h:554
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::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty >::genVoxelCenterFromOctreeKey
void genVoxelCenterFromOctreeKey(const OctreeKey &key_arg, uindex_t tree_depth_arg, PointT &point_arg) const
Definition octree_pointcloud.hpp:842
pcl::octree::OctreePointCloud< PointT, pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty >::genVoxelBoundsFromOctreeKey
void genVoxelBoundsFromOctreeKey(const OctreeKey &key_arg, uindex_t tree_depth_arg, Eigen::Vector3f &min_pt, Eigen::Vector3f &max_pt) const
Definition octree_pointcloud.hpp:871
pcl::octree::OctreePointCloudSearch::LeafNode
typename OctreeT::LeafNode LeafNode
Definition octree_search.h:78
pcl::octree::OctreePointCloudSearch< PointT, pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty >::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
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::getNextIntersectedNode
int getNextIntersectedNode(double x, double y, double z, int a, int b, int c) const
Get the next visited node given the current node upper bounding box corner.
Definition octree_search.h:641
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::AlignedPointTVector
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
Definition octree_search.h:75
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::initIntersectedVoxel
void initIntersectedVoxel(Eigen::Vector3f &origin, Eigen::Vector3f &direction, double &min_x, double &min_y, double &min_z, double &max_x, double &max_y, double &max_z, unsigned char &a) const
Initialize raytracing algorithm.
Definition octree_search.h:524
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::getFirstIntersectedNode
int getFirstIntersectedNode(double min_x, double min_y, double min_z, double mid_x, double mid_y, double mid_z) const
Find first child node ray will enter.
Definition octree_search.h:583
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::K
@ K
Definition norms.h:54
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::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