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 ())));
85 for (i = 0; i <
input_->points.size (); i++)
97 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 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_);
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 143 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
bool 147 const PointT& point = this->
input_->points[point_idx_arg];
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 163 return (this->existLeaf (key));
167 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 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];
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 ();
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));
237 if ((key == prev_key) && (bkeyDefined) )
245 voxel_center_list.push_back (center);
254 if (!(end_key == prev_key))
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;
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);
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);
368 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void 372 assert (this->leaf_count_ == 0);
374 assert (cubeLen_arg >= 0.0f);
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
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)
463 if (!bUpperBoundViolationY)
466 if (!bUpperBoundViolationZ)
470 this->octree_depth_++;
471 this->setTreeDepth (this->octree_depth_);
474 octreeSideLen =
static_cast<double> (1 << this->octree_depth_) *
resolution_ - minValue;
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)
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 ()));
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 ();
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();
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;
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);
695 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
bool 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 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)));
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.
IndicesConstPtr indices_
A pointer to the vector of point indices to use.
PointCloudConstPtr input_
Pointer to input point cloud dataset.
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.
bool isPointWithinBoundingBox(const PointT &point_idx_arg) const
Checks if given point is within the bounding box of the octree.
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.
std::size_t max_objs_per_leaf_
Amount of DataT objects per leafNode before expanding branch.
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.
bool bounding_box_defined_
Flag indicating if octree has defined bounding box.
Abstract octree node class
double resolution_
Octree resolution.