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 > >;
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 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.
void genOctreeKeyforPoint(const PointT &point_arg, OctreeKey &key_arg) const
Generate octree key for voxel at a given point.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
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.
void genLeafNodeCenterFromOctreeKey(const OctreeKey &key_arg, PointT &point_arg) const
Generate a point at center of leaf node voxel.
int getOccupiedVoxelCenters(AlignedPointTVector &voxel_center_list_arg) const
Get a PointT vector of centers of all occupied voxels.
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.
boost::shared_ptr< PointCloud > PointCloudPtr
static const unsigned char maxDepth
const PointT & getPointByIndex(const unsigned int index_arg) const
Get point at index from input pointcloud dataset.
bool isVoxelOccupiedAtPoint(const PointT &point_arg) const
Check if voxel at given point exist.
void adoptBoundingBoxToPoint(const PointT &point_idx_arg)
Grow the bounding box/octree until point fits.
boost::shared_ptr< std::vector< int > > IndicesPtr
void addPointFromCloud(const int point_idx_arg, IndicesPtr indices_arg)
Add point at given index from input point cloud to octree.
virtual void addPointIdx(const int point_idx_arg)
Add point at index from input pointcloud dataset to octree.
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)
Abstract octree node class
unsigned char getChildIdxWithDepthMask(unsigned int depthMask) const
get child node index using depthMask
double getVoxelSquaredDiameter() const
Calculates the squared diameter of a voxel at leaf depth.
void getKeyBitSize()
Define octree key setting and octree depth based on defined bounding box.
void defineBoundingBox()
Investigate dimensions of pointcloud data set and define corresponding 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 deleteVoxelAtPoint(const PointT &point_arg)
Delete leaf node / voxel at given point.
OctreeT::BranchNode BranchNode
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 addPointToCloud(const PointT &point_arg, PointCloudPtr cloud_arg)
Add point simultaneously to octree and input point cloud.
virtual ~OctreePointCloud()
Empty deconstructor.
OctreeT::LeafNode LeafNode
double getVoxelSquaredSideLen() const
Calculates the squared voxel cube side length at leaf level.
OctreePointCloud(const double resolution_arg)
Octree pointcloud constructor.
boost::shared_ptr< const PointCloud > PointCloudConstPtr
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.
virtual bool genOctreeKeyForDataT(const int &data_arg, OctreeKey &key_arg) const
Virtual method for generating octree key for a given point index.
boost::shared_ptr< const std::vector< int > > IndicesConstPtr
void addPointsFromInputCloud()
Add points from input point cloud to 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.
A point structure representing Euclidean xyz coordinates, and the RGB color.