39 #ifndef PCL_OCTREE_POINTCLOUD_HPP_
40 #define PCL_OCTREE_POINTCLOUD_HPP_
45 #include <pcl/common/common.h>
49 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
52 epsilon_ (0), resolution_ (resolution), min_x_ (0.0f), max_x_ (resolution), min_y_ (0.0f),
53 max_y_ (resolution), min_z_ (0.0f), max_z_ (resolution), bounding_box_defined_ (false), max_objs_per_leaf_(0)
55 assert (resolution > 0.0f);
59 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
65 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
72 for (std::vector<int>::const_iterator current = indices_->begin (); current != indices_->end (); ++current)
74 assert( (*current>=0) && (*current < static_cast<int> (input_->points.size ())));
76 if (
isFinite (input_->points[*current]))
79 this->addPointIdx (*current);
85 for (i = 0; i < input_->points.size (); i++)
90 this->addPointIdx (static_cast<unsigned int> (i));
97 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
100 this->addPointIdx (point_idx_arg);
102 indices_arg->push_back (point_idx_arg);
106 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
109 assert (cloud_arg==input_);
111 cloud_arg->push_back (point_arg);
113 this->addPointIdx (static_cast<const int> (cloud_arg->points.size ()) - 1);
117 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
121 assert (cloud_arg==input_);
122 assert (indices_arg==indices_);
124 cloud_arg->push_back (point_arg);
126 this->addPointFromCloud (static_cast<const int> (cloud_arg->points.size ()) - 1, indices_arg);
130 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
bool
136 this->genOctreeKeyforPoint (point_arg, key);
139 return (isPointWithinBoundingBox (point_arg) && this->existLeaf (key));
143 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
bool
147 const PointT& point = this->input_->points[point_idx_arg];
150 return (this->isVoxelOccupiedAtPoint (point));
154 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
bool
156 const double point_x_arg,
const double point_y_arg,
const double point_z_arg)
const
161 this->genOctreeKeyforPoint (point_x_arg, point_y_arg, point_z_arg, key);
163 return (this->existLeaf (key));
167 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
173 this->genOctreeKeyforPoint (point_arg, key);
175 this->removeLeaf (key);
179 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
183 const PointT& point = this->input_->points[point_idx_arg];
186 this->deleteVoxelAtPoint (point);
190 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
int
195 key.
x = key.
y = key.
z = 0;
197 voxel_center_list_arg.clear ();
199 return getOccupiedVoxelCentersRecursive (this->root_node_, key, voxel_center_list_arg);
204 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
int
206 const Eigen::Vector3f& origin,
207 const Eigen::Vector3f& end,
211 Eigen::Vector3f direction = end - origin;
212 float norm = direction.norm ();
213 direction.normalize ();
215 const float step_size =
static_cast<const float> (resolution_) * precision;
217 const int nsteps = std::max (1, static_cast<int> (norm / step_size));
221 bool bkeyDefined =
false;
224 for (
int i = 0; i < nsteps; ++i)
226 Eigen::Vector3f p = origin + (direction * step_size *
static_cast<const float> (i));
234 this->genOctreeKeyforPoint (octree_p, key);
237 if ((key == prev_key) && (bkeyDefined) )
244 genLeafNodeCenterFromOctreeKey (key, center);
245 voxel_center_list.push_back (center);
253 this->genOctreeKeyforPoint (end_p, end_key);
254 if (!(end_key == prev_key))
257 genLeafNodeCenterFromOctreeKey (end_key, center);
258 voxel_center_list.push_back (center);
261 return (static_cast<int> (voxel_center_list.size ()));
265 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
269 double minX, minY, minZ, maxX, maxY, maxZ;
275 assert (this->leaf_count_ == 0);
279 float minValue = std::numeric_limits<float>::epsilon () * 512.0f;
285 maxX = max_pt.x + minValue;
286 maxY = max_pt.y + minValue;
287 maxZ = max_pt.z + minValue;
290 defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);
294 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
296 const double min_y_arg,
297 const double min_z_arg,
298 const double max_x_arg,
299 const double max_y_arg,
300 const double max_z_arg)
303 assert (this->leaf_count_ == 0);
305 assert (max_x_arg >= min_x_arg);
306 assert (max_y_arg >= min_y_arg);
307 assert (max_z_arg >= min_z_arg);
318 min_x_ = std::min (min_x_, max_x_);
319 min_y_ = std::min (min_y_, max_y_);
320 min_z_ = std::min (min_z_, max_z_);
322 max_x_ = std::max (min_x_, max_x_);
323 max_y_ = std::max (min_y_, max_y_);
324 max_z_ = std::max (min_z_, max_z_);
329 bounding_box_defined_ =
true;
333 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
335 const double max_x_arg,
const double max_y_arg,
const double max_z_arg)
338 assert (this->leaf_count_ == 0);
340 assert (max_x_arg >= 0.0f);
341 assert (max_y_arg >= 0.0f);
342 assert (max_z_arg >= 0.0f);
353 min_x_ = std::min (min_x_, max_x_);
354 min_y_ = std::min (min_y_, max_y_);
355 min_z_ = std::min (min_z_, max_z_);
357 max_x_ = std::max (min_x_, max_x_);
358 max_y_ = std::max (min_y_, max_y_);
359 max_z_ = std::max (min_z_, max_z_);
364 bounding_box_defined_ =
true;
368 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
372 assert (this->leaf_count_ == 0);
374 assert (cubeLen_arg >= 0.0f);
377 max_x_ = cubeLen_arg;
380 max_y_ = cubeLen_arg;
383 max_z_ = cubeLen_arg;
385 min_x_ = std::min (min_x_, max_x_);
386 min_y_ = std::min (min_y_, max_y_);
387 min_z_ = std::min (min_z_, max_z_);
389 max_x_ = std::max (min_x_, max_x_);
390 max_y_ = std::max (min_y_, max_y_);
391 max_z_ = std::max (min_z_, max_z_);
396 bounding_box_defined_ =
true;
400 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
402 double& min_x_arg,
double& min_y_arg,
double& min_z_arg,
403 double& max_x_arg,
double& max_y_arg,
double& max_z_arg)
const
416 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
421 const float minValue = std::numeric_limits<float>::epsilon ();
426 bool bLowerBoundViolationX = (point_idx_arg.x < min_x_);
427 bool bLowerBoundViolationY = (point_idx_arg.y < min_y_);
428 bool bLowerBoundViolationZ = (point_idx_arg.z < min_z_);
430 bool bUpperBoundViolationX = (point_idx_arg.x >= max_x_);
431 bool bUpperBoundViolationY = (point_idx_arg.y >= max_y_);
432 bool bUpperBoundViolationZ = (point_idx_arg.z >= max_z_);
435 if (bLowerBoundViolationX || bLowerBoundViolationY || bLowerBoundViolationZ || bUpperBoundViolationX
436 || bUpperBoundViolationY || bUpperBoundViolationZ || (!bounding_box_defined_) )
439 if (bounding_box_defined_)
442 double octreeSideLen;
443 unsigned char child_idx;
446 child_idx =
static_cast<unsigned char> (((!bUpperBoundViolationX) << 2) | ((!bUpperBoundViolationY) << 1)
447 | ((!bUpperBoundViolationZ)));
452 this->branch_count_++;
454 this->setBranchChildPtr (*newRootBranch, child_idx, this->root_node_);
456 this->root_node_ = newRootBranch;
458 octreeSideLen =
static_cast<double> (1 << this->octree_depth_) * resolution_;
460 if (!bUpperBoundViolationX)
461 min_x_ -= octreeSideLen;
463 if (!bUpperBoundViolationY)
464 min_y_ -= octreeSideLen;
466 if (!bUpperBoundViolationZ)
467 min_z_ -= octreeSideLen;
470 this->octree_depth_++;
471 this->setTreeDepth (this->octree_depth_);
474 octreeSideLen =
static_cast<double> (1 << this->octree_depth_) * resolution_ - minValue;
477 max_x_ = min_x_ + octreeSideLen;
478 max_y_ = min_y_ + octreeSideLen;
479 max_z_ = min_z_ + octreeSideLen;
486 this->min_x_ = point_idx_arg.x - this->resolution_ / 2;
487 this->min_y_ = point_idx_arg.y - this->resolution_ / 2;
488 this->min_z_ = point_idx_arg.z - this->resolution_ / 2;
490 this->max_x_ = point_idx_arg.x + this->resolution_ / 2;
491 this->max_y_ = point_idx_arg.y + this->resolution_ / 2;
492 this->max_z_ = point_idx_arg.z + this->resolution_ / 2;
496 bounding_box_defined_ =
true;
507 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
514 size_t leaf_obj_count = (*leaf_node)->getSize ();
517 std::vector<int> leafIndices;
518 leafIndices.reserve(leaf_obj_count);
520 (*leaf_node)->getPointIndices(leafIndices);
523 this->deleteBranchChild(*parent_branch, child_idx);
524 this->leaf_count_ --;
527 BranchNode* childBranch = this->createBranchChild (*parent_branch, child_idx);
528 this->branch_count_ ++;
530 typename std::vector<int>::iterator it = leafIndices.begin();
531 typename std::vector<int>::const_iterator it_end = leafIndices.end();
536 for (it = leafIndices.begin(); it!=it_end; ++it)
539 const PointT& point_from_index = input_->points[*it];
541 genOctreeKeyforPoint (point_from_index, new_index_key);
545 this->createLeafRecursive (new_index_key, depth_mask, childBranch, newLeaf, newBranchParent);
547 (*newLeaf)->addPointIndex(*it);
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 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> ((max_x_ - min_x_) / resolution_);
624 max_key_y =
static_cast<unsigned int> ((max_y_ - min_y_) / resolution_);
625 max_key_z =
static_cast<unsigned int> ((max_z_ - min_z_) / 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> (ceil (this->Log2 (max_voxels)-minValue)))),
633 static_cast<unsigned int> (0));
635 octree_side_len =
static_cast<double> (1 << this->octree_depth_) * resolution_-minValue;
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 min_x_ -= octree_oversize_x;
648 min_y_ -= octree_oversize_y;
649 min_z_ -= octree_oversize_z;
651 max_x_ += octree_oversize_x;
652 max_y_ += octree_oversize_y;
653 max_z_ += octree_oversize_z;
657 max_x_ = min_x_ + octree_side_len;
658 max_y_ = min_y_ + octree_side_len;
659 max_z_ = min_z_ + octree_side_len;
663 this->setTreeDepth (this->octree_depth_);
668 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
673 key_arg.
x =
static_cast<unsigned int> ((point_arg.x - this->min_x_) / this->resolution_);
674 key_arg.
y =
static_cast<unsigned int> ((point_arg.y - this->min_y_) / this->resolution_);
675 key_arg.
z =
static_cast<unsigned int> ((point_arg.z - this->min_z_) / this->resolution_);
679 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
681 const double point_x_arg,
const double point_y_arg,
682 const double point_z_arg,
OctreeKey & key_arg)
const
686 temp_point.x =
static_cast<float> (point_x_arg);
687 temp_point.y =
static_cast<float> (point_y_arg);
688 temp_point.z =
static_cast<float> (point_z_arg);
691 genOctreeKeyforPoint (temp_point, key_arg);
695 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
bool
698 const PointT temp_point = getPointByIndex (data_arg);
701 genOctreeKeyforPoint (temp_point, key_arg);
707 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
711 point.x =
static_cast<float> ((
static_cast<double> (key.
x) + 0.5f) * this->resolution_ + this->min_x_);
712 point.y =
static_cast<float> ((
static_cast<double> (key.
y) + 0.5f) * this->resolution_ + this->min_y_);
713 point.z =
static_cast<float> ((
static_cast<double> (key.
z) + 0.5f) * this->resolution_ + this->min_z_);
717 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
720 unsigned int tree_depth_arg,
724 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_);
725 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_);
726 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_);
730 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
733 unsigned int tree_depth_arg,
734 Eigen::Vector3f &min_pt,
735 Eigen::Vector3f &max_pt)
const
738 double voxel_side_len = this->resolution_ *
static_cast<double> (1 << (this->octree_depth_ - tree_depth_arg));
741 min_pt (0) =
static_cast<float> (
static_cast<double> (key_arg.
x) * voxel_side_len + this->min_x_);
742 min_pt (1) =
static_cast<float> (
static_cast<double> (key_arg.
y) * voxel_side_len + this->min_y_);
743 min_pt (2) =
static_cast<float> (
static_cast<double> (key_arg.
z) * voxel_side_len + this->min_z_);
745 max_pt (0) =
static_cast<float> (
static_cast<double> (key_arg.
x + 1) * voxel_side_len + this->min_x_);
746 max_pt (1) =
static_cast<float> (
static_cast<double> (key_arg.
y + 1) * voxel_side_len + this->min_y_);
747 max_pt (2) =
static_cast<float> (
static_cast<double> (key_arg.
z + 1) * voxel_side_len + this->min_z_);
751 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
double
757 side_len = this->resolution_ *
static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg));
760 side_len *= side_len;
766 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
double
770 return (getVoxelSquaredSideLen (tree_depth_arg) * 3);
774 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
int
781 unsigned char child_idx;
786 for (child_idx = 0; child_idx < 8; child_idx++)
788 if (!this->branchHasChild (*node_arg, child_idx))
792 child_node = this->getBranchChildPtr (*node_arg, child_idx);
796 new_key.
x = (key_arg.
x << 1) | (!!(child_idx & (1 << 2)));
797 new_key.
y = (key_arg.
y << 1) | (!!(child_idx & (1 << 1)));
798 new_key.
z = (key_arg.
z << 1) | (!!(child_idx & (1 << 0)));
805 voxel_count += getOccupiedVoxelCentersRecursive (static_cast<const BranchNode*> (child_node), new_key, voxel_center_list_arg);
812 genLeafNodeCenterFromOctreeKey (new_key, new_point);
813 voxel_center_list_arg.push_back (new_point);
822 return (voxel_count);
825 #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 > >;
826 #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 > >;
828 #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 > >;
829 #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 > >;
831 #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 > >;
832 #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 > >;
const PointT & getPointByIndex(const unsigned int index_arg) const
Get point at index from input pointcloud dataset.
void genOctreeKeyforPoint(const PointT &point_arg, OctreeKey &key_arg) const
Generate octree key for voxel at a given point.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
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.
static const unsigned char maxDepth
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
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
double getVoxelSquaredDiameter() const
Calculates the squared diameter of a voxel at leaf depth.
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 defineBoundingBox()
Investigate dimensions of pointcloud data set and define corresponding bounding box for octree...
int getOccupiedVoxelCenters(AlignedPointTVector &voxel_center_list_arg) const
Get a PointT vector of centers of all occupied voxels.
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.
unsigned char getChildIdxWithDepthMask(unsigned int depthMask) const
get child node index using depthMask
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.
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 genLeafNodeCenterFromOctreeKey(const OctreeKey &key_arg, PointT &point_arg) const
Generate a point at center of leaf node voxel.
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.
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.
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.
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.
virtual bool genOctreeKeyForDataT(const int &data_arg, OctreeKey &key_arg) const
Virtual method for generating octree key for a given point index.
bool isVoxelOccupiedAtPoint(const PointT &point_arg) const
Check if voxel at given point exist.
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