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 135 this->genOctreeKeyforPoint (point_arg, key);
138 return (isPointWithinBoundingBox (point_arg) && this->existLeaf (key));
142 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
bool 146 const PointT& point = this->input_->points[point_idx_arg];
149 return (this->isVoxelOccupiedAtPoint (point));
153 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
bool 155 const double point_x_arg,
const double point_y_arg,
const double point_z_arg)
const 160 this->genOctreeKeyforPoint (point_x_arg, point_y_arg, point_z_arg, key);
162 return (this->existLeaf (key));
166 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 172 this->genOctreeKeyforPoint (point_arg, key);
174 this->removeLeaf (key);
178 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 182 const PointT& point = this->input_->points[point_idx_arg];
185 this->deleteVoxelAtPoint (point);
189 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
int 194 key.
x = key.
y = key.
z = 0;
196 voxel_center_list_arg.clear ();
198 return getOccupiedVoxelCentersRecursive (this->root_node_, key, voxel_center_list_arg);
203 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
int 205 const Eigen::Vector3f& origin,
206 const Eigen::Vector3f& end,
210 Eigen::Vector3f direction = end - origin;
211 float norm = direction.norm ();
212 direction.normalize ();
214 const float step_size = static_cast<const float> (resolution_) * precision;
216 const int nsteps = std::max (1, static_cast<int> (norm / step_size));
220 bool bkeyDefined =
false;
223 for (
int i = 0; i < nsteps; ++i)
225 Eigen::Vector3f p = origin + (direction * step_size * static_cast<const float> (i));
233 this->genOctreeKeyforPoint (octree_p, key);
236 if ((key == prev_key) && (bkeyDefined) )
243 genLeafNodeCenterFromOctreeKey (key, center);
244 voxel_center_list.push_back (center);
252 this->genOctreeKeyforPoint (end_p, end_key);
253 if (!(end_key == prev_key))
256 genLeafNodeCenterFromOctreeKey (end_key, center);
257 voxel_center_list.push_back (center);
260 return (static_cast<int> (voxel_center_list.size ()));
264 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 268 double minX, minY, minZ, maxX, maxY, maxZ;
274 assert (this->leaf_count_ == 0);
278 float minValue = std::numeric_limits<float>::epsilon () * 512.0f;
284 maxX = max_pt.x + minValue;
285 maxY = max_pt.y + minValue;
286 maxZ = max_pt.z + minValue;
289 defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);
293 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 295 const double min_y_arg,
296 const double min_z_arg,
297 const double max_x_arg,
298 const double max_y_arg,
299 const double max_z_arg)
302 assert (this->leaf_count_ == 0);
304 assert (max_x_arg >= min_x_arg);
305 assert (max_y_arg >= min_y_arg);
306 assert (max_z_arg >= min_z_arg);
317 min_x_ = std::min (min_x_, max_x_);
318 min_y_ = std::min (min_y_, max_y_);
319 min_z_ = std::min (min_z_, max_z_);
321 max_x_ = std::max (min_x_, max_x_);
322 max_y_ = std::max (min_y_, max_y_);
323 max_z_ = std::max (min_z_, max_z_);
328 bounding_box_defined_ =
true;
332 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 334 const double max_x_arg,
const double max_y_arg,
const double max_z_arg)
337 assert (this->leaf_count_ == 0);
339 assert (max_x_arg >= 0.0f);
340 assert (max_y_arg >= 0.0f);
341 assert (max_z_arg >= 0.0f);
352 min_x_ = std::min (min_x_, max_x_);
353 min_y_ = std::min (min_y_, max_y_);
354 min_z_ = std::min (min_z_, max_z_);
356 max_x_ = std::max (min_x_, max_x_);
357 max_y_ = std::max (min_y_, max_y_);
358 max_z_ = std::max (min_z_, max_z_);
363 bounding_box_defined_ =
true;
367 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 371 assert (this->leaf_count_ == 0);
373 assert (cubeLen_arg >= 0.0f);
376 max_x_ = cubeLen_arg;
379 max_y_ = cubeLen_arg;
382 max_z_ = cubeLen_arg;
384 min_x_ = std::min (min_x_, max_x_);
385 min_y_ = std::min (min_y_, max_y_);
386 min_z_ = std::min (min_z_, max_z_);
388 max_x_ = std::max (min_x_, max_x_);
389 max_y_ = std::max (min_y_, max_y_);
390 max_z_ = std::max (min_z_, max_z_);
395 bounding_box_defined_ =
true;
399 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 401 double& min_x_arg,
double& min_y_arg,
double& min_z_arg,
402 double& max_x_arg,
double& max_y_arg,
double& max_z_arg)
const 415 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
420 const float minValue = std::numeric_limits<float>::epsilon ();
425 bool bLowerBoundViolationX = (point_idx_arg.x < min_x_);
426 bool bLowerBoundViolationY = (point_idx_arg.y < min_y_);
427 bool bLowerBoundViolationZ = (point_idx_arg.z < min_z_);
429 bool bUpperBoundViolationX = (point_idx_arg.x >= max_x_);
430 bool bUpperBoundViolationY = (point_idx_arg.y >= max_y_);
431 bool bUpperBoundViolationZ = (point_idx_arg.z >= max_z_);
434 if (bLowerBoundViolationX || bLowerBoundViolationY || bLowerBoundViolationZ || bUpperBoundViolationX
435 || bUpperBoundViolationY || bUpperBoundViolationZ || (!bounding_box_defined_) )
438 if (bounding_box_defined_)
441 double octreeSideLen;
442 unsigned char child_idx;
445 child_idx = static_cast<unsigned char> (((!bUpperBoundViolationX) << 2) | ((!bUpperBoundViolationY) << 1)
446 | ((!bUpperBoundViolationZ)));
451 this->branch_count_++;
453 this->setBranchChildPtr (*newRootBranch, child_idx, this->root_node_);
455 this->root_node_ = newRootBranch;
457 octreeSideLen = static_cast<double> (1 << this->octree_depth_) * resolution_;
459 if (!bUpperBoundViolationX)
460 min_x_ -= octreeSideLen;
462 if (!bUpperBoundViolationY)
463 min_y_ -= octreeSideLen;
465 if (!bUpperBoundViolationZ)
466 min_z_ -= octreeSideLen;
469 this->octree_depth_++;
470 this->setTreeDepth (this->octree_depth_);
473 octreeSideLen = static_cast<double> (1 << this->octree_depth_) * resolution_ - minValue;
476 max_x_ = min_x_ + octreeSideLen;
477 max_y_ = min_y_ + octreeSideLen;
478 max_z_ = min_z_ + octreeSideLen;
485 this->min_x_ = point_idx_arg.x - this->resolution_ / 2;
486 this->min_y_ = point_idx_arg.y - this->resolution_ / 2;
487 this->min_z_ = point_idx_arg.z - this->resolution_ / 2;
489 this->max_x_ = point_idx_arg.x + this->resolution_ / 2;
490 this->max_y_ = point_idx_arg.y + this->resolution_ / 2;
491 this->max_z_ = point_idx_arg.z + this->resolution_ / 2;
495 bounding_box_defined_ =
true;
506 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 513 size_t leaf_obj_count = (*leaf_node)->getSize ();
516 std::vector<int> leafIndices;
517 leafIndices.reserve(leaf_obj_count);
519 (*leaf_node)->getPointIndices(leafIndices);
522 this->deleteBranchChild(*parent_branch, child_idx);
523 this->leaf_count_ --;
526 BranchNode* childBranch = this->createBranchChild (*parent_branch, child_idx);
527 this->branch_count_ ++;
529 typename std::vector<int>::iterator it = leafIndices.begin();
530 typename std::vector<int>::const_iterator it_end = leafIndices.end();
535 for (it = leafIndices.begin(); it!=it_end; ++it)
538 const PointT& point_from_index = input_->points[*it];
540 genOctreeKeyforPoint (point_from_index, new_index_key);
544 this->createLeafRecursive (new_index_key, depth_mask, childBranch, newLeaf, newBranchParent);
546 (*newLeaf)->addPointIndex(*it);
555 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 560 assert (point_idx_arg < static_cast<int> (input_->points.size ()));
562 const PointT& point = input_->points[point_idx_arg];
565 adoptBoundingBoxToPoint (point);
568 genOctreeKeyforPoint (point, key);
572 unsigned int depth_mask = this->createLeafRecursive (key, this->depth_mask_ ,this->root_node_, leaf_node, parent_branch_of_leaf_node);
574 if (this->dynamic_depth_enabled_ && depth_mask)
577 size_t leaf_obj_count = (*leaf_node)->getSize ();
579 while (leaf_obj_count>=max_objs_per_leaf_ && depth_mask)
584 expandLeafNode (leaf_node,
585 parent_branch_of_leaf_node,
589 depth_mask = this->createLeafRecursive (key, this->depth_mask_ ,this->root_node_, leaf_node, parent_branch_of_leaf_node);
590 leaf_obj_count = (*leaf_node)->getSize ();
595 (*leaf_node)->addPointIndex (point_idx_arg);
599 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
const PointT&
603 assert (index_arg < static_cast<unsigned int> (input_->points.size ()));
604 return (this->input_->points[index_arg]);
608 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 611 unsigned int max_voxels;
613 unsigned int max_key_x;
614 unsigned int max_key_y;
615 unsigned int max_key_z;
617 double octree_side_len;
619 const float minValue = std::numeric_limits<float>::epsilon();
622 max_key_x = static_cast<unsigned int> (ceil ((max_x_ - min_x_ - minValue) / resolution_));
623 max_key_y = static_cast<unsigned int> (ceil ((max_y_ - min_y_ - minValue) / resolution_));
624 max_key_z = static_cast<unsigned int> (ceil ((max_z_ - min_z_ - minValue) / resolution_));
627 max_voxels = std::max (std::max (std::max (max_key_x, max_key_y), max_key_z), static_cast<unsigned int> (2));
631 this->octree_depth_ = std::max ((std::min (static_cast<unsigned int> (OctreeKey::maxDepth), static_cast<unsigned int> (ceil (this->Log2 (max_voxels)-minValue)))),
632 static_cast<unsigned int> (0));
634 octree_side_len = static_cast<double> (1 << this->octree_depth_) * resolution_;
636 if (this->leaf_count_ == 0)
638 double octree_oversize_x;
639 double octree_oversize_y;
640 double octree_oversize_z;
642 octree_oversize_x = (octree_side_len - (max_x_ - min_x_)) / 2.0;
643 octree_oversize_y = (octree_side_len - (max_y_ - min_y_)) / 2.0;
644 octree_oversize_z = (octree_side_len - (max_z_ - min_z_)) / 2.0;
646 assert (octree_oversize_x > -minValue);
647 assert (octree_oversize_y > -minValue);
648 assert (octree_oversize_z > -minValue);
650 if (octree_oversize_x > minValue)
652 min_x_ -= octree_oversize_x;
653 max_x_ += octree_oversize_x;
655 if (octree_oversize_y > minValue)
657 min_y_ -= octree_oversize_y;
658 max_y_ += octree_oversize_y;
660 if (octree_oversize_z > minValue)
662 min_z_ -= octree_oversize_z;
663 max_z_ += octree_oversize_z;
668 max_x_ = min_x_ + octree_side_len;
669 max_y_ = min_y_ + octree_side_len;
670 max_z_ = min_z_ + octree_side_len;
674 this->setTreeDepth (this->octree_depth_);
679 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 684 key_arg.
x = static_cast<unsigned int> ((point_arg.x - this->min_x_) / this->resolution_);
685 key_arg.
y = static_cast<unsigned int> ((point_arg.y - this->min_y_) / this->resolution_);
686 key_arg.
z = static_cast<unsigned int> ((point_arg.z - this->min_z_) / this->resolution_);
688 assert (key_arg.
x <= this->max_key_.x);
689 assert (key_arg.
y <= this->max_key_.y);
690 assert (key_arg.
z <= this->max_key_.z);
694 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 696 const double point_x_arg,
const double point_y_arg,
697 const double point_z_arg,
OctreeKey & key_arg)
const 701 temp_point.x = static_cast<float> (point_x_arg);
702 temp_point.y = static_cast<float> (point_y_arg);
703 temp_point.z = static_cast<float> (point_z_arg);
706 genOctreeKeyforPoint (temp_point, key_arg);
710 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
bool 713 const PointT temp_point = getPointByIndex (data_arg);
716 genOctreeKeyforPoint (temp_point, key_arg);
722 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 726 point.x = static_cast<float> ((static_cast<double> (key.
x) + 0.5f) * this->resolution_ + this->min_x_);
727 point.y = static_cast<float> ((static_cast<double> (key.
y) + 0.5f) * this->resolution_ + this->min_y_);
728 point.z = static_cast<float> ((static_cast<double> (key.
z) + 0.5f) * this->resolution_ + this->min_z_);
732 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 735 unsigned int tree_depth_arg,
739 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_);
740 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_);
741 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_);
745 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 748 unsigned int tree_depth_arg,
749 Eigen::Vector3f &min_pt,
750 Eigen::Vector3f &max_pt)
const 753 double voxel_side_len = this->resolution_ * static_cast<double> (1 << (this->octree_depth_ - tree_depth_arg));
756 min_pt (0) = static_cast<float> (static_cast<double> (key_arg.
x) * voxel_side_len + this->min_x_);
757 min_pt (1) = static_cast<float> (static_cast<double> (key_arg.
y) * voxel_side_len + this->min_y_);
758 min_pt (2) = static_cast<float> (static_cast<double> (key_arg.
z) * voxel_side_len + this->min_z_);
760 max_pt (0) = static_cast<float> (static_cast<double> (key_arg.
x + 1) * voxel_side_len + this->min_x_);
761 max_pt (1) = static_cast<float> (static_cast<double> (key_arg.
y + 1) * voxel_side_len + this->min_y_);
762 max_pt (2) = static_cast<float> (static_cast<double> (key_arg.
z + 1) * voxel_side_len + this->min_z_);
766 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
double 772 side_len = this->resolution_ * static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg));
775 side_len *= side_len;
781 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
double 785 return (getVoxelSquaredSideLen (tree_depth_arg) * 3);
789 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
int 796 unsigned char child_idx;
801 for (child_idx = 0; child_idx < 8; child_idx++)
803 if (!this->branchHasChild (*node_arg, child_idx))
807 child_node = this->getBranchChildPtr (*node_arg, child_idx);
811 new_key.
x = (key_arg.
x << 1) | (!!(child_idx & (1 << 2)));
812 new_key.
y = (key_arg.
y << 1) | (!!(child_idx & (1 << 1)));
813 new_key.
z = (key_arg.
z << 1) | (!!(child_idx & (1 << 0)));
820 voxel_count += getOccupiedVoxelCentersRecursive (static_cast<const BranchNode*> (child_node), new_key, voxel_center_list_arg);
827 genLeafNodeCenterFromOctreeKey (new_key, new_point);
828 voxel_center_list_arg.push_back (new_point);
837 return (voxel_count);
840 #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 > >; 841 #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 > >; 843 #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 > >; 844 #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 > >; 846 #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 > >; 847 #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.