42#include <pcl/common/point_tests.h>
43#include <pcl/octree/impl/octree_base.hpp>
50 typename LeafContainerT,
51 typename BranchContainerT,
59, resolution_(resolution)
66, bounding_box_defined_(
false)
67, max_objs_per_leaf_(0)
74 typename LeafContainerT,
82 for (
const auto& index : *indices_) {
83 assert((index >= 0) && (
static_cast<std::size_t
>(index) < input_->size()));
87 this->addPointIdx(index);
103 typename LeafContainerT,
104 typename BranchContainerT,
117 typename LeafContainerT,
133 typename LeafContainerT,
152 typename LeafContainerT,
159 if (!isPointWithinBoundingBox(
point_arg)) {
166 this->genOctreeKeyforPoint(
point_arg, key);
169 return (this->existLeaf(key));
174 typename LeafContainerT,
185 return (this->isVoxelOccupiedAtPoint(point));
190 typename LeafContainerT,
206 return (this->isVoxelOccupiedAtPoint(point));
211 typename LeafContainerT,
218 if (!isPointWithinBoundingBox(
point_arg)) {
225 this->genOctreeKeyforPoint(
point_arg, key);
227 this->removeLeaf(key);
232 typename LeafContainerT,
243 this->deleteVoxelAtPoint(point);
248 typename LeafContainerT,
256 key.
x = key.
y = key.
z = 0;
265 typename LeafContainerT,
271 const Eigen::Vector3f& end,
275 Eigen::Vector3f direction = end -
origin;
276 float norm = direction.norm();
277 direction.normalize();
279 const float step_size =
static_cast<float>(resolution_) *
precision;
281 const auto nsteps = std::max<std::size_t>(1, norm / step_size);
288 for (std::size_t i = 0; i <
nsteps; ++i) {
289 Eigen::Vector3f p =
origin + (direction * step_size *
static_cast<float>(i));
297 this->genOctreeKeyforPoint(
octree_p, key);
307 genLeafNodeCenterFromOctreeKey(key,
center);
328 typename LeafContainerT,
342 assert(this->leaf_count_ == 0);
346 float minValue = std::numeric_limits<float>::epsilon() * 512.0f;
362 typename LeafContainerT,
375 assert(this->leaf_count_ == 0);
390 min_x_ = std::min(min_x_, max_x_);
391 min_y_ = std::min(min_y_, max_y_);
392 min_z_ = std::min(min_z_, max_z_);
394 max_x_ = std::max(min_x_, max_x_);
395 max_y_ = std::max(min_y_, max_y_);
396 max_z_ = std::max(min_z_, max_z_);
401 bounding_box_defined_ =
true;
406 typename LeafContainerT,
416 assert(this->leaf_count_ == 0);
431 min_x_ = std::min(min_x_, max_x_);
432 min_y_ = std::min(min_y_, max_y_);
433 min_z_ = std::min(min_z_, max_z_);
435 max_x_ = std::max(min_x_, max_x_);
436 max_y_ = std::max(min_y_, max_y_);
437 max_z_ = std::max(min_z_, max_z_);
442 bounding_box_defined_ =
true;
447 typename LeafContainerT,
455 assert(this->leaf_count_ == 0);
468 min_x_ = std::min(min_x_, max_x_);
469 min_y_ = std::min(min_y_, max_y_);
470 min_z_ = std::min(min_z_, max_z_);
472 max_x_ = std::max(min_x_, max_x_);
473 max_y_ = std::max(min_y_, max_y_);
474 max_z_ = std::max(min_z_, max_z_);
479 bounding_box_defined_ =
true;
484 typename LeafContainerT,
507 typename LeafContainerT,
515 const float minValue = std::numeric_limits<float>::epsilon();
530 (!bounding_box_defined_)) {
532 if (bounding_box_defined_) {
546 this->branch_count_++;
552 octreeSideLen =
static_cast<double>(1 << this->octree_depth_) * resolution_;
564 this->octree_depth_++;
565 this->setTreeDepth(this->octree_depth_);
569 static_cast<double>(1 << this->octree_depth_) * resolution_ -
minValue;
589 bounding_box_defined_ =
true;
600 typename LeafContainerT,
601 typename BranchContainerT,
627 this->branch_count_++;
640 this->createLeafRecursive(
650 typename LeafContainerT,
664 adoptBoundingBoxToPoint(point);
667 genOctreeKeyforPoint(point, key);
674 if (this->dynamic_depth_enabled_ &&
depth_mask) {
698 typename LeafContainerT,
712 typename LeafContainerT,
719 const float minValue = std::numeric_limits<float>::epsilon();
723 static_cast<uindex_t>(std::ceil((max_x_ - min_x_ -
minValue) / resolution_));
725 static_cast<uindex_t>(std::ceil((max_y_ - min_y_ -
minValue) / resolution_));
727 static_cast<uindex_t>(std::ceil((max_z_ - min_z_ -
minValue) / resolution_));
734 this->octree_depth_ = std::max<uindex_t>(
740 static_cast<double>(1 << this->octree_depth_) * resolution_;
742 if (this->leaf_count_ == 0) {
775 this->setTreeDepth(this->octree_depth_);
780 typename LeafContainerT,
799 typename LeafContainerT,
821 typename LeafContainerT,
838 typename LeafContainerT,
846 point.x =
static_cast<float>((
static_cast<double>(key.
x) + 0.5f) * this->resolution_ +
848 point.y =
static_cast<float>((
static_cast<double>(key.
y) + 0.5f) * this->resolution_ +
850 point.z =
static_cast<float>((
static_cast<double>(key.
z) + 0.5f) * this->resolution_ +
856 typename LeafContainerT,
867 (
static_cast<double>(
key_arg.x) + 0.5f) *
869 static_cast<double>(1 << (this->octree_depth_ -
tree_depth_arg))) +
872 (
static_cast<double>(
key_arg.y) + 0.5f) *
874 static_cast<double>(1 << (this->octree_depth_ -
tree_depth_arg))) +
877 (
static_cast<double>(
key_arg.z) + 0.5f) *
879 static_cast<double>(1 << (this->octree_depth_ -
tree_depth_arg))) +
885 typename LeafContainerT,
893 Eigen::Vector3f&
max_pt)
const
918 typename LeafContainerT,
939 typename LeafContainerT,
952 typename LeafContainerT,
1000#define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataTVector(T) \
1001 template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1003 pcl::octree::OctreeContainerPointIndices, \
1004 pcl::octree::OctreeContainerEmpty, \
1005 pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndices, \
1006 pcl::octree::OctreeContainerEmpty>>;
1007#define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataTVector(T) \
1008 template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1010 pcl::octree::OctreeContainerPointIndices, \
1011 pcl::octree::OctreeContainerEmpty, \
1012 pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerPointIndices, \
1013 pcl::octree::OctreeContainerEmpty>>;
1015#define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataT(T) \
1016 template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1018 pcl::octree::OctreeContainerPointIndex, \
1019 pcl::octree::OctreeContainerEmpty, \
1020 pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndex, \
1021 pcl::octree::OctreeContainerEmpty>>;
1022#define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataT(T) \
1023 template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1025 pcl::octree::OctreeContainerPointIndex, \
1026 pcl::octree::OctreeContainerEmpty, \
1027 pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerPointIndex, \
1028 pcl::octree::OctreeContainerEmpty>>;
1030#define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithEmptyLeaf(T) \
1031 template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1033 pcl::octree::OctreeContainerEmpty, \
1034 pcl::octree::OctreeContainerEmpty, \
1035 pcl::octree::OctreeBase<pcl::octree::OctreeContainerEmpty, \
1036 pcl::octree::OctreeContainerEmpty>>;
1037#define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithEmptyLeaf(T) \
1038 template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1040 pcl::octree::OctreeContainerEmpty, \
1041 pcl::octree::OctreeContainerEmpty, \
1042 pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerEmpty, \
1043 pcl::octree::OctreeContainerEmpty>>;
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
static const unsigned char maxDepth
unsigned char getChildIdxWithDepthMask(uindex_t depthMask) const
get child node index using depthMask
Abstract octree node class
double getVoxelSquaredDiameter() const
Calculates the squared diameter of a voxel at leaf depth.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
void adoptBoundingBoxToPoint(const PointT &point_idx_arg)
Grow the bounding box/octree until point fits.
uindex_t getOccupiedVoxelCenters(AlignedPointTVector &voxel_center_list_arg) const
Get a PointT vector of centers of all occupied voxels.
const PointT & getPointByIndex(uindex_t index_arg) const
Get point at index from input pointcloud dataset.
void expandLeafNode(LeafNode *leaf_node, BranchNode *parent_branch, unsigned char child_idx, uindex_t depth_mask)
Add point at index from input pointcloud dataset to octree.
void deleteVoxelAtPoint(const PointT &point_arg)
Delete leaf node / voxel at given point.
void genLeafNodeCenterFromOctreeKey(const OctreeKey &key_arg, PointT &point_arg) const
Generate a point at center of leaf node voxel.
void genVoxelCenterFromOctreeKey(const OctreeKey &key_arg, uindex_t tree_depth_arg, PointT &point_arg) const
Generate a point at center of octree voxel at given tree level.
typename PointCloud::Ptr PointCloudPtr
void addPointFromCloud(uindex_t point_idx_arg, IndicesPtr indices_arg)
Add point at given index from input point cloud to octree.
OctreePointCloud(const double resolution_arg)
Octree pointcloud constructor.
shared_ptr< Indices > IndicesPtr
void addPointToCloud(const PointT &point_arg, PointCloudPtr cloud_arg)
Add point simultaneously to octree and input point cloud.
typename PointCloud::ConstPtr PointCloudConstPtr
void defineBoundingBox()
Investigate dimensions of pointcloud data set and define corresponding bounding box for octree.
void genVoxelBoundsFromOctreeKey(const OctreeKey &key_arg, uindex_t 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.
typename OctreeT::LeafNode LeafNode
virtual void addPointIdx(uindex_t point_idx_arg)
Add point at index from input pointcloud dataset to octree.
double getVoxelSquaredSideLen() const
Calculates the squared voxel cube side length at leaf level.
bool isVoxelOccupiedAtPoint(const PointT &point_arg) const
Check if voxel at given point exist.
void addPointsFromInputCloud()
Add points from input point cloud to octree.
uindex_t 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.
typename OctreeT::BranchNode BranchNode
void genOctreeKeyforPoint(const PointT &point_arg, OctreeKey &key_arg) const
Generate octree key for voxel at a given point.
shared_ptr< const Indices > IndicesConstPtr
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.
uindex_t 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 bool genOctreeKeyForDataT(const index_t &data_arg, OctreeKey &key_arg) const
Virtual method for generating octree key for a given point index.
void getKeyBitSize()
Define octree key setting and octree depth based on defined bounding box.
Define standard C methods and C++ classes that are common to all methods.
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.
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...
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
A point structure representing Euclidean xyz coordinates, and the RGB color.
Defines basic non-point types used by PCL.