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>
64 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 71 for (std::vector<int>::const_iterator current = indices_->begin (); current != indices_->end (); ++current)
73 assert( (*current>=0) && (*current < static_cast<int> (input_->points.size ())));
75 if (
isFinite (input_->points[*current]))
78 this->addPointIdx (*current);
84 for (i = 0; i < input_->points.size (); i++)
89 this->addPointIdx (static_cast<unsigned int> (i));
96 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 99 this->addPointIdx (point_idx_arg);
101 indices_arg->push_back (point_idx_arg);
105 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 108 assert (cloud_arg==input_);
110 cloud_arg->push_back (point_arg);
112 this->addPointIdx (static_cast<const int> (cloud_arg->points.size ()) - 1);
116 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 120 assert (cloud_arg==input_);
121 assert (indices_arg==indices_);
123 cloud_arg->push_back (point_arg);
125 this->addPointFromCloud (static_cast<const int> (cloud_arg->points.size ()) - 1, indices_arg);
129 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
bool 132 if (!isPointWithinBoundingBox (point_arg))
140 this->genOctreeKeyforPoint (point_arg, key);
143 return (this->existLeaf (key));
147 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
bool 151 const PointT& point = this->input_->points[point_idx_arg];
154 return (this->isVoxelOccupiedAtPoint (point));
158 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
bool 160 const double point_x_arg,
const double point_y_arg,
const double point_z_arg)
const 164 point.x = point_x_arg;
165 point.y = point_y_arg;
166 point.z = point_z_arg;
169 return (this->isVoxelOccupiedAtPoint (point));
173 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 176 if (!isPointWithinBoundingBox (point_arg))
184 this->genOctreeKeyforPoint (point_arg, key);
186 this->removeLeaf (key);
190 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 194 const PointT& point = this->input_->points[point_idx_arg];
197 this->deleteVoxelAtPoint (point);
201 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
int 206 key.
x = key.
y = key.
z = 0;
208 voxel_center_list_arg.clear ();
210 return getOccupiedVoxelCentersRecursive (this->root_node_, key, voxel_center_list_arg);
215 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
int 217 const Eigen::Vector3f& origin,
218 const Eigen::Vector3f& end,
222 Eigen::Vector3f direction = end - origin;
223 float norm = direction.norm ();
224 direction.normalize ();
226 const float step_size = static_cast<const float> (resolution_) * precision;
228 const int nsteps = std::max (1, static_cast<int> (norm / step_size));
232 bool bkeyDefined =
false;
235 for (
int i = 0; i < nsteps; ++i)
237 Eigen::Vector3f p = origin + (direction * step_size * static_cast<const float> (i));
245 this->genOctreeKeyforPoint (octree_p, key);
248 if ((key == prev_key) && (bkeyDefined) )
255 genLeafNodeCenterFromOctreeKey (key, center);
256 voxel_center_list.push_back (center);
264 this->genOctreeKeyforPoint (end_p, end_key);
265 if (!(end_key == prev_key))
268 genLeafNodeCenterFromOctreeKey (end_key, center);
269 voxel_center_list.push_back (center);
272 return (static_cast<int> (voxel_center_list.size ()));
276 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 280 double minX, minY, minZ, maxX, maxY, maxZ;
286 assert (this->leaf_count_ == 0);
290 float minValue = std::numeric_limits<float>::epsilon () * 512.0f;
296 maxX = max_pt.x + minValue;
297 maxY = max_pt.y + minValue;
298 maxZ = max_pt.z + minValue;
301 defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);
305 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 307 const double min_y_arg,
308 const double min_z_arg,
309 const double max_x_arg,
310 const double max_y_arg,
311 const double max_z_arg)
314 assert (this->leaf_count_ == 0);
316 assert (max_x_arg >= min_x_arg);
317 assert (max_y_arg >= min_y_arg);
318 assert (max_z_arg >= min_z_arg);
329 min_x_ = std::min (min_x_, max_x_);
330 min_y_ = std::min (min_y_, max_y_);
331 min_z_ = std::min (min_z_, max_z_);
333 max_x_ = std::max (min_x_, max_x_);
334 max_y_ = std::max (min_y_, max_y_);
335 max_z_ = std::max (min_z_, max_z_);
340 bounding_box_defined_ =
true;
344 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 346 const double max_x_arg,
const double max_y_arg,
const double max_z_arg)
349 assert (this->leaf_count_ == 0);
351 assert (max_x_arg >= 0.0f);
352 assert (max_y_arg >= 0.0f);
353 assert (max_z_arg >= 0.0f);
364 min_x_ = std::min (min_x_, max_x_);
365 min_y_ = std::min (min_y_, max_y_);
366 min_z_ = std::min (min_z_, max_z_);
368 max_x_ = std::max (min_x_, max_x_);
369 max_y_ = std::max (min_y_, max_y_);
370 max_z_ = std::max (min_z_, max_z_);
375 bounding_box_defined_ =
true;
379 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 383 assert (this->leaf_count_ == 0);
385 assert (cubeLen_arg >= 0.0f);
388 max_x_ = cubeLen_arg;
391 max_y_ = cubeLen_arg;
394 max_z_ = cubeLen_arg;
396 min_x_ = std::min (min_x_, max_x_);
397 min_y_ = std::min (min_y_, max_y_);
398 min_z_ = std::min (min_z_, max_z_);
400 max_x_ = std::max (min_x_, max_x_);
401 max_y_ = std::max (min_y_, max_y_);
402 max_z_ = std::max (min_z_, max_z_);
407 bounding_box_defined_ =
true;
411 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 413 double& min_x_arg,
double& min_y_arg,
double& min_z_arg,
414 double& max_x_arg,
double& max_y_arg,
double& max_z_arg)
const 427 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
432 const float minValue = std::numeric_limits<float>::epsilon ();
437 bool bLowerBoundViolationX = (point_idx_arg.x < min_x_);
438 bool bLowerBoundViolationY = (point_idx_arg.y < min_y_);
439 bool bLowerBoundViolationZ = (point_idx_arg.z < min_z_);
441 bool bUpperBoundViolationX = (point_idx_arg.x >= max_x_);
442 bool bUpperBoundViolationY = (point_idx_arg.y >= max_y_);
443 bool bUpperBoundViolationZ = (point_idx_arg.z >= max_z_);
446 if (bLowerBoundViolationX || bLowerBoundViolationY || bLowerBoundViolationZ || bUpperBoundViolationX
447 || bUpperBoundViolationY || bUpperBoundViolationZ || (!bounding_box_defined_) )
450 if (bounding_box_defined_)
453 double octreeSideLen;
454 unsigned char child_idx;
457 child_idx = static_cast<unsigned char> (((!bUpperBoundViolationX) << 2) | ((!bUpperBoundViolationY) << 1)
458 | ((!bUpperBoundViolationZ)));
463 this->branch_count_++;
465 this->setBranchChildPtr (*newRootBranch, child_idx, this->root_node_);
467 this->root_node_ = newRootBranch;
469 octreeSideLen = static_cast<double> (1 << this->octree_depth_) * resolution_;
471 if (!bUpperBoundViolationX)
472 min_x_ -= octreeSideLen;
474 if (!bUpperBoundViolationY)
475 min_y_ -= octreeSideLen;
477 if (!bUpperBoundViolationZ)
478 min_z_ -= octreeSideLen;
481 this->octree_depth_++;
482 this->setTreeDepth (this->octree_depth_);
485 octreeSideLen = static_cast<double> (1 << this->octree_depth_) * resolution_ - minValue;
488 max_x_ = min_x_ + octreeSideLen;
489 max_y_ = min_y_ + octreeSideLen;
490 max_z_ = min_z_ + octreeSideLen;
497 this->min_x_ = point_idx_arg.x - this->resolution_ / 2;
498 this->min_y_ = point_idx_arg.y - this->resolution_ / 2;
499 this->min_z_ = point_idx_arg.z - this->resolution_ / 2;
501 this->max_x_ = point_idx_arg.x + this->resolution_ / 2;
502 this->max_y_ = point_idx_arg.y + this->resolution_ / 2;
503 this->max_z_ = point_idx_arg.z + this->resolution_ / 2;
507 bounding_box_defined_ =
true;
518 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 525 size_t leaf_obj_count = (*leaf_node)->getSize ();
528 std::vector<int> leafIndices;
529 leafIndices.reserve(leaf_obj_count);
531 (*leaf_node)->getPointIndices(leafIndices);
534 this->deleteBranchChild(*parent_branch, child_idx);
535 this->leaf_count_ --;
538 BranchNode* childBranch = this->createBranchChild (*parent_branch, child_idx);
539 this->branch_count_ ++;
541 typename std::vector<int>::iterator it = leafIndices.begin();
542 typename std::vector<int>::const_iterator it_end = leafIndices.end();
547 for (it = leafIndices.begin(); it!=it_end; ++it)
550 const PointT& point_from_index = input_->points[*it];
552 genOctreeKeyforPoint (point_from_index, new_index_key);
556 this->createLeafRecursive (new_index_key, depth_mask, childBranch, newLeaf, newBranchParent);
558 (*newLeaf)->addPointIndex(*it);
567 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 572 assert (point_idx_arg < static_cast<int> (input_->points.size ()));
574 const PointT& point = input_->points[point_idx_arg];
577 adoptBoundingBoxToPoint (point);
580 genOctreeKeyforPoint (point, key);
584 unsigned int depth_mask = this->createLeafRecursive (key, this->depth_mask_ ,this->root_node_, leaf_node, parent_branch_of_leaf_node);
586 if (this->dynamic_depth_enabled_ && depth_mask)
589 size_t leaf_obj_count = (*leaf_node)->getSize ();
591 while (leaf_obj_count>=max_objs_per_leaf_ && depth_mask)
596 expandLeafNode (leaf_node,
597 parent_branch_of_leaf_node,
601 depth_mask = this->createLeafRecursive (key, this->depth_mask_ ,this->root_node_, leaf_node, parent_branch_of_leaf_node);
602 leaf_obj_count = (*leaf_node)->getSize ();
607 (*leaf_node)->addPointIndex (point_idx_arg);
611 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
const PointT&
615 assert (index_arg < static_cast<unsigned int> (input_->points.size ()));
616 return (this->input_->points[index_arg]);
620 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 623 unsigned int max_voxels;
625 unsigned int max_key_x;
626 unsigned int max_key_y;
627 unsigned int max_key_z;
629 double octree_side_len;
631 const float minValue = std::numeric_limits<float>::epsilon();
634 max_key_x = static_cast<unsigned int> (ceil ((max_x_ - min_x_ - minValue) / resolution_));
635 max_key_y = static_cast<unsigned int> (ceil ((max_y_ - min_y_ - minValue) / resolution_));
636 max_key_z = static_cast<unsigned int> (ceil ((max_z_ - min_z_ - minValue) / resolution_));
639 max_voxels = std::max (std::max (std::max (max_key_x, max_key_y), max_key_z), static_cast<unsigned int> (2));
643 this->octree_depth_ = std::max ((std::min (static_cast<unsigned int> (OctreeKey::maxDepth), static_cast<unsigned int> (ceil (this->Log2 (max_voxels)-minValue)))),
644 static_cast<unsigned int> (0));
646 octree_side_len = static_cast<double> (1 << this->octree_depth_) * resolution_;
648 if (this->leaf_count_ == 0)
650 double octree_oversize_x;
651 double octree_oversize_y;
652 double octree_oversize_z;
654 octree_oversize_x = (octree_side_len - (max_x_ - min_x_)) / 2.0;
655 octree_oversize_y = (octree_side_len - (max_y_ - min_y_)) / 2.0;
656 octree_oversize_z = (octree_side_len - (max_z_ - min_z_)) / 2.0;
658 assert (octree_oversize_x > -minValue);
659 assert (octree_oversize_y > -minValue);
660 assert (octree_oversize_z > -minValue);
662 if (octree_oversize_x > minValue)
664 min_x_ -= octree_oversize_x;
665 max_x_ += octree_oversize_x;
667 if (octree_oversize_y > minValue)
669 min_y_ -= octree_oversize_y;
670 max_y_ += octree_oversize_y;
672 if (octree_oversize_z > minValue)
674 min_z_ -= octree_oversize_z;
675 max_z_ += octree_oversize_z;
680 max_x_ = min_x_ + octree_side_len;
681 max_y_ = min_y_ + octree_side_len;
682 max_z_ = min_z_ + octree_side_len;
686 this->setTreeDepth (this->octree_depth_);
691 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 696 key_arg.
x = static_cast<unsigned int> ((point_arg.x - this->min_x_) / this->resolution_);
697 key_arg.
y = static_cast<unsigned int> ((point_arg.y - this->min_y_) / this->resolution_);
698 key_arg.
z = static_cast<unsigned int> ((point_arg.z - this->min_z_) / this->resolution_);
700 assert (key_arg.
x <= this->max_key_.x);
701 assert (key_arg.
y <= this->max_key_.y);
702 assert (key_arg.
z <= this->max_key_.z);
706 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 708 const double point_x_arg,
const double point_y_arg,
709 const double point_z_arg,
OctreeKey & key_arg)
const 713 temp_point.x = static_cast<float> (point_x_arg);
714 temp_point.y = static_cast<float> (point_y_arg);
715 temp_point.z = static_cast<float> (point_z_arg);
718 genOctreeKeyforPoint (temp_point, key_arg);
722 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
bool 725 const PointT temp_point = getPointByIndex (data_arg);
728 genOctreeKeyforPoint (temp_point, key_arg);
734 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 738 point.x = static_cast<float> ((static_cast<double> (key.
x) + 0.5f) * this->resolution_ + this->min_x_);
739 point.y = static_cast<float> ((static_cast<double> (key.
y) + 0.5f) * this->resolution_ + this->min_y_);
740 point.z = static_cast<float> ((static_cast<double> (key.
z) + 0.5f) * this->resolution_ + this->min_z_);
744 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 747 unsigned int tree_depth_arg,
751 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_);
752 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_);
753 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_);
757 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 760 unsigned int tree_depth_arg,
761 Eigen::Vector3f &min_pt,
762 Eigen::Vector3f &max_pt)
const 765 double voxel_side_len = this->resolution_ * static_cast<double> (1 << (this->octree_depth_ - tree_depth_arg));
768 min_pt (0) = static_cast<float> (static_cast<double> (key_arg.
x) * voxel_side_len + this->min_x_);
769 min_pt (1) = static_cast<float> (static_cast<double> (key_arg.
y) * voxel_side_len + this->min_y_);
770 min_pt (2) = static_cast<float> (static_cast<double> (key_arg.
z) * voxel_side_len + this->min_z_);
772 max_pt (0) = static_cast<float> (static_cast<double> (key_arg.
x + 1) * voxel_side_len + this->min_x_);
773 max_pt (1) = static_cast<float> (static_cast<double> (key_arg.
y + 1) * voxel_side_len + this->min_y_);
774 max_pt (2) = static_cast<float> (static_cast<double> (key_arg.
z + 1) * voxel_side_len + this->min_z_);
778 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
double 784 side_len = this->resolution_ * static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg));
787 side_len *= side_len;
793 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
double 797 return (getVoxelSquaredSideLen (tree_depth_arg) * 3);
801 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
int 808 unsigned char child_idx;
813 for (child_idx = 0; child_idx < 8; child_idx++)
815 if (!this->branchHasChild (*node_arg, child_idx))
819 child_node = this->getBranchChildPtr (*node_arg, child_idx);
823 new_key.
x = (key_arg.
x << 1) | (!!(child_idx & (1 << 2)));
824 new_key.
y = (key_arg.
y << 1) | (!!(child_idx & (1 << 1)));
825 new_key.
z = (key_arg.
z << 1) | (!!(child_idx & (1 << 0)));
832 voxel_count += getOccupiedVoxelCentersRecursive (static_cast<const BranchNode*> (child_node), new_key, voxel_center_list_arg);
839 genLeafNodeCenterFromOctreeKey (new_key, new_point);
840 voxel_center_list_arg.push_back (new_point);
849 return (voxel_count);
852 #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 > >; 853 #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 > >; 855 #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 > >; 856 #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 > >; 858 #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 > >; 859 #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 > >; 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...
void addPointsFromInputCloud()
Add points from input point cloud to octree.
int 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 ~OctreePointCloud()
Empty deconstructor.
void genLeafNodeCenterFromOctreeKey(const OctreeKey &key_arg, PointT &point_arg) const
Generate a point at center of leaf node voxel.
void addPointToCloud(const PointT &point_arg, PointCloudPtr cloud_arg)
Add point simultaneously to octree and input point cloud.
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)
boost::shared_ptr< PointCloud > PointCloudPtr
const PointT & getPointByIndex(const unsigned int index_arg) const
Get point at index from input pointcloud dataset.
virtual void addPointIdx(const int point_idx_arg)
Add point at index from input pointcloud dataset to octree.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
boost::shared_ptr< const PointCloud > PointCloudConstPtr
OctreePointCloud(const double resolution_arg)
Octree pointcloud constructor.
void adoptBoundingBoxToPoint(const PointT &point_idx_arg)
Grow the bounding box/octree until point fits.
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.
Define standard C methods and C++ classes that are common to all methods.
void defineBoundingBox()
Investigate dimensions of pointcloud data set and define corresponding bounding box for octree.
void genVoxelBoundsFromOctreeKey(const OctreeKey &key_arg, unsigned int 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.
int 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.
unsigned char getChildIdxWithDepthMask(unsigned int depthMask) const
get child node index using depthMask
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.
void getKeyBitSize()
Define octree key setting and octree depth based on defined bounding box.
void addPointFromCloud(const int point_idx_arg, IndicesPtr indices_arg)
Add point at given index from input point cloud to octree.
void genVoxelCenterFromOctreeKey(const OctreeKey &key_arg, unsigned int tree_depth_arg, PointT &point_arg) const
Generate a point at center of octree voxel at given tree level.
bool isVoxelOccupiedAtPoint(const PointT &point_arg) const
Check if voxel at given point exist.
void expandLeafNode(LeafNode *leaf_node, BranchNode *parent_branch, unsigned char child_idx, unsigned int depth_mask)
Add point at index from input pointcloud dataset to octree.
OctreeT::LeafNode LeafNode
boost::shared_ptr< const std::vector< int > > IndicesConstPtr
boost::shared_ptr< std::vector< int > > IndicesPtr
OctreeT::BranchNode BranchNode
double getVoxelSquaredSideLen() const
Calculates the squared voxel cube side length at leaf level.
A point structure representing Euclidean xyz coordinates, and the RGB color.
void deleteVoxelAtPoint(const PointT &point_arg)
Delete leaf node / voxel at given point.
Abstract octree node class
double getVoxelSquaredDiameter() const
Calculates the squared diameter of a voxel at leaf depth.
virtual bool genOctreeKeyForDataT(const int &data_arg, OctreeKey &key_arg) const
Virtual method for generating octree key for a given point index.
int getOccupiedVoxelCenters(AlignedPointTVector &voxel_center_list_arg) const
Get a PointT vector of centers of all occupied voxels.
void genOctreeKeyforPoint(const PointT &point_arg, OctreeKey &key_arg) const
Generate octree key for voxel at a given point.