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");
104 template<
typename ContainerT,
typename Po
intT>
107 , read_write_mutex_ ()
109 , sample_percent_ (0.125)
126 template<
typename ContainerT,
typename Po
intT>
129 , read_write_mutex_ ()
131 , sample_percent_ (0.125)
139 template<
typename ContainerT,
typename Po
intT>
void
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");
158 if (!boost::filesystem::exists (
dir))
160 boost::filesystem::create_directory (
dir);
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_);
223 template<
typename ContainerT,
typename Po
intT> std::uint64_t
231 template<
typename ContainerT,
typename Po
intT> std::uint64_t
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_);
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_);
289 template<
typename Container,
typename Po
intT>
void
292 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
298 template<
typename Container,
typename Po
intT>
void
301 const Eigen::Vector3d &
eye,
306 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
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]);
323 template<
typename ContainerT,
typename Po
intT>
void
326 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
337 template<
typename ContainerT,
typename Po
intT>
void
340 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
346 template<
typename ContainerT,
typename Po
intT>
void
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_);
383 template<
typename ContainerT,
typename Po
intT>
void
386 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
389 root_node_->getOccupiedVoxelCentersRecursive (
voxel_centers, metadata_->getDepth ());
399 template<
typename ContainerT,
typename Po
intT>
void
402 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
405 root_node_->getOccupiedVoxelCentersRecursive (
voxel_centers, metadata_->getDepth ());
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)
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
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;
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_);
582 template<
typename ContainerT,
typename Po
intT>
void
585 Eigen::Vector3d min, max;
586 node.getBoundingBox (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
615 for (std::int64_t level =
static_cast<std::int64_t
>(
current_branch.
size ()-1); level >= 1; level--)
637 lod_filter_ptr_->setSample (
static_cast<unsigned int>(
sample_size));
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());
699 template<
typename ContainerT,
typename Po
intT>
bool
713 template<
typename ContainerT,
typename Po
intT>
void
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)
741 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBase::calculateDepth] Setting depth to %d\n",res);
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
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 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 writeVPythonVisual(const boost::filesystem::path filename)
Write a python script using the vpython module containing all the bounding boxes.
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,...
OutofcoreOctreeBase< ContainerT, PointT > * m_tree_
The tree we belong to.
shared_ptr< Indices > IndicesPtr
shared_ptr< ::pcl::PCLPointCloud2 > Ptr