40 #ifndef PCL_OUTOFCORE_OCTREE_BASE_IMPL_H_
41 #define PCL_OUTOFCORE_OCTREE_BASE_IMPL_H_
44 #include <pcl/outofcore/octree_base.h>
47 #include <pcl/outofcore/cJSON.h>
49 #include <pcl/filters/random_sample.h>
50 #include <pcl/filters/extract_indices.h>
68 template<
typename ContainerT,
typename Po
intT>
71 template<
typename ContainerT,
typename Po
intT>
76 template<
typename ContainerT,
typename Po
intT>
79 , read_write_mutex_ ()
81 , sample_percent_ (0.125)
87 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBase] Bad extension. Outofcore Octrees must have a root node ending in .oct_idx\n");
96 boost::filesystem::path treepath = root_name.parent_path () / (boost::filesystem::basename (root_name) +
TREE_EXTENSION_);
99 metadata_->loadMetadataFromDisk (treepath);
104 template<
typename ContainerT,
typename Po
intT>
107 , read_write_mutex_ ()
109 , sample_percent_ (0.125)
113 Eigen::Vector3d tmp_min = min;
114 Eigen::Vector3d tmp_max = max;
115 this->enlargeToCube (tmp_min, tmp_max);
118 std::uint64_t depth = this->calculateDepth (tmp_min, tmp_max, resolution_arg);
121 this->
init (depth, tmp_min, tmp_max, root_node_name, coord_sys);
126 template<
typename ContainerT,
typename Po
intT>
129 , read_write_mutex_ ()
131 , sample_percent_ (0.125)
135 this->
init (max_depth, min, max, root_node_name, coord_sys);
139 template<
typename ContainerT,
typename Po
intT>
void
143 if (!this->checkExtension (root_name))
145 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBase] Bad extension. Outofcore Octrees must have a root node ending in .oct_idx\n");
149 if (boost::filesystem::exists (root_name.parent_path ()))
151 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBase] A dir named %s already exists. Overwriting an existing tree is not supported.\n", root_name.parent_path ().c_str () );
152 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBase] Directory exists; Overwriting an existing tree is not supported\n");
156 boost::filesystem::path dir = root_name.parent_path ();
158 if (!boost::filesystem::exists (dir))
160 boost::filesystem::create_directory (dir);
163 Eigen::Vector3d tmp_min = min;
164 Eigen::Vector3d tmp_max = max;
165 this->enlargeToCube (tmp_min, tmp_max);
169 root_node_->m_tree_ =
this;
172 boost::filesystem::path treepath = dir / (boost::filesystem::basename (root_name) + TREE_EXTENSION_);
175 metadata_->setCoordinateSystem (coord_sys);
176 metadata_->setDepth (depth);
177 metadata_->setLODPoints (depth+1);
178 metadata_->setMetadataFilename (treepath);
179 metadata_->setOutofcoreVersion (OUTOFCORE_VERSION_);
183 metadata_->serializeMetadataToDisk ();
188 template<
typename ContainerT,
typename Po
intT>
191 root_node_->flushToDiskRecursive ();
199 template<
typename ContainerT,
typename Po
intT>
void
202 this->metadata_->serializeMetadataToDisk ();
207 template<
typename ContainerT,
typename Po
intT> std::uint64_t
210 std::unique_lock < std::shared_timed_mutex > lock (read_write_mutex_);
212 constexpr
bool _FORCE_BB_CHECK =
true;
214 std::uint64_t pt_added = root_node_->addDataToLeaf (p, _FORCE_BB_CHECK);
216 assert (p.size () == pt_added);
223 template<
typename ContainerT,
typename Po
intT> std::uint64_t
226 return (addDataToLeaf (point_cloud->points));
231 template<
typename ContainerT,
typename Po
intT> std::uint64_t
234 std::uint64_t pt_added = this->root_node_->addPointCloud (input_cloud, skip_bb_check) ;
242 template<
typename ContainerT,
typename Po
intT> std::uint64_t
246 std::unique_lock < std::shared_timed_mutex > lock (read_write_mutex_);
247 std::uint64_t pt_added = root_node_->addDataToLeaf_and_genLOD (point_cloud->points,
false);
253 template<
typename ContainerT,
typename Po
intT> std::uint64_t
257 std::unique_lock < std::shared_timed_mutex > lock (read_write_mutex_);
258 std::uint64_t pt_added = root_node_->addPointCloud_and_genLOD (input_cloud);
260 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBase::%s] Points added %lu, points in input cloud, %lu\n",__FUNCTION__, pt_added, input_cloud->width*input_cloud->height );
262 assert ( input_cloud->width*input_cloud->height == pt_added );
269 template<
typename ContainerT,
typename Po
intT> std::uint64_t
273 std::unique_lock < std::shared_timed_mutex > lock (read_write_mutex_);
274 std::uint64_t pt_added = root_node_->addDataToLeaf_and_genLOD (src,
false);
280 template<
typename Container,
typename Po
intT>
void
283 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
284 root_node_->queryFrustum (planes, file_names, this->getTreeDepth());
289 template<
typename Container,
typename Po
intT>
void
292 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
293 root_node_->queryFrustum (planes, file_names, query_depth);
298 template<
typename Container,
typename Po
intT>
void
300 const double *planes,
301 const Eigen::Vector3d &eye,
302 const Eigen::Matrix4d &view_projection_matrix,
303 std::list<std::string>& file_names,
304 const std::uint32_t query_depth)
const
306 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
307 root_node_->queryFrustum (planes, eye, view_projection_matrix, file_names, query_depth);
312 template<
typename ContainerT,
typename Po
intT>
void
315 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
317 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Querying Bounding Box %.2lf %.2lf %.2lf, %.2lf %.2lf %.2lf", min[0], min[1], min[2], max[0], max[1], max[2]);
318 root_node_->queryBBIncludes (min, max, query_depth, dst);
323 template<
typename ContainerT,
typename Po
intT>
void
326 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
328 dst_blob->data.clear ();
332 root_node_->queryBBIncludes ( min, max, query_depth, dst_blob );
337 template<
typename ContainerT,
typename Po
intT>
void
340 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
342 root_node_->queryBBIncludes_subsample (min, max, query_depth, percent, dst);
346 template<
typename ContainerT,
typename Po
intT>
void
351 root_node_->queryBBIncludes (min, max, query_depth, dst_blob);
355 root_node_->queryBBIncludes_subsample (min, max, query_depth, dst_blob, percent);
361 template<
typename ContainerT,
typename Po
intT>
bool
364 if (root_node_!=
nullptr)
366 root_node_->getBoundingBox (min, max);
374 template<
typename ContainerT,
typename Po
intT>
void
377 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
378 root_node_->printBoundingBox (query_depth);
383 template<
typename ContainerT,
typename Po
intT>
void
386 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
387 if (query_depth > metadata_->getDepth ())
389 root_node_->getOccupiedVoxelCentersRecursive (voxel_centers, metadata_->getDepth ());
393 root_node_->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
399 template<
typename ContainerT,
typename Po
intT>
void
402 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
403 if (query_depth > metadata_->getDepth ())
405 root_node_->getOccupiedVoxelCentersRecursive (voxel_centers, metadata_->getDepth ());
409 root_node_->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
415 template<
typename ContainerT,
typename Po
intT>
void
418 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
421 #pragma warning(push)
422 #pragma warning(disable : 4267)
424 root_node_->queryBBIntersects (min, max, query_depth, bin_name);
432 template<
typename ContainerT,
typename Po
intT>
void
435 std::ofstream f (filename.c_str ());
437 f <<
"from visual import *\n\n";
439 root_node_->writeVPythonVisual (f);
444 template<
typename ContainerT,
typename Po
intT>
void
447 root_node_->flushToDisk ();
452 template<
typename ContainerT,
typename Po
intT>
void
455 root_node_->flushToDiskLazy ();
460 template<
typename ContainerT,
typename Po
intT>
void
464 root_node_->convertToXYZ ();
469 template<
typename ContainerT,
typename Po
intT>
void
472 DeAllocEmptyNodeCache (root_node_);
477 template<
typename ContainerT,
typename Po
intT>
void
480 if (current ==
nullptr)
481 current = root_node_;
483 if (current->
size () == 0)
485 current->flush_DeAlloc_this_only ();
488 for (
int i = 0; i < current->numchildren (); i++)
490 DeAllocEmptyNodeCache (current->children[i]);
495 template<
typename ContainerT,
typename Po
intT> OutofcoreOctreeBaseNode<ContainerT, PointT>*
505 return (lod_filter_ptr_);
513 return (lod_filter_ptr_);
518 template<
typename ContainerT,
typename Po
intT>
void
521 lod_filter_ptr_ = std::static_pointer_cast<decltype(lod_filter_ptr_)>(filter_arg);
526 template<
typename ContainerT,
typename Po
intT>
bool
529 if (root_node_==
nullptr)
536 Eigen::Vector3d min, max;
537 this->getBoundingBox (min, max);
539 double depth =
static_cast<double> (metadata_->getDepth ());
540 Eigen::Vector3d diff = max-min;
542 y = diff[1] * pow (.5, depth);
543 x = diff[0] * pow (.5, depth);
550 template<
typename ContainerT,
typename Po
intT>
double
553 Eigen::Vector3d min, max;
554 this->getBoundingBox (min, max);
555 double result = (max[0] - min[0]) * pow (.5,
static_cast<double> (metadata_->getDepth ())) *
static_cast<double> (1 << (metadata_->getDepth () - depth));
561 template<
typename ContainerT,
typename Po
intT>
void
564 if (root_node_==
nullptr)
566 PCL_ERROR (
"Root node is null; aborting buildLOD.\n");
570 std::unique_lock < std::shared_timed_mutex > lock (read_write_mutex_);
572 constexpr
int number_of_nodes = 1;
574 std::vector<BranchNode*> current_branch (number_of_nodes,
static_cast<BranchNode*
>(
nullptr));
575 current_branch[0] = root_node_;
576 assert (current_branch.back () !=
nullptr);
577 this->buildLODRecursive (current_branch);
582 template<
typename ContainerT,
typename Po
intT>
void
585 Eigen::Vector3d min, max;
587 PCL_INFO (
"[pcl::outofcore::OutofcoreOctreeBase::%s] min(%lf,%lf,%lf), max(%lf,%lf,%lf)\n", __FUNCTION__, min[0], min[1], min[2], max[0], max[1], max[2]);
593 template<
typename ContainerT,
typename Po
intT>
void
596 PCL_DEBUG (
"%s Building LOD at depth %d",__PRETTY_FUNCTION__, current_branch.size ());
598 if (!current_branch.back ())
605 assert (current_branch.back ()->getDepth () == this->getDepth ());
612 assert (leaf_input_cloud->width*leaf_input_cloud->height > 0);
615 for (
auto level =
static_cast<std::int64_t
>(current_branch.size ()-1); level >= 1; level--)
617 BranchNode* target_parent = current_branch[level-1];
618 assert (target_parent !=
nullptr);
619 double exponent =
static_cast<double>(current_branch.size () - target_parent->
getDepth ());
620 double current_depth_sample_percent = pow (sample_percent_, exponent);
622 assert (current_depth_sample_percent > 0.0);
629 lod_filter_ptr_->setInputCloud (leaf_input_cloud);
632 auto sample_size =
static_cast<std::uint64_t
> (
static_cast<double> (leaf_input_cloud->width*leaf_input_cloud->height) * current_depth_sample_percent);
634 if (sample_size == 0)
637 lod_filter_ptr_->setSample (
static_cast<unsigned int>(sample_size));
644 lod_filter_ptr_->filter (*downsampled_cloud_indices);
649 extractor.
setIndices (downsampled_cloud_indices);
650 extractor.
filter (*downsampled_cloud);
653 if (downsampled_cloud->width*downsampled_cloud->height > 0)
655 target_parent->
payload_->insertRange (downsampled_cloud);
656 this->incrementPointsInLOD (target_parent->
getDepth (), downsampled_cloud->width*downsampled_cloud->height);
663 current_branch.back ()->clearData ();
665 std::vector<BranchNode*> next_branch (current_branch);
667 if (current_branch.back ()->hasUnloadedChildren ())
669 current_branch.back ()->loadChildren (
false);
672 for (std::size_t i = 0; i < 8; i++)
674 next_branch.push_back (current_branch.back ()->getChildPtr (i));
676 if (next_branch.back () !=
nullptr)
677 buildLODRecursive (next_branch);
679 next_branch.pop_back ();
685 template<
typename ContainerT,
typename Po
intT>
void
688 if (std::numeric_limits<std::uint64_t>::max () - metadata_->getLODPoints (depth) < new_point_count)
690 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBase::incrementPointsInLOD] Overflow error. Too many points in depth %d of outofcore octree with root at %s\n", depth, metadata_->getMetadataFilename().c_str());
694 metadata_->setLODPoints (depth, new_point_count,
true );
699 template<
typename ContainerT,
typename Po
intT>
bool
713 template<
typename ContainerT,
typename Po
intT>
void
716 Eigen::Vector3d diff = bb_max - bb_min;
717 assert (diff[0] > 0);
718 assert (diff[1] > 0);
719 assert (diff[2] > 0);
720 Eigen::Vector3d center = (bb_max + bb_min)/2.0;
722 double max_sidelength = std::max (std::max (std::abs (diff[0]), std::abs (diff[1])), std::abs (diff[2]));
723 assert (max_sidelength > 0);
724 bb_min = center - Eigen::Vector3d (1.0, 1.0, 1.0)*(max_sidelength/2.0);
725 bb_max = center + Eigen::Vector3d (1.0, 1.0, 1.0)*(max_sidelength/2.0);
730 template<
typename ContainerT,
typename Po
intT> std::uint64_t
731 OutofcoreOctreeBase<ContainerT, PointT>::calculateDepth (
const Eigen::Vector3d& min_bb,
const Eigen::Vector3d& max_bb,
const double leaf_resolution)
734 double side_length = max_bb[0] - min_bb[0];
736 if (side_length < leaf_resolution)
739 auto res =
static_cast<std::uint64_t
> (std::ceil (std::log2 (side_length / leaf_resolution)));
741 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBase::calculateDepth] Setting depth to %d\n",res);
Filter represents the base filter class.
void filter(Indices &indices)
Calls the filtering method and returns the filtered point cloud indices.
void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
void setInputCloud(const PCLPointCloud2ConstPtr &cloud)
Provide a pointer to the input dataset.
A base class for all pcl exceptions which inherits from std::runtime_error.
RandomSample applies a random sampling with uniform probability.
This code defines the octree used for point storage at Urban Robotics.
void DeAllocEmptyNodeCache()
Flush empty nodes only.
OutofcoreOctreeBaseMetadata::Ptr metadata_
std::uint64_t addDataToLeaf(const AlignedPointTVector &p)
Recursively add points to the tree.
void incrementPointsInLOD(std::uint64_t depth, std::uint64_t inc)
Increment current depths (LOD for branch nodes) point count; called by addDataAtMaxDepth in Outofcore...
void queryFrustum(const double *planes, std::list< std::string > &file_names) const
std::uint64_t addPointCloud_and_genLOD(pcl::PCLPointCloud2::Ptr &input_cloud)
Recursively add points to the tree.
void convertToXYZ()
Save each .bin file as an XYZ file.
OutofcoreOctreeBase(const boost::filesystem::path &root_node_name, const bool load_all)
Load an existing tree.
void getOccupiedVoxelCenters(AlignedPointTVector &voxel_centers, std::size_t query_depth) const
Returns the voxel centers of all existing voxels at query_depth.
void writeVPythonVisual(const boost::filesystem::path &filename)
Write a python script using the vpython module containing all the bounding boxes.
void buildLOD()
Generate multi-resolution LODs for the tree, which are a uniform random sampling all child leafs belo...
void queryBBIncludes(const Eigen::Vector3d &min, const Eigen::Vector3d &max, const std::uint64_t query_depth, AlignedPointTVector &dst) const
Get Points in BB, only points inside BB.
void queryBBIntersects(const Eigen::Vector3d &min, const Eigen::Vector3d &max, const std::uint32_t query_depth, std::list< std::string > &bin_name) const
Get a list of file paths at query_depth that intersect with your bounding box specified by min and ma...
bool getBoundingBox(Eigen::Vector3d &min, Eigen::Vector3d &max) const
Get the overall bounding box of the outofcore octree; this is the same as the bounding box of the roo...
double getVoxelSideLength() const
Gets the smallest (assumed) cubic voxel side lengths.
void flushToDiskLazy()
Flush all non leaf nodes' cache.
virtual void queryBoundingBox(const Eigen::Vector3d &min, const Eigen::Vector3d &max, const int query_depth, const pcl::PCLPointCloud2::Ptr &dst_blob, double percent=1.0)
Query all points falling within the input bounding box at query_depth and return a PCLPointCloud2 obj...
bool checkExtension(const boost::filesystem::path &path_name)
Auxiliary function to validate path_name extension is .octree.
std::uint64_t addDataToLeaf_and_genLOD(AlignedPointTVector &p)
Recursively add points to the tree subsampling LODs on the way.
bool getBinDimension(double &x, double &y) const
Computes the expected voxel dimensions at the leaves.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
void flushToDisk()
Flush all nodes' cache.
void printBoundingBox() const
Prints size of the bounding boxes to stdou.
void buildLODRecursive(const std::vector< BranchNode * > ¤t_branch)
recursive portion of lod builder
static const std::string TREE_EXTENSION_
defined as ".octree" to append to treepath files
OutofcoreNodeType * getBranchChildPtr(const BranchNode &branch_arg, unsigned char childIdx_arg) const
friend class OutofcoreOctreeBaseNode< ContainerT, PointT >
void queryBBIncludes_subsample(const Eigen::Vector3d &min, const Eigen::Vector3d &max, std::uint64_t query_depth, const double percent, AlignedPointTVector &dst) const
Returns a random subsample of points within the given bounding box at query_depth.
std::uint64_t addPointCloud(PointCloudConstPtr point_cloud)
Copies the points from the point_cloud falling within the bounding box of the octree to the out-of-co...
void init(const std::uint64_t &depth, const Eigen::Vector3d &min, const Eigen::Vector3d &max, const boost::filesystem::path &root_name, const std::string &coord_sys)
void saveToFile()
Write octree definition ".octree" (defined by octree_extension_) to disk.
typename PointCloud::ConstPtr PointCloudConstPtr
void setLODFilter(const pcl::Filter< pcl::PCLPointCloud2 >::Ptr &filter_arg)
Sets the filter to use when building the levels of depth.
OutofcoreNodeType * root_node_
Pointer to the root node of the octree data structure.
pcl::Filter< pcl::PCLPointCloud2 >::Ptr getLODFilter()
virtual ~OutofcoreOctreeBase()
OutofcoreOctreeBaseNode Class internally representing nodes of an outofcore octree,...
virtual void getBoundingBox(Eigen::Vector3d &min_bb, Eigen::Vector3d &max_bb) const
gets the minimum and maximum corner of the bounding box represented by this node
OutofcoreOctreeBase< ContainerT, PointT > * m_tree_
The tree we belong to.
std::shared_ptr< ContainerT > payload_
what holds the points.
virtual int read(pcl::PCLPointCloud2::Ptr &output_cloud)
std::uint64_t size() const
Number of points in the payload.
virtual std::size_t getDepth() const
virtual OutofcoreOctreeBaseNode * getChildPtr(std::size_t index_arg) const
Returns a pointer to the child in octant index_arg.
IndicesAllocator<> Indices
Type used for indices in PCL.
shared_ptr< Indices > IndicesPtr
shared_ptr< ::pcl::PCLPointCloud2 > Ptr