Point Cloud Library (PCL) 1.13.1
Loading...
Searching...
No Matches
octree_pointcloud.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2012, 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#pragma once
40
41#include <pcl/common/common.h>
42#include <pcl/common/point_tests.h> // for pcl::isFinite
43#include <pcl/octree/impl/octree_base.hpp>
44#include <pcl/types.h>
45
46#include <cassert>
47
48//////////////////////////////////////////////////////////////////////////////////////////////
49template <typename PointT,
50 typename LeafContainerT,
51 typename BranchContainerT,
52 typename OctreeT>
54 OctreePointCloud(const double resolution)
55: OctreeT()
56, input_(PointCloudConstPtr())
57, indices_(IndicesConstPtr())
58, epsilon_(0)
59, resolution_(resolution)
60, min_x_(0.0f)
61, max_x_(resolution)
62, min_y_(0.0f)
63, max_y_(resolution)
64, min_z_(0.0f)
65, max_z_(resolution)
66, bounding_box_defined_(false)
67, max_objs_per_leaf_(0)
68{
69 if (resolution <= 0.0) {
70 PCL_THROW_EXCEPTION(InitFailedException,
71 "[pcl::octree::OctreePointCloud::OctreePointCloud] Resolution "
72 << resolution << " must be > 0!");
73 }
74}
75
76//////////////////////////////////////////////////////////////////////////////////////////////
77template <typename PointT,
78 typename LeafContainerT,
79 typename BranchContainerT,
80 typename OctreeT>
81void
84{
85 if (indices_) {
86 for (const auto& index : *indices_) {
87 assert((index >= 0) && (static_cast<std::size_t>(index) < input_->size()));
88
89 if (isFinite((*input_)[index])) {
90 // add points to octree
91 this->addPointIdx(index);
92 }
93 }
94 }
95 else {
96 for (index_t i = 0; i < static_cast<index_t>(input_->size()); i++) {
97 if (isFinite((*input_)[i])) {
98 // add points to octree
99 this->addPointIdx(i);
100 }
101 }
102 }
103}
104
105//////////////////////////////////////////////////////////////////////////////////////////////
106template <typename PointT,
107 typename LeafContainerT,
108 typename BranchContainerT,
109 typename OctreeT>
110void
112 addPointFromCloud(const uindex_t point_idx_arg, IndicesPtr indices_arg)
113{
114 this->addPointIdx(point_idx_arg);
115 if (indices_arg)
116 indices_arg->push_back(point_idx_arg);
117}
118
119//////////////////////////////////////////////////////////////////////////////////////////////
120template <typename PointT,
121 typename LeafContainerT,
122 typename BranchContainerT,
123 typename OctreeT>
124void
126 addPointToCloud(const PointT& point_arg, PointCloudPtr cloud_arg)
127{
128 assert(cloud_arg == input_);
129
130 cloud_arg->push_back(point_arg);
131
132 this->addPointIdx(cloud_arg->size() - 1);
133}
134
135//////////////////////////////////////////////////////////////////////////////////////////////
136template <typename PointT,
137 typename LeafContainerT,
138 typename BranchContainerT,
139 typename OctreeT>
140void
142 addPointToCloud(const PointT& point_arg,
143 PointCloudPtr cloud_arg,
144 IndicesPtr indices_arg)
145{
146 assert(cloud_arg == input_);
147 assert(indices_arg == indices_);
148
149 cloud_arg->push_back(point_arg);
150
151 this->addPointFromCloud(cloud_arg->size() - 1, indices_arg);
152}
153
154//////////////////////////////////////////////////////////////////////////////////////////////
155template <typename PointT,
156 typename LeafContainerT,
157 typename BranchContainerT,
158 typename OctreeT>
159bool
161 isVoxelOccupiedAtPoint(const PointT& point_arg) const
162{
163 if (!isPointWithinBoundingBox(point_arg)) {
164 return false;
165 }
166
167 OctreeKey key;
168
169 // generate key for point
170 this->genOctreeKeyforPoint(point_arg, key);
171
172 // search for key in octree
173 return (this->existLeaf(key));
174}
175
176//////////////////////////////////////////////////////////////////////////////////////////////
177template <typename PointT,
178 typename LeafContainerT,
179 typename BranchContainerT,
180 typename OctreeT>
181bool
183 isVoxelOccupiedAtPoint(const index_t& point_idx_arg) const
184{
185 // retrieve point from input cloud
186 const PointT& point = (*this->input_)[point_idx_arg];
187
188 // search for voxel at point in octree
189 return (this->isVoxelOccupiedAtPoint(point));
190}
191
192//////////////////////////////////////////////////////////////////////////////////////////////
193template <typename PointT,
194 typename LeafContainerT,
195 typename BranchContainerT,
196 typename OctreeT>
197bool
199 isVoxelOccupiedAtPoint(const double point_x_arg,
200 const double point_y_arg,
201 const double point_z_arg) const
202{
203 // create a new point with the argument coordinates
204 PointT point;
205 point.x = point_x_arg;
206 point.y = point_y_arg;
207 point.z = point_z_arg;
208
209 // search for voxel at point in octree
210 return (this->isVoxelOccupiedAtPoint(point));
211}
212
213//////////////////////////////////////////////////////////////////////////////////////////////
214template <typename PointT,
215 typename LeafContainerT,
216 typename BranchContainerT,
217 typename OctreeT>
218void
220 deleteVoxelAtPoint(const PointT& point_arg)
221{
222 if (!isPointWithinBoundingBox(point_arg)) {
223 return;
224 }
225
227
228 // generate key for point
229 this->genOctreeKeyforPoint(point_arg, key);
230
231 this->removeLeaf(key);
232}
233
234//////////////////////////////////////////////////////////////////////////////////////////////
235template <typename PointT,
236 typename LeafContainerT,
237 typename BranchContainerT,
238 typename OctreeT>
239void
241 deleteVoxelAtPoint(const index_t& point_idx_arg)
242{
243 // retrieve point from input cloud
244 const PointT& point = (*this->input_)[point_idx_arg];
245
246 // delete leaf at point
247 this->deleteVoxelAtPoint(point);
248}
249
250//////////////////////////////////////////////////////////////////////////////////////////////
251template <typename PointT,
252 typename LeafContainerT,
253 typename BranchContainerT,
254 typename OctreeT>
257 getOccupiedVoxelCenters(AlignedPointTVector& voxel_center_list_arg) const
258{
259 OctreeKey key;
260 key.x = key.y = key.z = 0;
261
262 voxel_center_list_arg.clear();
263
264 return getOccupiedVoxelCentersRecursive(this->root_node_, key, voxel_center_list_arg);
266
267//////////////////////////////////////////////////////////////////////////////////////////////
268template <typename PointT,
269 typename LeafContainerT,
270 typename BranchContainerT,
271 typename OctreeT>
274 getApproxIntersectedVoxelCentersBySegment(const Eigen::Vector3f& origin,
275 const Eigen::Vector3f& end,
276 AlignedPointTVector& voxel_center_list,
277 float precision)
278{
279 Eigen::Vector3f direction = end - origin;
280 float norm = direction.norm();
281 direction.normalize();
282
283 const float step_size = static_cast<float>(resolution_) * precision;
284 // Ensure we get at least one step for the first voxel.
285 const auto nsteps = std::max<std::size_t>(1, norm / step_size);
286
287 OctreeKey prev_key;
288
289 bool bkeyDefined = false;
290
291 // Walk along the line segment with small steps.
292 for (std::size_t i = 0; i < nsteps; ++i) {
293 Eigen::Vector3f p = origin + (direction * step_size * static_cast<float>(i));
294
295 PointT octree_p;
296 octree_p.x = p.x();
297 octree_p.y = p.y();
298 octree_p.z = p.z();
299
300 OctreeKey key;
301 this->genOctreeKeyforPoint(octree_p, key);
303 // Not a new key, still the same voxel.
304 if ((key == prev_key) && (bkeyDefined))
305 continue;
306
307 prev_key = key;
308 bkeyDefined = true;
309
310 PointT center;
311 genLeafNodeCenterFromOctreeKey(key, center);
312 voxel_center_list.push_back(center);
313 }
314
315 OctreeKey end_key;
316 PointT end_p;
317 end_p.x = end.x();
318 end_p.y = end.y();
319 end_p.z = end.z();
320 this->genOctreeKeyforPoint(end_p, end_key);
321 if (!(end_key == prev_key)) {
322 PointT center;
323 genLeafNodeCenterFromOctreeKey(end_key, center);
324 voxel_center_list.push_back(center);
325 }
326
327 return (static_cast<uindex_t>(voxel_center_list.size()));
328}
329
330//////////////////////////////////////////////////////////////////////////////////////////////
331template <typename PointT,
332 typename LeafContainerT,
333 typename BranchContainerT,
334 typename OctreeT>
335void
339
340 double minX, minY, minZ, maxX, maxY, maxZ;
341
342 PointT min_pt;
343 PointT max_pt;
344
345 // bounding box cannot be changed once the octree contains elements
346 if (this->leaf_count_ != 0) {
347 PCL_ERROR("[pcl::octree::OctreePointCloud::defineBoundingBox] Leaf count (%lu) "
348 "must be 0\n",
349 this->leaf_count_);
350 return;
351 }
352
353 pcl::getMinMax3D(*input_, min_pt, max_pt);
354
355 float minValue = std::numeric_limits<float>::epsilon() * 512.0f;
356
357 minX = min_pt.x;
358 minY = min_pt.y;
359 minZ = min_pt.z;
361 maxX = max_pt.x + minValue;
362 maxY = max_pt.y + minValue;
363 maxZ = max_pt.z + minValue;
364
365 // generate bit masks for octree
366 defineBoundingBox(minX, minY, minZ, maxX, maxY, maxZ);
367}
368
369//////////////////////////////////////////////////////////////////////////////////////////////
370template <typename PointT,
371 typename LeafContainerT,
372 typename BranchContainerT,
373 typename OctreeT>
374void
376 defineBoundingBox(const double min_x_arg,
377 const double min_y_arg,
378 const double min_z_arg,
379 const double max_x_arg,
380 const double max_y_arg,
381 const double max_z_arg)
382{
383 // bounding box cannot be changed once the octree contains elements
384 if (this->leaf_count_ != 0) {
385 PCL_ERROR("[pcl::octree::OctreePointCloud::defineBoundingBox] Leaf count (%lu) "
386 "must be 0\n",
387 this->leaf_count_);
388 return;
389 }
390
391 min_x_ = std::min(min_x_arg, max_x_arg);
392 min_y_ = std::min(min_y_arg, max_y_arg);
393 min_z_ = std::min(min_z_arg, max_z_arg);
394
395 max_x_ = std::max(min_x_arg, max_x_arg);
396 max_y_ = std::max(min_y_arg, max_y_arg);
397 max_z_ = std::max(min_z_arg, max_z_arg);
398
399 // generate bit masks for octree
400 getKeyBitSize();
401
402 bounding_box_defined_ = true;
403}
404
405//////////////////////////////////////////////////////////////////////////////////////////////
406template <typename PointT,
407 typename LeafContainerT,
408 typename BranchContainerT,
409 typename OctreeT>
410void
412 defineBoundingBox(const double max_x_arg,
413 const double max_y_arg,
414 const double max_z_arg)
415{
416 // bounding box cannot be changed once the octree contains elements
417 if (this->leaf_count_ != 0) {
418 PCL_ERROR("[pcl::octree::OctreePointCloud::defineBoundingBox] Leaf count (%lu) "
419 "must be 0\n",
420 this->leaf_count_);
421 return;
422 }
423
424 min_x_ = std::min(0.0, max_x_arg);
425 min_y_ = std::min(0.0, max_y_arg);
426 min_z_ = std::min(0.0, max_z_arg);
427
428 max_x_ = std::max(0.0, max_x_arg);
429 max_y_ = std::max(0.0, max_y_arg);
430 max_z_ = std::max(0.0, max_z_arg);
431
432 // generate bit masks for octree
433 getKeyBitSize();
434
435 bounding_box_defined_ = true;
436}
437
438//////////////////////////////////////////////////////////////////////////////////////////////
439template <typename PointT,
440 typename LeafContainerT,
441 typename BranchContainerT,
442 typename OctreeT>
443void
445 defineBoundingBox(const double cubeLen_arg)
446{
447 // bounding box cannot be changed once the octree contains elements
448 if (this->leaf_count_ != 0) {
449 PCL_ERROR("[pcl::octree::OctreePointCloud::defineBoundingBox] Leaf count (%lu) "
450 "must be 0\n",
451 this->leaf_count_);
452 return;
453 }
454
455 min_x_ = std::min(0.0, cubeLen_arg);
456 min_y_ = std::min(0.0, cubeLen_arg);
457 min_z_ = std::min(0.0, cubeLen_arg);
458
459 max_x_ = std::max(0.0, cubeLen_arg);
460 max_y_ = std::max(0.0, cubeLen_arg);
461 max_z_ = std::max(0.0, cubeLen_arg);
462
463 // generate bit masks for octree
464 getKeyBitSize();
465
466 bounding_box_defined_ = true;
467}
468
469//////////////////////////////////////////////////////////////////////////////////////////////
470template <typename PointT,
471 typename LeafContainerT,
472 typename BranchContainerT,
473 typename OctreeT>
474void
476 getBoundingBox(double& min_x_arg,
477 double& min_y_arg,
478 double& min_z_arg,
479 double& max_x_arg,
480 double& max_y_arg,
481 double& max_z_arg) const
482{
483 min_x_arg = min_x_;
484 min_y_arg = min_y_;
485 min_z_arg = min_z_;
486
487 max_x_arg = max_x_;
488 max_y_arg = max_y_;
489 max_z_arg = max_z_;
490}
491
492//////////////////////////////////////////////////////////////////////////////////////////////
493template <typename PointT,
494 typename LeafContainerT,
495 typename BranchContainerT,
496 typename OctreeT>
497void
499 adoptBoundingBoxToPoint(const PointT& point_arg)
500{
501
502 constexpr float minValue = std::numeric_limits<float>::epsilon();
503
504 // increase octree size until point fits into bounding box
505 while (true) {
506 bool bLowerBoundViolationX = (point_arg.x < min_x_);
507 bool bLowerBoundViolationY = (point_arg.y < min_y_);
508 bool bLowerBoundViolationZ = (point_arg.z < min_z_);
509
510 bool bUpperBoundViolationX = (point_arg.x >= max_x_);
511 bool bUpperBoundViolationY = (point_arg.y >= max_y_);
512 bool bUpperBoundViolationZ = (point_arg.z >= max_z_);
513
514 // do we violate any bounds?
515 if (bLowerBoundViolationX || bLowerBoundViolationY || bLowerBoundViolationZ ||
516 bUpperBoundViolationX || bUpperBoundViolationY || bUpperBoundViolationZ ||
517 (!bounding_box_defined_)) {
518
519 if (bounding_box_defined_) {
520
521 double octreeSideLen;
522 unsigned char child_idx;
523
524 // octree not empty - we add another tree level and thus increase its size by a
525 // factor of 2*2*2
526 child_idx = static_cast<unsigned char>(((!bUpperBoundViolationX) << 2) |
527 ((!bUpperBoundViolationY) << 1) |
528 ((!bUpperBoundViolationZ)));
529
530 BranchNode* newRootBranch;
532 newRootBranch = new BranchNode();
533 this->branch_count_++;
534
535 this->setBranchChildPtr(*newRootBranch, child_idx, this->root_node_);
536
537 this->root_node_ = newRootBranch;
539 octreeSideLen = static_cast<double>(1 << this->octree_depth_) * resolution_;
540
541 if (!bUpperBoundViolationX)
542 min_x_ -= octreeSideLen;
543
544 if (!bUpperBoundViolationY)
545 min_y_ -= octreeSideLen;
547 if (!bUpperBoundViolationZ)
548 min_z_ -= octreeSideLen;
549
550 // configure tree depth of octree
551 this->octree_depth_++;
552 this->setTreeDepth(this->octree_depth_);
553
554 // recalculate bounding box width
555 octreeSideLen =
556 static_cast<double>(1 << this->octree_depth_) * resolution_ - minValue;
557
558 // increase octree bounding box
559 max_x_ = min_x_ + octreeSideLen;
560 max_y_ = min_y_ + octreeSideLen;
561 max_z_ = min_z_ + octreeSideLen;
562 }
563 // bounding box is not defined - set it to point position
564 else {
565 // octree is empty - we set the center of the bounding box to our first pixel
566 this->min_x_ = point_arg.x - this->resolution_ / 2;
567 this->min_y_ = point_arg.y - this->resolution_ / 2;
568 this->min_z_ = point_arg.z - this->resolution_ / 2;
569
570 this->max_x_ = point_arg.x + this->resolution_ / 2;
571 this->max_y_ = point_arg.y + this->resolution_ / 2;
572 this->max_z_ = point_arg.z + this->resolution_ / 2;
573
574 getKeyBitSize();
575
576 bounding_box_defined_ = true;
577 }
578 }
579 else
580 // no bound violations anymore - leave while loop
581 break;
582 }
583}
584
585//////////////////////////////////////////////////////////////////////////////////////////////
586template <typename PointT,
587 typename LeafContainerT,
588 typename BranchContainerT,
589 typename OctreeT>
590void
592 expandLeafNode(LeafNode* leaf_node,
593 BranchNode* parent_branch,
594 unsigned char child_idx,
595 uindex_t depth_mask)
596{
597
598 if (depth_mask) {
599 // get amount of objects in leaf container
600 std::size_t leaf_obj_count = (*leaf_node)->getSize();
601
602 // copy leaf data
603 Indices leafIndices;
604 leafIndices.reserve(leaf_obj_count);
605
606 (*leaf_node)->getPointIndices(leafIndices);
607
608 // delete current leaf node
609 this->deleteBranchChild(*parent_branch, child_idx);
610 this->leaf_count_--;
611
612 // create new branch node
613 BranchNode* childBranch = this->createBranchChild(*parent_branch, child_idx);
614 this->branch_count_++;
615
616 // add data to new branch
617 OctreeKey new_index_key;
618
619 for (const auto& leafIndex : leafIndices) {
620
621 const PointT& point_from_index = (*input_)[leafIndex];
622 // generate key
623 genOctreeKeyforPoint(point_from_index, new_index_key);
624
625 LeafNode* newLeaf;
626 BranchNode* newBranchParent;
627 this->createLeafRecursive(
628 new_index_key, depth_mask, childBranch, newLeaf, newBranchParent);
629
630 (*newLeaf)->addPointIndex(leafIndex);
631 }
632 }
633}
634
635//////////////////////////////////////////////////////////////////////////////////////////////
636template <typename PointT,
637 typename LeafContainerT,
638 typename BranchContainerT,
639 typename OctreeT>
640void
642 addPointIdx(const uindex_t point_idx_arg)
643{
644 OctreeKey key;
645
646 assert(point_idx_arg < input_->size());
647
648 const PointT& point = (*input_)[point_idx_arg];
649
650 // make sure bounding box is big enough
651 adoptBoundingBoxToPoint(point);
652
653 // generate key
654 genOctreeKeyforPoint(point, key);
655
656 LeafNode* leaf_node;
657 BranchNode* parent_branch_of_leaf_node;
658 auto depth_mask = this->createLeafRecursive(
659 key, this->depth_mask_, this->root_node_, leaf_node, parent_branch_of_leaf_node);
660
661 if (this->dynamic_depth_enabled_ && depth_mask) {
662 // get amount of objects in leaf container
663 std::size_t leaf_obj_count = (*leaf_node)->getSize();
664
665 while (leaf_obj_count >= max_objs_per_leaf_ && depth_mask) {
666 // index to branch child
667 unsigned char child_idx = key.getChildIdxWithDepthMask(depth_mask * 2);
668
669 expandLeafNode(leaf_node, parent_branch_of_leaf_node, child_idx, depth_mask);
670
671 depth_mask = this->createLeafRecursive(key,
672 this->depth_mask_,
673 this->root_node_,
674 leaf_node,
675 parent_branch_of_leaf_node);
676 leaf_obj_count = (*leaf_node)->getSize();
677 }
678 }
679
680 (*leaf_node)->addPointIndex(point_idx_arg);
681}
682
683//////////////////////////////////////////////////////////////////////////////////////////////
684template <typename PointT,
685 typename LeafContainerT,
686 typename BranchContainerT,
687 typename OctreeT>
688const PointT&
690 getPointByIndex(const uindex_t index_arg) const
691{
692 // retrieve point from input cloud
693 assert(index_arg < input_->size());
694 return ((*this->input_)[index_arg]);
695}
696
697//////////////////////////////////////////////////////////////////////////////////////////////
698template <typename PointT,
699 typename LeafContainerT,
700 typename BranchContainerT,
701 typename OctreeT>
702void
705{
706 constexpr float minValue = std::numeric_limits<float>::epsilon();
707
708 // find maximum key values for x, y, z
709 const auto max_key_x =
710 static_cast<uindex_t>(std::ceil((max_x_ - min_x_ - minValue) / resolution_));
711 const auto max_key_y =
712 static_cast<uindex_t>(std::ceil((max_y_ - min_y_ - minValue) / resolution_));
713 const auto max_key_z =
714 static_cast<uindex_t>(std::ceil((max_z_ - min_z_ - minValue) / resolution_));
715
716 // find maximum amount of keys
717 const auto max_voxels =
718 std::max<uindex_t>(std::max(std::max(max_key_x, max_key_y), max_key_z), 2);
719
720 // tree depth == amount of bits of max_voxels
721 this->octree_depth_ = std::max<uindex_t>(
722 std::min<uindex_t>(OctreeKey::maxDepth,
723 std::ceil(std::log2(max_voxels) - minValue)),
724 0);
725
726 const auto octree_side_len =
727 static_cast<double>(1 << this->octree_depth_) * resolution_;
728
729 if (this->leaf_count_ == 0) {
730 double octree_oversize_x;
731 double octree_oversize_y;
732 double octree_oversize_z;
733
734 octree_oversize_x = (octree_side_len - (max_x_ - min_x_)) / 2.0;
735 octree_oversize_y = (octree_side_len - (max_y_ - min_y_)) / 2.0;
736 octree_oversize_z = (octree_side_len - (max_z_ - min_z_)) / 2.0;
737
738 assert(octree_oversize_x > -minValue);
739 assert(octree_oversize_y > -minValue);
740 assert(octree_oversize_z > -minValue);
741
742 if (octree_oversize_x > minValue) {
743 min_x_ -= octree_oversize_x;
744 max_x_ += octree_oversize_x;
745 }
746 if (octree_oversize_y > minValue) {
747 min_y_ -= octree_oversize_y;
748 max_y_ += octree_oversize_y;
749 }
750 if (octree_oversize_z > minValue) {
751 min_z_ -= octree_oversize_z;
752 max_z_ += octree_oversize_z;
753 }
754 }
755 else {
756 max_x_ = min_x_ + octree_side_len;
757 max_y_ = min_y_ + octree_side_len;
758 max_z_ = min_z_ + octree_side_len;
759 }
760
761 // configure tree depth of octree
762 this->setTreeDepth(this->octree_depth_);
763}
764
765//////////////////////////////////////////////////////////////////////////////////////////////
766template <typename PointT,
767 typename LeafContainerT,
768 typename BranchContainerT,
769 typename OctreeT>
770void
772 genOctreeKeyforPoint(const PointT& point_arg, OctreeKey& key_arg) const
773{
774 // calculate integer key for point coordinates
775 key_arg.x = static_cast<uindex_t>((point_arg.x - this->min_x_) / this->resolution_);
776 key_arg.y = static_cast<uindex_t>((point_arg.y - this->min_y_) / this->resolution_);
777 key_arg.z = static_cast<uindex_t>((point_arg.z - this->min_z_) / this->resolution_);
778
779 assert(key_arg.x <= this->max_key_.x);
780 assert(key_arg.y <= this->max_key_.y);
781 assert(key_arg.z <= this->max_key_.z);
782}
783
784//////////////////////////////////////////////////////////////////////////////////////////////
785template <typename PointT,
786 typename LeafContainerT,
787 typename BranchContainerT,
788 typename OctreeT>
789void
791 genOctreeKeyforPoint(const double point_x_arg,
792 const double point_y_arg,
793 const double point_z_arg,
794 OctreeKey& key_arg) const
795{
796 PointT temp_point;
797
798 temp_point.x = static_cast<float>(point_x_arg);
799 temp_point.y = static_cast<float>(point_y_arg);
800 temp_point.z = static_cast<float>(point_z_arg);
801
802 // generate key for point
803 genOctreeKeyforPoint(temp_point, key_arg);
804}
805
806//////////////////////////////////////////////////////////////////////////////////////////////
807template <typename PointT,
808 typename LeafContainerT,
809 typename BranchContainerT,
810 typename OctreeT>
811bool
813 genOctreeKeyForDataT(const index_t& data_arg, OctreeKey& key_arg) const
814{
815 const PointT temp_point = getPointByIndex(data_arg);
816
817 // generate key for point
818 genOctreeKeyforPoint(temp_point, key_arg);
819
820 return (true);
821}
822
823//////////////////////////////////////////////////////////////////////////////////////////////
824template <typename PointT,
825 typename LeafContainerT,
826 typename BranchContainerT,
827 typename OctreeT>
828void
830 genLeafNodeCenterFromOctreeKey(const OctreeKey& key, PointT& point) const
831{
832 // define point to leaf node voxel center
833 point.x = static_cast<float>((static_cast<double>(key.x) + 0.5f) * this->resolution_ +
834 this->min_x_);
835 point.y = static_cast<float>((static_cast<double>(key.y) + 0.5f) * this->resolution_ +
836 this->min_y_);
837 point.z = static_cast<float>((static_cast<double>(key.z) + 0.5f) * this->resolution_ +
838 this->min_z_);
839}
840
841//////////////////////////////////////////////////////////////////////////////////////////////
842template <typename PointT,
843 typename LeafContainerT,
844 typename BranchContainerT,
845 typename OctreeT>
846void
849 uindex_t tree_depth_arg,
850 PointT& point_arg) const
851{
852 // generate point for voxel center defined by treedepth (bitLen) and key
853 point_arg.x = static_cast<float>(
854 (static_cast<double>(key_arg.x) + 0.5f) *
855 (this->resolution_ *
856 static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg))) +
857 this->min_x_);
858 point_arg.y = static_cast<float>(
859 (static_cast<double>(key_arg.y) + 0.5f) *
860 (this->resolution_ *
861 static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg))) +
862 this->min_y_);
863 point_arg.z = static_cast<float>(
864 (static_cast<double>(key_arg.z) + 0.5f) *
865 (this->resolution_ *
866 static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg))) +
867 this->min_z_);
868}
869
870//////////////////////////////////////////////////////////////////////////////////////////////
871template <typename PointT,
872 typename LeafContainerT,
873 typename BranchContainerT,
874 typename OctreeT>
875void
878 uindex_t tree_depth_arg,
879 Eigen::Vector3f& min_pt,
880 Eigen::Vector3f& max_pt) const
881{
882 // calculate voxel size of current tree depth
883 double voxel_side_len =
884 this->resolution_ *
885 static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg));
886
887 // calculate voxel bounds
888 min_pt(0) = static_cast<float>(static_cast<double>(key_arg.x) * voxel_side_len +
889 this->min_x_);
890 min_pt(1) = static_cast<float>(static_cast<double>(key_arg.y) * voxel_side_len +
891 this->min_y_);
892 min_pt(2) = static_cast<float>(static_cast<double>(key_arg.z) * voxel_side_len +
893 this->min_z_);
894
895 max_pt(0) = static_cast<float>(static_cast<double>(key_arg.x + 1) * voxel_side_len +
896 this->min_x_);
897 max_pt(1) = static_cast<float>(static_cast<double>(key_arg.y + 1) * voxel_side_len +
898 this->min_y_);
899 max_pt(2) = static_cast<float>(static_cast<double>(key_arg.z + 1) * voxel_side_len +
900 this->min_z_);
901}
902
903//////////////////////////////////////////////////////////////////////////////////////////////
904template <typename PointT,
905 typename LeafContainerT,
906 typename BranchContainerT,
907 typename OctreeT>
908double
910 getVoxelSquaredSideLen(uindex_t tree_depth_arg) const
911{
912 double side_len;
913
914 // side length of the voxel cube increases exponentially with the octree depth
915 side_len = this->resolution_ *
916 static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg));
917
918 // squared voxel side length
919 side_len *= side_len;
920
921 return (side_len);
922}
923
924//////////////////////////////////////////////////////////////////////////////////////////////
925template <typename PointT,
926 typename LeafContainerT,
927 typename BranchContainerT,
928 typename OctreeT>
929double
931 getVoxelSquaredDiameter(uindex_t tree_depth_arg) const
932{
933 // return the squared side length of the voxel cube as a function of the octree depth
934 return (getVoxelSquaredSideLen(tree_depth_arg) * 3);
935}
936
937//////////////////////////////////////////////////////////////////////////////////////////////
938template <typename PointT,
939 typename LeafContainerT,
940 typename BranchContainerT,
941 typename OctreeT>
945 const OctreeKey& key_arg,
946 AlignedPointTVector& voxel_center_list_arg) const
947{
948 uindex_t voxel_count = 0;
949
950 // iterate over all children
951 for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
952 if (!this->branchHasChild(*node_arg, child_idx))
953 continue;
954
955 const OctreeNode* child_node;
956 child_node = this->getBranchChildPtr(*node_arg, child_idx);
957
958 // generate new key for current branch voxel
959 OctreeKey new_key;
960 new_key.x = (key_arg.x << 1) | (!!(child_idx & (1 << 2)));
961 new_key.y = (key_arg.y << 1) | (!!(child_idx & (1 << 1)));
962 new_key.z = (key_arg.z << 1) | (!!(child_idx & (1 << 0)));
963
964 switch (child_node->getNodeType()) {
965 case BRANCH_NODE: {
966 // recursively proceed with indexed child branch
967 voxel_count += getOccupiedVoxelCentersRecursive(
968 static_cast<const BranchNode*>(child_node), new_key, voxel_center_list_arg);
969 break;
970 }
971 case LEAF_NODE: {
972 PointT new_point;
973
974 genLeafNodeCenterFromOctreeKey(new_key, new_point);
975 voxel_center_list_arg.push_back(new_point);
976
977 voxel_count++;
978 break;
979 }
980 default:
981 break;
982 }
983 }
984 return (voxel_count);
985}
986
987#define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataTVector(T) \
988 template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
989 T, \
990 pcl::octree::OctreeContainerPointIndices, \
991 pcl::octree::OctreeContainerEmpty, \
992 pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndices, \
993 pcl::octree::OctreeContainerEmpty>>;
994#define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataTVector(T) \
995 template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
996 T, \
997 pcl::octree::OctreeContainerPointIndices, \
998 pcl::octree::OctreeContainerEmpty, \
999 pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerPointIndices, \
1000 pcl::octree::OctreeContainerEmpty>>;
1001
1002#define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataT(T) \
1003 template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1004 T, \
1005 pcl::octree::OctreeContainerPointIndex, \
1006 pcl::octree::OctreeContainerEmpty, \
1007 pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndex, \
1008 pcl::octree::OctreeContainerEmpty>>;
1009#define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataT(T) \
1010 template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1011 T, \
1012 pcl::octree::OctreeContainerPointIndex, \
1013 pcl::octree::OctreeContainerEmpty, \
1014 pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerPointIndex, \
1015 pcl::octree::OctreeContainerEmpty>>;
1016
1017#define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithEmptyLeaf(T) \
1018 template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1019 T, \
1020 pcl::octree::OctreeContainerEmpty, \
1021 pcl::octree::OctreeContainerEmpty, \
1022 pcl::octree::OctreeBase<pcl::octree::OctreeContainerEmpty, \
1023 pcl::octree::OctreeContainerEmpty>>;
1024#define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithEmptyLeaf(T) \
1025 template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1026 T, \
1027 pcl::octree::OctreeContainerEmpty, \
1028 pcl::octree::OctreeContainerEmpty, \
1029 pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerEmpty, \
1030 pcl::octree::OctreeContainerEmpty>>;
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
Definition exceptions.h:194
Octree key class
Definition octree_key.h:54
static const unsigned char maxDepth
Definition octree_key.h:142
unsigned char getChildIdxWithDepthMask(uindex_t depthMask) const
get child node index using depthMask
Definition octree_key.h:134
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 getVoxelSquaredDiameter() const
Calculates the squared diameter of a voxel at leaf depth.
void adoptBoundingBoxToPoint(const PointT &point_idx_arg)
Grow the bounding box/octree until point fits.
uindex_t getOccupiedVoxelCenters(AlignedPointTVector &voxel_center_list_arg) const
Get a PointT vector of centers of all occupied voxels.
const PointT & getPointByIndex(uindex_t index_arg) const
Get point at index from input pointcloud dataset.
void expandLeafNode(LeafNode *leaf_node, BranchNode *parent_branch, unsigned char child_idx, uindex_t depth_mask)
Add point at index from input pointcloud dataset to octree.
void deleteVoxelAtPoint(const PointT &point_arg)
Delete leaf node / voxel at given point.
void genLeafNodeCenterFromOctreeKey(const OctreeKey &key_arg, PointT &point_arg) const
Generate a point at center of leaf node voxel.
void genVoxelCenterFromOctreeKey(const OctreeKey &key_arg, uindex_t tree_depth_arg, PointT &point_arg) const
Generate a point at center of octree voxel at given tree level.
typename PointCloud::Ptr PointCloudPtr
void addPointFromCloud(uindex_t point_idx_arg, IndicesPtr indices_arg)
Add point at given index from input point cloud to octree.
OctreePointCloud(const double resolution_arg)
Octree pointcloud constructor.
shared_ptr< Indices > IndicesPtr
void addPointToCloud(const PointT &point_arg, PointCloudPtr cloud_arg)
Add point simultaneously to octree and input point cloud.
typename PointCloud::ConstPtr PointCloudConstPtr
void defineBoundingBox()
Investigate dimensions of pointcloud data set and define corresponding bounding box for octree.
void genVoxelBoundsFromOctreeKey(const OctreeKey &key_arg, uindex_t tree_depth_arg, Eigen::Vector3f &min_pt, Eigen::Vector3f &max_pt) const
Generate bounds of an octree voxel using octree key and tree depth arguments.
typename OctreeT::LeafNode LeafNode
virtual void addPointIdx(uindex_t point_idx_arg)
Add point at index from input pointcloud dataset to octree.
double getVoxelSquaredSideLen() const
Calculates the squared voxel cube side length at leaf level.
bool isVoxelOccupiedAtPoint(const PointT &point_arg) const
Check if voxel at given point exist.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
void addPointsFromInputCloud()
Add points from input point cloud to octree.
uindex_t getOccupiedVoxelCentersRecursive(const BranchNode *node_arg, const OctreeKey &key_arg, AlignedPointTVector &voxel_center_list_arg) const
Recursively search the tree for all leaf nodes and return a vector of voxel centers.
void genOctreeKeyforPoint(const PointT &point_arg, OctreeKey &key_arg) const
Generate octree key for voxel at a given point.
shared_ptr< const Indices > IndicesConstPtr
void getBoundingBox(double &min_x_arg, double &min_y_arg, double &min_z_arg, double &max_x_arg, double &max_y_arg, double &max_z_arg) const
Get bounding box for octree.
uindex_t getApproxIntersectedVoxelCentersBySegment(const Eigen::Vector3f &origin, const Eigen::Vector3f &end, AlignedPointTVector &voxel_center_list, float precision=0.2)
Get a PointT vector of centers of voxels intersected by a line segment.
virtual bool genOctreeKeyForDataT(const index_t &data_arg, OctreeKey &key_arg) const
Virtual method for generating octree key for a given point index.
void getKeyBitSize()
Define octree key setting and octree depth based on defined bounding box.
Define standard C methods and C++ classes that are common to all methods.
void getMinMax3D(const pcl::PointCloud< PointT > &cloud, PointT &min_pt, PointT &max_pt)
Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud.
Definition common.hpp:295
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.
Defines basic non-point types used by PCL.