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