39 #ifndef PCL_OCTREE_POINTCLOUD_HPP_
40 #define PCL_OCTREE_POINTCLOUD_HPP_
45 #include <pcl/common/common.h>
50 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
53 epsilon_ (0), resolution_ (resolution), min_x_ (0.0f), max_x_ (resolution), min_y_ (0.0f),
54 max_y_ (resolution), min_z_ (0.0f), max_z_ (resolution), bounding_box_defined_ (false), max_objs_per_leaf_(0)
56 assert (resolution > 0.0f);
60 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
66 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
73 for (std::vector<int>::const_iterator current = indices_->begin (); current != indices_->end (); ++current)
75 assert( (*current>=0) && (*current < static_cast<int> (input_->points.size ())));
77 if (
isFinite (input_->points[*current]))
80 this->addPointIdx (*current);
86 for (i = 0; i < input_->points.size (); i++)
91 this->addPointIdx (static_cast<unsigned int> (i));
98 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
101 this->addPointIdx (point_idx_arg);
103 indices_arg->push_back (point_idx_arg);
107 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
110 assert (cloud_arg==input_);
112 cloud_arg->push_back (point_arg);
114 this->addPointIdx (static_cast<const int> (cloud_arg->points.size ()) - 1);
118 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
122 assert (cloud_arg==input_);
123 assert (indices_arg==indices_);
125 cloud_arg->push_back (point_arg);
127 this->addPointFromCloud (static_cast<const int> (cloud_arg->points.size ()) - 1, indices_arg);
131 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
bool
137 this->genOctreeKeyforPoint (point_arg, key);
140 return (isPointWithinBoundingBox (point_arg) && this->existLeaf (key));
144 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
bool
148 const PointT& point = this->input_->points[point_idx_arg];
151 return (this->isVoxelOccupiedAtPoint (point));
155 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
bool
157 const double point_x_arg,
const double point_y_arg,
const double point_z_arg)
const
162 this->genOctreeKeyforPoint (point_x_arg, point_y_arg, point_z_arg, key);
164 return (this->existLeaf (key));
168 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
174 this->genOctreeKeyforPoint (point_arg, key);
176 this->removeLeaf (key);
180 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
184 const PointT& point = this->input_->points[point_idx_arg];
187 this->deleteVoxelAtPoint (point);
191 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
int
196 key.
x = key.
y = key.
z = 0;
198 voxel_center_list_arg.clear ();
200 return getOccupiedVoxelCentersRecursive (this->root_node_, key, voxel_center_list_arg);
205 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
int
207 const Eigen::Vector3f& origin,
208 const Eigen::Vector3f& end,
212 Eigen::Vector3f direction = end - origin;
213 float norm = direction.norm ();
214 direction.normalize ();
216 const float step_size =
static_cast<const float> (resolution_) * precision;
218 const int nsteps = std::max (1, static_cast<int> (norm / step_size));
222 bool bkeyDefined =
false;
225 for (
int i = 0; i < nsteps; ++i)
227 Eigen::Vector3f p = origin + (direction * step_size *
static_cast<const float> (i));
235 this->genOctreeKeyforPoint (octree_p, key);
238 if ((key == prev_key) && (bkeyDefined) )
245 genLeafNodeCenterFromOctreeKey (key, center);
246 voxel_center_list.push_back (center);
254 this->genOctreeKeyforPoint (end_p, end_key);
255 if (!(end_key == prev_key))
258 genLeafNodeCenterFromOctreeKey (end_key, center);
259 voxel_center_list.push_back (center);
262 return (static_cast<int> (voxel_center_list.size ()));
266 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
270 double minX, minY, minZ, maxX, maxY, maxZ;
276 assert (this->leaf_count_ == 0);
280 float minValue = std::numeric_limits<float>::epsilon () * 512.0f;
286 maxX = max_pt.x + minValue;
287 maxY = max_pt.y + minValue;
288 maxZ = max_pt.z + minValue;
291 defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);
295 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
297 const double min_y_arg,
298 const double min_z_arg,
299 const double max_x_arg,
300 const double max_y_arg,
301 const double max_z_arg)
304 assert (this->leaf_count_ == 0);
306 assert (max_x_arg >= min_x_arg);
307 assert (max_y_arg >= min_y_arg);
308 assert (max_z_arg >= min_z_arg);
319 min_x_ = min (min_x_, max_x_);
320 min_y_ = min (min_y_, max_y_);
321 min_z_ = min (min_z_, max_z_);
323 max_x_ = max (min_x_, max_x_);
324 max_y_ = max (min_y_, max_y_);
325 max_z_ = max (min_z_, max_z_);
330 bounding_box_defined_ =
true;
334 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
336 const double max_x_arg,
const double max_y_arg,
const double max_z_arg)
339 assert (this->leaf_count_ == 0);
341 assert (max_x_arg >= 0.0f);
342 assert (max_y_arg >= 0.0f);
343 assert (max_z_arg >= 0.0f);
354 min_x_ = min (min_x_, max_x_);
355 min_y_ = min (min_y_, max_y_);
356 min_z_ = min (min_z_, max_z_);
358 max_x_ = max (min_x_, max_x_);
359 max_y_ = max (min_y_, max_y_);
360 max_z_ = max (min_z_, max_z_);
365 bounding_box_defined_ =
true;
369 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
373 assert (this->leaf_count_ == 0);
375 assert (cubeLen_arg >= 0.0f);
378 max_x_ = cubeLen_arg;
381 max_y_ = cubeLen_arg;
384 max_z_ = cubeLen_arg;
386 min_x_ = min (min_x_, max_x_);
387 min_y_ = min (min_y_, max_y_);
388 min_z_ = min (min_z_, max_z_);
390 max_x_ = max (min_x_, max_x_);
391 max_y_ = max (min_y_, max_y_);
392 max_z_ = max (min_z_, max_z_);
397 bounding_box_defined_ =
true;
401 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
403 double& min_x_arg,
double& min_y_arg,
double& min_z_arg,
404 double& max_x_arg,
double& max_y_arg,
double& max_z_arg)
const
417 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
422 const float minValue = std::numeric_limits<float>::epsilon ();
427 bool bLowerBoundViolationX = (point_idx_arg.x < min_x_);
428 bool bLowerBoundViolationY = (point_idx_arg.y < min_y_);
429 bool bLowerBoundViolationZ = (point_idx_arg.z < min_z_);
431 bool bUpperBoundViolationX = (point_idx_arg.x >= max_x_);
432 bool bUpperBoundViolationY = (point_idx_arg.y >= max_y_);
433 bool bUpperBoundViolationZ = (point_idx_arg.z >= max_z_);
436 if (bLowerBoundViolationX || bLowerBoundViolationY || bLowerBoundViolationZ || bUpperBoundViolationX
437 || bUpperBoundViolationY || bUpperBoundViolationZ || (!bounding_box_defined_) )
440 if (bounding_box_defined_)
443 double octreeSideLen;
444 unsigned char child_idx;
447 child_idx =
static_cast<unsigned char> (((!bUpperBoundViolationX) << 2) | ((!bUpperBoundViolationY) << 1)
448 | ((!bUpperBoundViolationZ)));
453 this->branch_count_++;
455 this->setBranchChildPtr (*newRootBranch, child_idx, this->root_node_);
457 this->root_node_ = newRootBranch;
459 octreeSideLen =
static_cast<double> (1 << this->octree_depth_) * resolution_;
461 if (!bUpperBoundViolationX)
462 min_x_ -= octreeSideLen;
464 if (!bUpperBoundViolationY)
465 min_y_ -= octreeSideLen;
467 if (!bUpperBoundViolationZ)
468 min_z_ -= octreeSideLen;
471 this->octree_depth_++;
472 this->setTreeDepth (this->octree_depth_);
475 octreeSideLen =
static_cast<double> (1 << this->octree_depth_) * resolution_ - minValue;
478 max_x_ = min_x_ + octreeSideLen;
479 max_y_ = min_y_ + octreeSideLen;
480 max_z_ = min_z_ + octreeSideLen;
487 this->min_x_ = point_idx_arg.x - this->resolution_ / 2;
488 this->min_y_ = point_idx_arg.y - this->resolution_ / 2;
489 this->min_z_ = point_idx_arg.z - this->resolution_ / 2;
491 this->max_x_ = point_idx_arg.x + this->resolution_ / 2;
492 this->max_y_ = point_idx_arg.y + this->resolution_ / 2;
493 this->max_z_ = point_idx_arg.z + this->resolution_ / 2;
497 bounding_box_defined_ =
true;
508 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
515 size_t leaf_obj_count = (*leaf_node)->getSize ();
518 std::vector<int> leafIndices;
519 leafIndices.reserve(leaf_obj_count);
521 (*leaf_node)->getPointIndices(leafIndices);
524 this->deleteBranchChild(*parent_branch, child_idx);
525 this->leaf_count_ --;
528 BranchNode* childBranch = this->createBranchChild (*parent_branch, child_idx);
529 this->branch_count_ ++;
531 typename std::vector<int>::iterator it = leafIndices.begin();
532 typename std::vector<int>::const_iterator it_end = leafIndices.end();
537 for (it = leafIndices.begin(); it!=it_end; ++it)
540 const PointT& point_from_index = input_->points[*it];
542 genOctreeKeyforPoint (point_from_index, new_index_key);
546 this->createLeafRecursive (new_index_key, depth_mask, childBranch, newLeaf, newBranchParent);
548 (*newLeaf)->addPointIndex(*it);
557 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
562 assert (point_idx_arg < static_cast<int> (input_->points.size ()));
564 const PointT& point = input_->points[point_idx_arg];
567 adoptBoundingBoxToPoint (point);
570 genOctreeKeyforPoint (point, key);
574 unsigned int depth_mask = this->createLeafRecursive (key, this->depth_mask_ ,this->root_node_, leaf_node, parent_branch_of_leaf_node);
576 if (this->dynamic_depth_enabled_ && depth_mask)
579 size_t leaf_obj_count = (*leaf_node)->getSize ();
581 while (leaf_obj_count>=max_objs_per_leaf_ && depth_mask)
586 expandLeafNode (leaf_node,
587 parent_branch_of_leaf_node,
591 depth_mask = this->createLeafRecursive (key, this->depth_mask_ ,this->root_node_, leaf_node, parent_branch_of_leaf_node);
592 leaf_obj_count = (*leaf_node)->getSize ();
597 (*leaf_node)->addPointIndex (point_idx_arg);
601 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
const PointT&
605 assert (index_arg < static_cast<unsigned int> (input_->points.size ()));
606 return (this->input_->points[index_arg]);
610 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
613 unsigned int max_voxels;
615 unsigned int max_key_x;
616 unsigned int max_key_y;
617 unsigned int max_key_z;
619 double octree_side_len;
621 const float minValue = std::numeric_limits<float>::epsilon();
624 max_key_x =
static_cast<unsigned int> ((max_x_ - min_x_) / resolution_);
625 max_key_y =
static_cast<unsigned int> ((max_y_ - min_y_) / resolution_);
626 max_key_z =
static_cast<unsigned int> ((max_z_ - min_z_) / resolution_);
629 max_voxels = max (max (max (max_key_x, max_key_y), max_key_z), static_cast<unsigned int> (2));
633 this->octree_depth_ = max ((min (static_cast<unsigned int> (
OctreeKey::maxDepth), static_cast<unsigned int> (ceil (this->Log2 (max_voxels)-minValue)))),
634 static_cast<unsigned int> (0));
636 octree_side_len =
static_cast<double> (1 << this->octree_depth_) * resolution_-minValue;
638 if (this->leaf_count_ == 0)
640 double octree_oversize_x;
641 double octree_oversize_y;
642 double octree_oversize_z;
644 octree_oversize_x = (octree_side_len - (max_x_ - min_x_)) / 2.0;
645 octree_oversize_y = (octree_side_len - (max_y_ - min_y_)) / 2.0;
646 octree_oversize_z = (octree_side_len - (max_z_ - min_z_)) / 2.0;
648 min_x_ -= octree_oversize_x;
649 min_y_ -= octree_oversize_y;
650 min_z_ -= octree_oversize_z;
652 max_x_ += octree_oversize_x;
653 max_y_ += octree_oversize_y;
654 max_z_ += octree_oversize_z;
658 max_x_ = min_x_ + octree_side_len;
659 max_y_ = min_y_ + octree_side_len;
660 max_z_ = min_z_ + octree_side_len;
664 this->setTreeDepth (this->octree_depth_);
669 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
674 key_arg.
x =
static_cast<unsigned int> ((point_arg.x - this->min_x_) / this->resolution_);
675 key_arg.
y =
static_cast<unsigned int> ((point_arg.y - this->min_y_) / this->resolution_);
676 key_arg.
z =
static_cast<unsigned int> ((point_arg.z - this->min_z_) / this->resolution_);
680 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
682 const double point_x_arg,
const double point_y_arg,
683 const double point_z_arg,
OctreeKey & key_arg)
const
687 temp_point.x =
static_cast<float> (point_x_arg);
688 temp_point.y =
static_cast<float> (point_y_arg);
689 temp_point.z =
static_cast<float> (point_z_arg);
692 genOctreeKeyforPoint (temp_point, key_arg);
696 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
bool
699 const PointT temp_point = getPointByIndex (data_arg);
702 genOctreeKeyforPoint (temp_point, key_arg);
708 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
712 point.x =
static_cast<float> ((
static_cast<double> (key.
x) + 0.5f) * this->resolution_ + this->min_x_);
713 point.y =
static_cast<float> ((
static_cast<double> (key.
y) + 0.5f) * this->resolution_ + this->min_y_);
714 point.z =
static_cast<float> ((
static_cast<double> (key.
z) + 0.5f) * this->resolution_ + this->min_z_);
718 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
721 unsigned int tree_depth_arg,
725 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_);
726 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_);
727 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_);
731 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
734 unsigned int tree_depth_arg,
735 Eigen::Vector3f &min_pt,
736 Eigen::Vector3f &max_pt)
const
739 double voxel_side_len = this->resolution_ *
static_cast<double> (1 << (this->octree_depth_ - tree_depth_arg));
742 min_pt (0) =
static_cast<float> (
static_cast<double> (key_arg.
x) * voxel_side_len + this->min_x_);
743 min_pt (1) =
static_cast<float> (
static_cast<double> (key_arg.
y) * voxel_side_len + this->min_y_);
744 min_pt (2) =
static_cast<float> (
static_cast<double> (key_arg.
z) * voxel_side_len + this->min_z_);
746 max_pt (0) =
static_cast<float> (
static_cast<double> (key_arg.
x + 1) * voxel_side_len + this->min_x_);
747 max_pt (1) =
static_cast<float> (
static_cast<double> (key_arg.
y + 1) * voxel_side_len + this->min_y_);
748 max_pt (2) =
static_cast<float> (
static_cast<double> (key_arg.
z + 1) * voxel_side_len + this->min_z_);
752 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
double
758 side_len = this->resolution_ *
static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg));
761 side_len *= side_len;
767 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
double
771 return (getVoxelSquaredSideLen (tree_depth_arg) * 3);
775 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
int
782 unsigned char child_idx;
787 for (child_idx = 0; child_idx < 8; child_idx++)
789 if (!this->branchHasChild (*node_arg, child_idx))
793 child_node = this->getBranchChildPtr (*node_arg, child_idx);
797 new_key.
x = (key_arg.
x << 1) | (!!(child_idx & (1 << 2)));
798 new_key.
y = (key_arg.
y << 1) | (!!(child_idx & (1 << 1)));
799 new_key.
z = (key_arg.
z << 1) | (!!(child_idx & (1 << 0)));
806 voxel_count += getOccupiedVoxelCentersRecursive (static_cast<const BranchNode*> (child_node), new_key, voxel_center_list_arg);
813 genLeafNodeCenterFromOctreeKey (new_key, new_point);
814 voxel_center_list_arg.push_back (new_point);
823 return (voxel_count);
826 #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 > >;
827 #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 > >;
829 #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 > >;
830 #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 > >;
832 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafEmpty, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeBase<pcl::octree::OctreeLeafEmpty, pcl::octree::OctreeContainerEmpty > >;
833 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafEmpty, pcl::octree::OctreeContainerEmpty, pcl::octree::Octree2BufBase<pcl::octree::OctreeLeafEmpty, pcl::octree::OctreeContainerEmpty > >;
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 genOctreeKeyforPoint(const PointT &point_arg, OctreeKey &key_arg) const
Generate octree key for voxel at a given point.
void genLeafNodeCenterFromOctreeKey(const OctreeKey &key_arg, PointT &point_arg) const
Generate a point at center of leaf node voxel.
boost::shared_ptr< PointCloud > PointCloudPtr
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...
const PointT & getPointByIndex(const unsigned int index_arg) const
Get point at index from input pointcloud dataset.
void getKeyBitSize()
Define octree key setting and octree depth based on defined bounding box.
void adoptBoundingBoxToPoint(const PointT &point_idx_arg)
Grow the bounding box/octree until point fits.
boost::shared_ptr< std::vector< int > > IndicesPtr
unsigned char getChildIdxWithDepthMask(unsigned int depthMask) const
get child node index using depthMask
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.
Abstract octree node class
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
void addPointFromCloud(const int point_idx_arg, IndicesPtr indices_arg)
Add point at given index from input point cloud to octree.
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.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
void addPointToCloud(const PointT &point_arg, PointCloudPtr cloud_arg)
Add point simultaneously to octree and input point cloud.
void deleteVoxelAtPoint(const PointT &point_arg)
Delete leaf node / voxel at given point.
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.
void addPointsFromInputCloud()
Add points from input point cloud 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
double getVoxelSquaredSideLen() const
Calculates the squared voxel cube side length at leaf level.
virtual void addPointIdx(const int point_idx_arg)
Add point at index from input pointcloud dataset to octree.
void defineBoundingBox()
Investigate dimensions of pointcloud data set and define corresponding bounding box for octree...
boost::shared_ptr< const PointCloud > PointCloudConstPtr
OctreeT::BranchNode BranchNode
static const unsigned char maxDepth
virtual ~OctreePointCloud()
Empty deconstructor.
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.
boost::shared_ptr< const std::vector< int > > IndicesConstPtr
double getVoxelSquaredDiameter() const
Calculates the squared diameter of a voxel at leaf depth.
A point structure representing Euclidean xyz coordinates, and the RGB color.
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.
OctreePointCloud(const double resolution_arg)
Octree pointcloud constructor.
int getOccupiedVoxelCenters(AlignedPointTVector &voxel_center_list_arg) const
Get a PointT vector of centers of all occupied voxels.
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)