42 #include <pcl/common/point_tests.h>
43 #include <pcl/octree/impl/octree_base.hpp>
49 typename LeafContainerT,
50 typename BranchContainerT,
58 , resolution_(resolution)
65 , bounding_box_defined_(false)
66 , max_objs_per_leaf_(0)
68 assert(resolution > 0.0f);
73 typename LeafContainerT,
74 typename BranchContainerT,
81 for (
const int& index : *indices_) {
82 assert((index >= 0) && (index <
static_cast<int>(input_->points.size())));
84 if (
isFinite(input_->points[index])) {
86 this->addPointIdx(index);
91 for (std::size_t i = 0; i < input_->points.size(); i++) {
94 this->addPointIdx(
static_cast<unsigned int>(i));
101 template <
typename PointT,
102 typename LeafContainerT,
103 typename BranchContainerT,
109 this->addPointIdx(point_idx_arg);
111 indices_arg->push_back(point_idx_arg);
115 template <
typename PointT,
116 typename LeafContainerT,
117 typename BranchContainerT,
123 assert(cloud_arg == input_);
125 cloud_arg->push_back(point_arg);
127 this->addPointIdx(
static_cast<const int>(cloud_arg->points.size()) - 1);
131 template <
typename PointT,
132 typename LeafContainerT,
133 typename BranchContainerT,
141 assert(cloud_arg == input_);
142 assert(indices_arg == indices_);
144 cloud_arg->push_back(point_arg);
146 this->addPointFromCloud(
static_cast<const int>(cloud_arg->points.size()) - 1,
151 template <
typename PointT,
152 typename LeafContainerT,
153 typename BranchContainerT,
159 if (!isPointWithinBoundingBox(point_arg)) {
166 this->genOctreeKeyforPoint(point_arg, key);
169 return (this->existLeaf(key));
173 template <
typename PointT,
174 typename LeafContainerT,
175 typename BranchContainerT,
182 const PointT& point = this->input_->points[point_idx_arg];
185 return (this->isVoxelOccupiedAtPoint(point));
189 template <
typename PointT,
190 typename LeafContainerT,
191 typename BranchContainerT,
196 const double point_y_arg,
197 const double point_z_arg)
const
201 point.x = point_x_arg;
202 point.y = point_y_arg;
203 point.z = point_z_arg;
206 return (this->isVoxelOccupiedAtPoint(point));
210 template <
typename PointT,
211 typename LeafContainerT,
212 typename BranchContainerT,
218 if (!isPointWithinBoundingBox(point_arg)) {
225 this->genOctreeKeyforPoint(point_arg, key);
227 this->removeLeaf(key);
232 typename LeafContainerT,
233 typename BranchContainerT,
240 const PointT& point = this->input_->points[point_idx_arg];
243 this->deleteVoxelAtPoint(point);
247 template <
typename PointT,
248 typename LeafContainerT,
249 typename BranchContainerT,
256 key.
x = key.
y = key.
z = 0;
258 voxel_center_list_arg.clear();
260 return getOccupiedVoxelCentersRecursive(this->root_node_, key, voxel_center_list_arg);
264 template <
typename PointT,
265 typename LeafContainerT,
266 typename BranchContainerT,
271 const Eigen::Vector3f& end,
275 Eigen::Vector3f direction = end - origin;
276 float norm = direction.norm();
277 direction.normalize();
279 const float step_size =
static_cast<float>(resolution_) * precision;
281 const int nsteps = std::max(1,
static_cast<int>(norm / step_size));
285 bool bkeyDefined =
false;
288 for (
int i = 0; i < nsteps; ++i) {
289 Eigen::Vector3f p = origin + (direction * step_size *
static_cast<float>(i));
297 this->genOctreeKeyforPoint(octree_p, key);
300 if ((key == prev_key) && (bkeyDefined))
307 genLeafNodeCenterFromOctreeKey(key, center);
308 voxel_center_list.push_back(center);
316 this->genOctreeKeyforPoint(end_p, end_key);
317 if (!(end_key == prev_key)) {
319 genLeafNodeCenterFromOctreeKey(end_key, center);
320 voxel_center_list.push_back(center);
323 return (
static_cast<int>(voxel_center_list.size()));
327 template <
typename PointT,
328 typename LeafContainerT,
329 typename BranchContainerT,
336 double minX, minY, minZ, maxX, maxY, maxZ;
342 assert(this->leaf_count_ == 0);
346 float minValue = std::numeric_limits<float>::epsilon() * 512.0f;
352 maxX = max_pt.x + minValue;
353 maxY = max_pt.y + minValue;
354 maxZ = max_pt.z + minValue;
357 defineBoundingBox(minX, minY, minZ, maxX, maxY, maxZ);
361 template <
typename PointT,
362 typename LeafContainerT,
363 typename BranchContainerT,
368 const double min_y_arg,
369 const double min_z_arg,
370 const double max_x_arg,
371 const double max_y_arg,
372 const double max_z_arg)
375 assert(this->leaf_count_ == 0);
377 assert(max_x_arg >= min_x_arg);
378 assert(max_y_arg >= min_y_arg);
379 assert(max_z_arg >= min_z_arg);
390 min_x_ = std::min(min_x_, max_x_);
391 min_y_ = std::min(min_y_, max_y_);
392 min_z_ = std::min(min_z_, max_z_);
394 max_x_ = std::max(min_x_, max_x_);
395 max_y_ = std::max(min_y_, max_y_);
396 max_z_ = std::max(min_z_, max_z_);
401 bounding_box_defined_ =
true;
405 template <
typename PointT,
406 typename LeafContainerT,
407 typename BranchContainerT,
412 const double max_y_arg,
413 const double max_z_arg)
416 assert(this->leaf_count_ == 0);
418 assert(max_x_arg >= 0.0f);
419 assert(max_y_arg >= 0.0f);
420 assert(max_z_arg >= 0.0f);
431 min_x_ = std::min(min_x_, max_x_);
432 min_y_ = std::min(min_y_, max_y_);
433 min_z_ = std::min(min_z_, max_z_);
435 max_x_ = std::max(min_x_, max_x_);
436 max_y_ = std::max(min_y_, max_y_);
437 max_z_ = std::max(min_z_, max_z_);
442 bounding_box_defined_ =
true;
446 template <
typename PointT,
447 typename LeafContainerT,
448 typename BranchContainerT,
455 assert(this->leaf_count_ == 0);
457 assert(cubeLen_arg >= 0.0f);
460 max_x_ = cubeLen_arg;
463 max_y_ = cubeLen_arg;
466 max_z_ = cubeLen_arg;
468 min_x_ = std::min(min_x_, max_x_);
469 min_y_ = std::min(min_y_, max_y_);
470 min_z_ = std::min(min_z_, max_z_);
472 max_x_ = std::max(min_x_, max_x_);
473 max_y_ = std::max(min_y_, max_y_);
474 max_z_ = std::max(min_z_, max_z_);
479 bounding_box_defined_ =
true;
483 template <
typename PointT,
484 typename LeafContainerT,
485 typename BranchContainerT,
494 double& max_z_arg)
const
506 template <
typename PointT,
507 typename LeafContainerT,
508 typename BranchContainerT,
515 const float minValue = std::numeric_limits<float>::epsilon();
519 bool bLowerBoundViolationX = (point_idx_arg.x < min_x_);
520 bool bLowerBoundViolationY = (point_idx_arg.y < min_y_);
521 bool bLowerBoundViolationZ = (point_idx_arg.z < min_z_);
523 bool bUpperBoundViolationX = (point_idx_arg.x >= max_x_);
524 bool bUpperBoundViolationY = (point_idx_arg.y >= max_y_);
525 bool bUpperBoundViolationZ = (point_idx_arg.z >= max_z_);
528 if (bLowerBoundViolationX || bLowerBoundViolationY || bLowerBoundViolationZ ||
529 bUpperBoundViolationX || bUpperBoundViolationY || bUpperBoundViolationZ ||
530 (!bounding_box_defined_)) {
532 if (bounding_box_defined_) {
534 double octreeSideLen;
535 unsigned char child_idx;
539 child_idx =
static_cast<unsigned char>(((!bUpperBoundViolationX) << 2) |
540 ((!bUpperBoundViolationY) << 1) |
541 ((!bUpperBoundViolationZ)));
546 this->branch_count_++;
548 this->setBranchChildPtr(*newRootBranch, child_idx, this->root_node_);
550 this->root_node_ = newRootBranch;
552 octreeSideLen =
static_cast<double>(1 << this->octree_depth_) * resolution_;
554 if (!bUpperBoundViolationX)
555 min_x_ -= octreeSideLen;
557 if (!bUpperBoundViolationY)
558 min_y_ -= octreeSideLen;
560 if (!bUpperBoundViolationZ)
561 min_z_ -= octreeSideLen;
564 this->octree_depth_++;
565 this->setTreeDepth(this->octree_depth_);
569 static_cast<double>(1 << this->octree_depth_) * resolution_ - minValue;
572 max_x_ = min_x_ + octreeSideLen;
573 max_y_ = min_y_ + octreeSideLen;
574 max_z_ = min_z_ + octreeSideLen;
579 this->min_x_ = point_idx_arg.x - this->resolution_ / 2;
580 this->min_y_ = point_idx_arg.y - this->resolution_ / 2;
581 this->min_z_ = point_idx_arg.z - this->resolution_ / 2;
583 this->max_x_ = point_idx_arg.x + this->resolution_ / 2;
584 this->max_y_ = point_idx_arg.y + this->resolution_ / 2;
585 this->max_z_ = point_idx_arg.z + this->resolution_ / 2;
589 bounding_box_defined_ =
true;
599 template <
typename PointT,
600 typename LeafContainerT,
601 typename BranchContainerT,
607 unsigned char child_idx,
608 unsigned int depth_mask)
613 std::size_t leaf_obj_count = (*leaf_node)->getSize();
616 std::vector<int> leafIndices;
617 leafIndices.reserve(leaf_obj_count);
619 (*leaf_node)->getPointIndices(leafIndices);
622 this->deleteBranchChild(*parent_branch, child_idx);
626 BranchNode* childBranch = this->createBranchChild(*parent_branch, child_idx);
627 this->branch_count_++;
632 for (
const int& leafIndex : leafIndices) {
634 const PointT& point_from_index = input_->points[leafIndex];
636 genOctreeKeyforPoint(point_from_index, new_index_key);
640 this->createLeafRecursive(
641 new_index_key, depth_mask, childBranch, newLeaf, newBranchParent);
643 (*newLeaf)->addPointIndex(leafIndex);
649 template <
typename PointT,
650 typename LeafContainerT,
651 typename BranchContainerT,
659 assert(point_idx_arg <
static_cast<int>(input_->points.size()));
661 const PointT& point = input_->points[point_idx_arg];
664 adoptBoundingBoxToPoint(point);
667 genOctreeKeyforPoint(point, key);
671 unsigned int depth_mask = this->createLeafRecursive(
672 key, this->depth_mask_, this->root_node_, leaf_node, parent_branch_of_leaf_node);
674 if (this->dynamic_depth_enabled_ && depth_mask) {
676 std::size_t leaf_obj_count = (*leaf_node)->getSize();
678 while (leaf_obj_count >= max_objs_per_leaf_ && depth_mask) {
682 expandLeafNode(leaf_node, parent_branch_of_leaf_node, child_idx, depth_mask);
684 depth_mask = this->createLeafRecursive(key,
688 parent_branch_of_leaf_node);
689 leaf_obj_count = (*leaf_node)->getSize();
693 (*leaf_node)->addPointIndex(point_idx_arg);
697 template <
typename PointT,
698 typename LeafContainerT,
699 typename BranchContainerT,
706 assert(index_arg <
static_cast<unsigned int>(input_->points.size()));
707 return (this->input_->points[index_arg]);
711 template <
typename PointT,
712 typename LeafContainerT,
713 typename BranchContainerT,
719 unsigned int max_voxels;
721 unsigned int max_key_x;
722 unsigned int max_key_y;
723 unsigned int max_key_z;
725 double octree_side_len;
727 const float minValue = std::numeric_limits<float>::epsilon();
731 static_cast<unsigned int>(std::ceil((max_x_ - min_x_ - minValue) / resolution_));
733 static_cast<unsigned int>(std::ceil((max_y_ - min_y_ - minValue) / resolution_));
735 static_cast<unsigned int>(std::ceil((max_z_ - min_z_ - minValue) / resolution_));
738 max_voxels = std::max(std::max(std::max(max_key_x, max_key_y), max_key_z),
739 static_cast<unsigned int>(2));
742 this->octree_depth_ =
744 static_cast<unsigned int>(
745 std::ceil(std::log2(max_voxels) - minValue)))),
746 static_cast<unsigned int>(0));
748 octree_side_len =
static_cast<double>(1 << this->octree_depth_) * resolution_;
750 if (this->leaf_count_ == 0) {
751 double octree_oversize_x;
752 double octree_oversize_y;
753 double octree_oversize_z;
755 octree_oversize_x = (octree_side_len - (max_x_ - min_x_)) / 2.0;
756 octree_oversize_y = (octree_side_len - (max_y_ - min_y_)) / 2.0;
757 octree_oversize_z = (octree_side_len - (max_z_ - min_z_)) / 2.0;
759 assert(octree_oversize_x > -minValue);
760 assert(octree_oversize_y > -minValue);
761 assert(octree_oversize_z > -minValue);
763 if (octree_oversize_x > minValue) {
764 min_x_ -= octree_oversize_x;
765 max_x_ += octree_oversize_x;
767 if (octree_oversize_y > minValue) {
768 min_y_ -= octree_oversize_y;
769 max_y_ += octree_oversize_y;
771 if (octree_oversize_z > minValue) {
772 min_z_ -= octree_oversize_z;
773 max_z_ += octree_oversize_z;
777 max_x_ = min_x_ + octree_side_len;
778 max_y_ = min_y_ + octree_side_len;
779 max_z_ = min_z_ + octree_side_len;
783 this->setTreeDepth(this->octree_depth_);
787 template <
typename PointT,
788 typename LeafContainerT,
789 typename BranchContainerT,
797 static_cast<unsigned int>((point_arg.x - this->min_x_) / this->resolution_);
799 static_cast<unsigned int>((point_arg.y - this->min_y_) / this->resolution_);
801 static_cast<unsigned int>((point_arg.z - this->min_z_) / this->resolution_);
803 assert(key_arg.
x <= this->max_key_.x);
804 assert(key_arg.
y <= this->max_key_.y);
805 assert(key_arg.
z <= this->max_key_.z);
809 template <
typename PointT,
810 typename LeafContainerT,
811 typename BranchContainerT,
816 const double point_y_arg,
817 const double point_z_arg,
822 temp_point.x =
static_cast<float>(point_x_arg);
823 temp_point.y =
static_cast<float>(point_y_arg);
824 temp_point.z =
static_cast<float>(point_z_arg);
827 genOctreeKeyforPoint(temp_point, key_arg);
831 template <
typename PointT,
832 typename LeafContainerT,
833 typename BranchContainerT,
839 const PointT temp_point = getPointByIndex(data_arg);
842 genOctreeKeyforPoint(temp_point, key_arg);
848 template <
typename PointT,
849 typename LeafContainerT,
850 typename BranchContainerT,
857 point.x =
static_cast<float>((
static_cast<double>(key.
x) + 0.5f) * this->resolution_ +
859 point.y =
static_cast<float>((
static_cast<double>(key.
y) + 0.5f) * this->resolution_ +
861 point.z =
static_cast<float>((
static_cast<double>(key.
z) + 0.5f) * this->resolution_ +
866 template <
typename PointT,
867 typename LeafContainerT,
868 typename BranchContainerT,
873 unsigned int tree_depth_arg,
877 point_arg.x =
static_cast<float>(
878 (
static_cast<double>(key_arg.
x) + 0.5f) *
880 static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg))) +
882 point_arg.y =
static_cast<float>(
883 (
static_cast<double>(key_arg.
y) + 0.5f) *
885 static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg))) +
887 point_arg.z =
static_cast<float>(
888 (
static_cast<double>(key_arg.
z) + 0.5f) *
890 static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg))) +
895 template <
typename PointT,
896 typename LeafContainerT,
897 typename BranchContainerT,
902 unsigned int tree_depth_arg,
903 Eigen::Vector3f& min_pt,
904 Eigen::Vector3f& max_pt)
const
907 double voxel_side_len =
909 static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg));
912 min_pt(0) =
static_cast<float>(
static_cast<double>(key_arg.
x) * voxel_side_len +
914 min_pt(1) =
static_cast<float>(
static_cast<double>(key_arg.
y) * voxel_side_len +
916 min_pt(2) =
static_cast<float>(
static_cast<double>(key_arg.
z) * voxel_side_len +
919 max_pt(0) =
static_cast<float>(
static_cast<double>(key_arg.
x + 1) * voxel_side_len +
921 max_pt(1) =
static_cast<float>(
static_cast<double>(key_arg.
y + 1) * voxel_side_len +
923 max_pt(2) =
static_cast<float>(
static_cast<double>(key_arg.
z + 1) * voxel_side_len +
928 template <
typename PointT,
929 typename LeafContainerT,
930 typename BranchContainerT,
939 side_len = this->resolution_ *
940 static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg));
943 side_len *= side_len;
949 template <
typename PointT,
950 typename LeafContainerT,
951 typename BranchContainerT,
958 return (getVoxelSquaredSideLen(tree_depth_arg) * 3);
962 template <
typename PointT,
963 typename LeafContainerT,
964 typename BranchContainerT,
975 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
976 if (!this->branchHasChild(*node_arg, child_idx))
980 child_node = this->getBranchChildPtr(*node_arg, child_idx);
984 new_key.
x = (key_arg.
x << 1) | (!!(child_idx & (1 << 2)));
985 new_key.
y = (key_arg.
y << 1) | (!!(child_idx & (1 << 1)));
986 new_key.
z = (key_arg.
z << 1) | (!!(child_idx & (1 << 0)));
991 voxel_count += getOccupiedVoxelCentersRecursive(
992 static_cast<const BranchNode*
>(child_node), new_key, voxel_center_list_arg);
998 genLeafNodeCenterFromOctreeKey(new_key, new_point);
999 voxel_center_list_arg.push_back(new_point);
1008 return (voxel_count);
1011 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataTVector(T) \
1012 template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1014 pcl::octree::OctreeContainerPointIndices, \
1015 pcl::octree::OctreeContainerEmpty, \
1016 pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndices, \
1017 pcl::octree::OctreeContainerEmpty>>;
1018 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataTVector(T) \
1019 template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1021 pcl::octree::OctreeContainerPointIndices, \
1022 pcl::octree::OctreeContainerEmpty, \
1023 pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerPointIndices, \
1024 pcl::octree::OctreeContainerEmpty>>;
1026 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataT(T) \
1027 template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1029 pcl::octree::OctreeContainerPointIndex, \
1030 pcl::octree::OctreeContainerEmpty, \
1031 pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndex, \
1032 pcl::octree::OctreeContainerEmpty>>;
1033 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataT(T) \
1034 template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1036 pcl::octree::OctreeContainerPointIndex, \
1037 pcl::octree::OctreeContainerEmpty, \
1038 pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerPointIndex, \
1039 pcl::octree::OctreeContainerEmpty>>;
1041 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithEmptyLeaf(T) \
1042 template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1044 pcl::octree::OctreeContainerEmpty, \
1045 pcl::octree::OctreeContainerEmpty, \
1046 pcl::octree::OctreeBase<pcl::octree::OctreeContainerEmpty, \
1047 pcl::octree::OctreeContainerEmpty>>;
1048 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithEmptyLeaf(T) \
1049 template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1051 pcl::octree::OctreeContainerEmpty, \
1052 pcl::octree::OctreeContainerEmpty, \
1053 pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerEmpty, \
1054 pcl::octree::OctreeContainerEmpty>>;