39 #ifndef PCL_OCTREE_POINTCLOUD_HPP_
40 #define PCL_OCTREE_POINTCLOUD_HPP_
45 #include <pcl/octree/impl/octree_base.hpp>
48 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
51 epsilon_ (0), resolution_ (resolution), min_x_ (0.0f), max_x_ (resolution), min_y_ (0.0f),
52 max_y_ (resolution), min_z_ (0.0f), max_z_ (resolution), bounding_box_defined_ (false), max_objs_per_leaf_(0)
54 assert (resolution > 0.0f);
58 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
63 for (
const int &index : *indices_)
65 assert( (index >= 0) && (index < static_cast<int> (input_->points.size ())));
67 if (
isFinite (input_->points[index]))
70 this->addPointIdx (index);
76 for (std::size_t i = 0; i < input_->points.size (); i++)
81 this->addPointIdx (static_cast<unsigned int> (i));
88 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
91 this->addPointIdx (point_idx_arg);
93 indices_arg->push_back (point_idx_arg);
97 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
100 assert (cloud_arg==input_);
102 cloud_arg->push_back (point_arg);
104 this->addPointIdx (static_cast<const int> (cloud_arg->points.size ()) - 1);
108 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
112 assert (cloud_arg==input_);
113 assert (indices_arg==indices_);
115 cloud_arg->push_back (point_arg);
117 this->addPointFromCloud (static_cast<const int> (cloud_arg->points.size ()) - 1, indices_arg);
121 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
bool
124 if (!isPointWithinBoundingBox (point_arg))
132 this->genOctreeKeyforPoint (point_arg, key);
135 return (this->existLeaf (key));
139 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
bool
143 const PointT& point = this->input_->points[point_idx_arg];
146 return (this->isVoxelOccupiedAtPoint (point));
150 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
bool
152 const double point_x_arg,
const double point_y_arg,
const double point_z_arg)
const
156 point.x = point_x_arg;
157 point.y = point_y_arg;
158 point.z = point_z_arg;
161 return (this->isVoxelOccupiedAtPoint (point));
165 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
168 if (!isPointWithinBoundingBox (point_arg))
176 this->genOctreeKeyforPoint (point_arg, key);
178 this->removeLeaf (key);
182 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
186 const PointT& point = this->input_->points[point_idx_arg];
189 this->deleteVoxelAtPoint (point);
193 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
int
198 key.
x = key.
y = key.
z = 0;
200 voxel_center_list_arg.clear ();
202 return getOccupiedVoxelCentersRecursive (this->root_node_, key, voxel_center_list_arg);
207 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
int
209 const Eigen::Vector3f& origin,
210 const Eigen::Vector3f& end,
214 Eigen::Vector3f direction = end - origin;
215 float norm = direction.norm ();
216 direction.normalize ();
218 const float step_size = static_cast<float> (resolution_) * precision;
220 const int nsteps = std::max (1, static_cast<int> (norm / step_size));
224 bool bkeyDefined =
false;
227 for (
int i = 0; i < nsteps; ++i)
229 Eigen::Vector3f p = origin + (direction * step_size * static_cast<float> (i));
237 this->genOctreeKeyforPoint (octree_p, key);
240 if ((key == prev_key) && (bkeyDefined) )
247 genLeafNodeCenterFromOctreeKey (key, center);
248 voxel_center_list.push_back (center);
256 this->genOctreeKeyforPoint (end_p, end_key);
257 if (!(end_key == prev_key))
260 genLeafNodeCenterFromOctreeKey (end_key, center);
261 voxel_center_list.push_back (center);
264 return (static_cast<int> (voxel_center_list.size ()));
268 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
272 double minX, minY, minZ, maxX, maxY, maxZ;
278 assert (this->leaf_count_ == 0);
282 float minValue = std::numeric_limits<float>::epsilon () * 512.0f;
288 maxX = max_pt.x + minValue;
289 maxY = max_pt.y + minValue;
290 maxZ = max_pt.z + minValue;
293 defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);
297 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
299 const double min_y_arg,
300 const double min_z_arg,
301 const double max_x_arg,
302 const double max_y_arg,
303 const double max_z_arg)
306 assert (this->leaf_count_ == 0);
308 assert (max_x_arg >= min_x_arg);
309 assert (max_y_arg >= min_y_arg);
310 assert (max_z_arg >= min_z_arg);
321 min_x_ = std::min (min_x_, max_x_);
322 min_y_ = std::min (min_y_, max_y_);
323 min_z_ = std::min (min_z_, max_z_);
325 max_x_ = std::max (min_x_, max_x_);
326 max_y_ = std::max (min_y_, max_y_);
327 max_z_ = std::max (min_z_, max_z_);
332 bounding_box_defined_ =
true;
336 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
338 const double max_x_arg,
const double max_y_arg,
const double max_z_arg)
341 assert (this->leaf_count_ == 0);
343 assert (max_x_arg >= 0.0f);
344 assert (max_y_arg >= 0.0f);
345 assert (max_z_arg >= 0.0f);
356 min_x_ = std::min (min_x_, max_x_);
357 min_y_ = std::min (min_y_, max_y_);
358 min_z_ = std::min (min_z_, max_z_);
360 max_x_ = std::max (min_x_, max_x_);
361 max_y_ = std::max (min_y_, max_y_);
362 max_z_ = std::max (min_z_, max_z_);
367 bounding_box_defined_ =
true;
371 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
375 assert (this->leaf_count_ == 0);
377 assert (cubeLen_arg >= 0.0f);
380 max_x_ = cubeLen_arg;
383 max_y_ = cubeLen_arg;
386 max_z_ = cubeLen_arg;
388 min_x_ = std::min (min_x_, max_x_);
389 min_y_ = std::min (min_y_, max_y_);
390 min_z_ = std::min (min_z_, max_z_);
392 max_x_ = std::max (min_x_, max_x_);
393 max_y_ = std::max (min_y_, max_y_);
394 max_z_ = std::max (min_z_, max_z_);
399 bounding_box_defined_ =
true;
403 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
405 double& min_x_arg,
double& min_y_arg,
double& min_z_arg,
406 double& max_x_arg,
double& max_y_arg,
double& max_z_arg)
const
419 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
424 const float minValue = std::numeric_limits<float>::epsilon ();
429 bool bLowerBoundViolationX = (point_idx_arg.x < min_x_);
430 bool bLowerBoundViolationY = (point_idx_arg.y < min_y_);
431 bool bLowerBoundViolationZ = (point_idx_arg.z < min_z_);
433 bool bUpperBoundViolationX = (point_idx_arg.x >= max_x_);
434 bool bUpperBoundViolationY = (point_idx_arg.y >= max_y_);
435 bool bUpperBoundViolationZ = (point_idx_arg.z >= max_z_);
438 if (bLowerBoundViolationX || bLowerBoundViolationY || bLowerBoundViolationZ || bUpperBoundViolationX
439 || bUpperBoundViolationY || bUpperBoundViolationZ || (!bounding_box_defined_) )
442 if (bounding_box_defined_)
445 double octreeSideLen;
446 unsigned char child_idx;
449 child_idx = static_cast<unsigned char> (((!bUpperBoundViolationX) << 2) | ((!bUpperBoundViolationY) << 1)
450 | ((!bUpperBoundViolationZ)));
455 this->branch_count_++;
457 this->setBranchChildPtr (*newRootBranch, child_idx, this->root_node_);
459 this->root_node_ = newRootBranch;
461 octreeSideLen = static_cast<double> (1 << this->octree_depth_) * resolution_;
463 if (!bUpperBoundViolationX)
464 min_x_ -= octreeSideLen;
466 if (!bUpperBoundViolationY)
467 min_y_ -= octreeSideLen;
469 if (!bUpperBoundViolationZ)
470 min_z_ -= octreeSideLen;
473 this->octree_depth_++;
474 this->setTreeDepth (this->octree_depth_);
477 octreeSideLen = static_cast<double> (1 << this->octree_depth_) * resolution_ - minValue;
480 max_x_ = min_x_ + octreeSideLen;
481 max_y_ = min_y_ + octreeSideLen;
482 max_z_ = min_z_ + octreeSideLen;
489 this->min_x_ = point_idx_arg.x - this->resolution_ / 2;
490 this->min_y_ = point_idx_arg.y - this->resolution_ / 2;
491 this->min_z_ = point_idx_arg.z - this->resolution_ / 2;
493 this->max_x_ = point_idx_arg.x + this->resolution_ / 2;
494 this->max_y_ = point_idx_arg.y + this->resolution_ / 2;
495 this->max_z_ = point_idx_arg.z + this->resolution_ / 2;
499 bounding_box_defined_ =
true;
510 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
517 std::size_t leaf_obj_count = (*leaf_node)->getSize ();
520 std::vector<int> leafIndices;
521 leafIndices.reserve(leaf_obj_count);
523 (*leaf_node)->getPointIndices(leafIndices);
526 this->deleteBranchChild(*parent_branch, child_idx);
527 this->leaf_count_ --;
530 BranchNode* childBranch = this->createBranchChild (*parent_branch, child_idx);
531 this->branch_count_ ++;
536 for (
const int &leafIndex : leafIndices)
539 const PointT& point_from_index = input_->points[leafIndex];
541 genOctreeKeyforPoint (point_from_index, new_index_key);
545 this->createLeafRecursive (new_index_key, depth_mask, childBranch, newLeaf, newBranchParent);
547 (*newLeaf)->addPointIndex(leafIndex);
556 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
561 assert (point_idx_arg < static_cast<int> (input_->points.size ()));
563 const PointT& point = input_->points[point_idx_arg];
566 adoptBoundingBoxToPoint (point);
569 genOctreeKeyforPoint (point, key);
573 unsigned int depth_mask = this->createLeafRecursive (key, this->depth_mask_ ,this->root_node_, leaf_node, parent_branch_of_leaf_node);
575 if (this->dynamic_depth_enabled_ && depth_mask)
578 std::size_t leaf_obj_count = (*leaf_node)->getSize ();
580 while (leaf_obj_count>=max_objs_per_leaf_ && depth_mask)
585 expandLeafNode (leaf_node,
586 parent_branch_of_leaf_node,
590 depth_mask = this->createLeafRecursive (key, this->depth_mask_ ,this->root_node_, leaf_node, parent_branch_of_leaf_node);
591 leaf_obj_count = (*leaf_node)->getSize ();
596 (*leaf_node)->addPointIndex (point_idx_arg);
600 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
const PointT&
604 assert (index_arg < static_cast<unsigned int> (input_->points.size ()));
605 return (this->input_->points[index_arg]);
609 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
612 unsigned int max_voxels;
614 unsigned int max_key_x;
615 unsigned int max_key_y;
616 unsigned int max_key_z;
618 double octree_side_len;
620 const float minValue = std::numeric_limits<float>::epsilon();
623 max_key_x = static_cast<unsigned int> (std::ceil ((max_x_ - min_x_ - minValue) / resolution_));
624 max_key_y = static_cast<unsigned int> (std::ceil ((max_y_ - min_y_ - minValue) / resolution_));
625 max_key_z = static_cast<unsigned int> (std::ceil ((max_z_ - min_z_ - minValue) / resolution_));
628 max_voxels = std::max (std::max (std::max (max_key_x, max_key_y), max_key_z), static_cast<unsigned int> (2));
632 this->octree_depth_ = std::max ((std::min (static_cast<unsigned int> (OctreeKey::maxDepth), static_cast<unsigned int> (std::ceil (std::log2 (max_voxels) - minValue)))),
633 static_cast<unsigned int> (0));
635 octree_side_len = static_cast<double> (1 << this->octree_depth_) * resolution_;
637 if (this->leaf_count_ == 0)
639 double octree_oversize_x;
640 double octree_oversize_y;
641 double octree_oversize_z;
643 octree_oversize_x = (octree_side_len - (max_x_ - min_x_)) / 2.0;
644 octree_oversize_y = (octree_side_len - (max_y_ - min_y_)) / 2.0;
645 octree_oversize_z = (octree_side_len - (max_z_ - min_z_)) / 2.0;
647 assert (octree_oversize_x > -minValue);
648 assert (octree_oversize_y > -minValue);
649 assert (octree_oversize_z > -minValue);
651 if (octree_oversize_x > minValue)
653 min_x_ -= octree_oversize_x;
654 max_x_ += octree_oversize_x;
656 if (octree_oversize_y > minValue)
658 min_y_ -= octree_oversize_y;
659 max_y_ += octree_oversize_y;
661 if (octree_oversize_z > minValue)
663 min_z_ -= octree_oversize_z;
664 max_z_ += octree_oversize_z;
669 max_x_ = min_x_ + octree_side_len;
670 max_y_ = min_y_ + octree_side_len;
671 max_z_ = min_z_ + octree_side_len;
675 this->setTreeDepth (this->octree_depth_);
680 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
685 key_arg.
x = static_cast<unsigned int> ((point_arg.x - this->min_x_) / this->resolution_);
686 key_arg.
y = static_cast<unsigned int> ((point_arg.y - this->min_y_) / this->resolution_);
687 key_arg.
z = static_cast<unsigned int> ((point_arg.z - this->min_z_) / this->resolution_);
689 assert (key_arg.
x <= this->max_key_.x);
690 assert (key_arg.
y <= this->max_key_.y);
691 assert (key_arg.
z <= this->max_key_.z);
695 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
697 const double point_x_arg,
const double point_y_arg,
698 const double point_z_arg,
OctreeKey & key_arg)
const
702 temp_point.x = static_cast<float> (point_x_arg);
703 temp_point.y = static_cast<float> (point_y_arg);
704 temp_point.z = static_cast<float> (point_z_arg);
707 genOctreeKeyforPoint (temp_point, key_arg);
711 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
bool
714 const PointT temp_point = getPointByIndex (data_arg);
717 genOctreeKeyforPoint (temp_point, key_arg);
723 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
727 point.x = static_cast<float> ((static_cast<double> (key.
x) + 0.5f) * this->resolution_ + this->min_x_);
728 point.y = static_cast<float> ((static_cast<double> (key.
y) + 0.5f) * this->resolution_ + this->min_y_);
729 point.z = static_cast<float> ((static_cast<double> (key.
z) + 0.5f) * this->resolution_ + this->min_z_);
733 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
736 unsigned int tree_depth_arg,
740 point_arg.x = static_cast<float> ((static_cast <double> (key_arg.
x) + 0.5f) * (this->resolution_ * static_cast<double> (1 << (this->octree_depth_ - tree_depth_arg))) + this->min_x_);
741 point_arg.y = static_cast<float> ((static_cast <double> (key_arg.
y) + 0.5f) * (this->resolution_ * static_cast<double> (1 << (this->octree_depth_ - tree_depth_arg))) + this->min_y_);
742 point_arg.z = static_cast<float> ((static_cast <double> (key_arg.
z) + 0.5f) * (this->resolution_ * static_cast<double> (1 << (this->octree_depth_ - tree_depth_arg))) + this->min_z_);
746 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
749 unsigned int tree_depth_arg,
750 Eigen::Vector3f &min_pt,
751 Eigen::Vector3f &max_pt)
const
754 double voxel_side_len = this->resolution_ * static_cast<double> (1 << (this->octree_depth_ - tree_depth_arg));
757 min_pt (0) = static_cast<float> (static_cast<double> (key_arg.
x) * voxel_side_len + this->min_x_);
758 min_pt (1) = static_cast<float> (static_cast<double> (key_arg.
y) * voxel_side_len + this->min_y_);
759 min_pt (2) = static_cast<float> (static_cast<double> (key_arg.
z) * voxel_side_len + this->min_z_);
761 max_pt (0) = static_cast<float> (static_cast<double> (key_arg.
x + 1) * voxel_side_len + this->min_x_);
762 max_pt (1) = static_cast<float> (static_cast<double> (key_arg.
y + 1) * voxel_side_len + this->min_y_);
763 max_pt (2) = static_cast<float> (static_cast<double> (key_arg.
z + 1) * voxel_side_len + this->min_z_);
767 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
double
773 side_len = this->resolution_ * static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg));
776 side_len *= side_len;
782 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
double
786 return (getVoxelSquaredSideLen (tree_depth_arg) * 3);
790 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
int
799 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++)
801 if (!this->branchHasChild (*node_arg, child_idx))
805 child_node = this->getBranchChildPtr (*node_arg, child_idx);
809 new_key.
x = (key_arg.
x << 1) | (!!(child_idx & (1 << 2)));
810 new_key.
y = (key_arg.
y << 1) | (!!(child_idx & (1 << 1)));
811 new_key.
z = (key_arg.
z << 1) | (!!(child_idx & (1 << 0)));
818 voxel_count += getOccupiedVoxelCentersRecursive (static_cast<const BranchNode*> (child_node), new_key, voxel_center_list_arg);
825 genLeafNodeCenterFromOctreeKey (new_key, new_point);
826 voxel_center_list_arg.push_back (new_point);
835 return (voxel_count);
838 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataTVector(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty > >;
839 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataTVector(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty, pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty > >;
841 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataT(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerPointIndex, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndex, pcl::octree::OctreeContainerEmpty > >;
842 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataT(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerPointIndex, pcl::octree::OctreeContainerEmpty, pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerPointIndex, pcl::octree::OctreeContainerEmpty > >;
844 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeBase<pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeContainerEmpty > >;
845 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeContainerEmpty, pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeContainerEmpty > >;