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 boost::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);
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> boost::uint64_t
210 boost::unique_lock < boost::shared_mutex > lock (read_write_mutex_);
212 const bool _FORCE_BB_CHECK =
true;
214 uint64_t pt_added = root_node_->addDataToLeaf (p, _FORCE_BB_CHECK);
216 assert (p.size () == pt_added);
223 template<
typename ContainerT,
typename Po
intT> boost::uint64_t
226 return (addDataToLeaf (point_cloud->points));
231 template<
typename ContainerT,
typename Po
intT> boost::uint64_t
234 uint64_t pt_added = this->root_node_->addPointCloud (input_cloud, skip_bb_check) ;
242 template<
typename ContainerT,
typename Po
intT> boost::uint64_t
246 boost::unique_lock < boost::shared_mutex > lock (read_write_mutex_);
247 boost::uint64_t pt_added = root_node_->addDataToLeaf_and_genLOD (point_cloud->points,
false);
253 template<
typename ContainerT,
typename Po
intT> boost::uint64_t
257 boost::unique_lock < boost::shared_mutex > lock (read_write_mutex_);
258 boost::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> boost::uint64_t
273 boost::unique_lock < boost::shared_mutex > lock (read_write_mutex_);
274 boost::uint64_t pt_added = root_node_->addDataToLeaf_and_genLOD (src,
false);
280 template<
typename Container,
typename Po
intT>
void
283 boost::shared_lock < boost::shared_mutex > lock (read_write_mutex_);
284 root_node_->queryFrustum (planes, file_names, this->getTreeDepth());
289 template<
typename Container,
typename Po
intT>
void
292 boost::shared_lock < boost::shared_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 boost::uint32_t query_depth)
const
306 boost::shared_lock < boost::shared_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 boost::shared_lock < boost::shared_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 boost::shared_lock < boost::shared_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 boost::shared_lock < boost::shared_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_!= NULL)
366 root_node_->getBoundingBox (min, max);
374 template<
typename ContainerT,
typename Po
intT>
void
377 boost::shared_lock < boost::shared_mutex > lock (read_write_mutex_);
378 root_node_->printBoundingBox (query_depth);
383 template<
typename ContainerT,
typename Po
intT>
void
386 boost::shared_lock < boost::shared_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 boost::shared_lock < boost::shared_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 boost::shared_lock < boost::shared_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->
size () == 0)
482 current->flush_DeAlloc_this_only ();
485 for (
int i = 0; i < current->numchildren (); i++)
487 DeAllocEmptyNodeCache (current->children[i]);
493 template<
typename ContainerT,
typename Po
intT> OutofcoreOctreeBaseNode<ContainerT, PointT>*
503 return (lod_filter_ptr_);
511 return (lod_filter_ptr_);
516 template<
typename ContainerT,
typename Po
intT>
void
519 lod_filter_ptr_ = filter_arg;
524 template<
typename ContainerT,
typename Po
intT>
bool
527 if (root_node_== NULL)
534 Eigen::Vector3d min, max;
535 this->getBoundingBox (min, max);
537 double depth =
static_cast<double> (metadata_->getDepth ());
538 Eigen::Vector3d diff = max-min;
540 y = diff[1] * pow (.5, depth);
541 x = diff[0] * pow (.5, depth);
548 template<
typename ContainerT,
typename Po
intT>
double
551 Eigen::Vector3d min, max;
552 this->getBoundingBox (min, max);
553 double result = (max[0] - min[0]) * pow (.5, static_cast<double> (metadata_->getDepth ())) *
static_cast<double> (1 << (metadata_->getDepth () - depth));
559 template<
typename ContainerT,
typename Po
intT>
void
562 if (root_node_== NULL)
564 PCL_ERROR (
"Root node is null; aborting buildLOD.\n");
568 boost::unique_lock < boost::shared_mutex > lock (read_write_mutex_);
570 const int number_of_nodes = 1;
572 std::vector<BranchNode*> current_branch (number_of_nodes, static_cast<BranchNode*>(0));
573 current_branch[0] = root_node_;
574 assert (current_branch.back () != 0);
575 this->buildLODRecursive (current_branch);
580 template<
typename ContainerT,
typename Po
intT>
void
583 Eigen::Vector3d min, max;
585 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]);
591 template<
typename ContainerT,
typename Po
intT>
void
594 PCL_DEBUG (
"%s Building LOD at depth %d",__PRETTY_FUNCTION__, current_branch.size ());
596 if (!current_branch.back ())
603 assert (current_branch.back ()->getDepth () == this->getDepth ());
609 leaf->read (leaf_input_cloud);
610 assert (leaf_input_cloud->width*leaf_input_cloud->height > 0);
613 for (int64_t level = static_cast<int64_t>(current_branch.size ()-1); level >= 1; level--)
615 BranchNode* target_parent = current_branch[level-1];
616 assert (target_parent != 0);
617 double exponent =
static_cast<double>(current_branch.size () - target_parent->
getDepth ());
618 double current_depth_sample_percent = pow (sample_percent_, exponent);
620 assert (current_depth_sample_percent > 0.0);
627 lod_filter_ptr_->setInputCloud (leaf_input_cloud);
630 uint64_t sample_size =
static_cast<uint64_t
> (
static_cast<double> (leaf_input_cloud->width*leaf_input_cloud->height) * current_depth_sample_percent);
632 if (sample_size == 0)
635 lod_filter_ptr_->setSample (static_cast<unsigned int>(sample_size));
642 lod_filter_ptr_->filter (*downsampled_cloud_indices);
647 extractor.
setIndices (downsampled_cloud_indices);
648 extractor.
filter (*downsampled_cloud);
651 if (downsampled_cloud->width*downsampled_cloud->height > 0)
653 target_parent->
payload_->insertRange (downsampled_cloud);
654 this->incrementPointsInLOD (target_parent->
getDepth (), downsampled_cloud->width*downsampled_cloud->height);
661 current_branch.back ()->clearData ();
663 std::vector<BranchNode*> next_branch (current_branch);
665 if (current_branch.back ()->hasUnloadedChildren ())
667 current_branch.back ()->loadChildren (
false);
670 for (
size_t i = 0; i < 8; i++)
672 next_branch.push_back (current_branch.back ()->getChildPtr (i));
674 if (next_branch.back () != 0)
675 buildLODRecursive (next_branch);
677 next_branch.pop_back ();
683 template<
typename ContainerT,
typename Po
intT>
void
686 if (std::numeric_limits<uint64_t>::max () - metadata_->getLODPoints (depth) < new_point_count)
688 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());
692 metadata_->setLODPoints (depth, new_point_count,
true );
697 template<
typename ContainerT,
typename Po
intT>
bool
711 template<
typename ContainerT,
typename Po
intT>
void
714 Eigen::Vector3d diff = bb_max - bb_min;
715 assert (diff[0] > 0);
716 assert (diff[1] > 0);
717 assert (diff[2] > 0);
718 Eigen::Vector3d center = (bb_max + bb_min)/2.0;
720 double max_sidelength = std::max (std::max (fabs (diff[0]), fabs (diff[1])), fabs (diff[2]));
721 assert (max_sidelength > 0);
722 bb_min = center - Eigen::Vector3d (1.0, 1.0, 1.0)*(max_sidelength/2.0);
723 bb_max = center + Eigen::Vector3d (1.0, 1.0, 1.0)*(max_sidelength/2.0);
728 template<
typename ContainerT,
typename Po
intT> boost::uint64_t
729 OutofcoreOctreeBase<ContainerT, PointT>::calculateDepth (
const Eigen::Vector3d& min_bb,
const Eigen::Vector3d& max_bb,
const double leaf_resolution)
732 double side_length = max_bb[0] - min_bb[0];
734 if (side_length < leaf_resolution)
737 boost::uint64_t res =
static_cast<boost::uint64_t
> (std::ceil (log2f (static_cast<float> (side_length / leaf_resolution))));
739 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBase::calculateDepth] Setting depth to %d\n",res);
745 #endif //PCL_OUTOFCORE_OCTREE_BASE_IMPL_H_
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
void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
void DeAllocEmptyNodeCache()
DEPRECATED - Flush empty nodes only.
void queryFrustum(const double *planes, std::list< std::string > &file_names) const
void setLODFilter(const pcl::Filter< pcl::PCLPointCloud2 >::Ptr &filter_arg)
Sets the filter to use when building the levels of depth.
void flushToDisk()
DEPRECATED - Flush all nodes' cache.
virtual OutofcoreOctreeBaseNode * getChildPtr(size_t index_arg) const
Returns a pointer to the child in octant index_arg.
A base class for all pcl exceptions which inherits from std::runtime_error.
boost::shared_ptr< std::vector< int > > IndicesPtr
double getVoxelSideLength() const
Gets the smallest (assumed) cubic voxel side lengths.
boost::shared_ptr< ContainerT > payload_
what holds the points.
This file defines compatibility wrappers for low level I/O functions.
virtual void filter(PCLPointCloud2 &output)
boost::shared_ptr< OutofcoreOctreeBaseMetadata > metadata_
boost::uint64_t addDataToLeaf(const AlignedPointTVector &p)
Recursively add points to the tree.
void queryBBIncludes_subsample(const Eigen::Vector3d &min, const Eigen::Vector3d &max, uint64_t query_depth, const double percent, AlignedPointTVector &dst) const
Returns a random subsample of points within the given bounding box at query_depth.
OutofcoreOctreeBase(const boost::filesystem::path &root_node_name, const bool load_all)
Load an existing tree.
void queryBBIncludes(const Eigen::Vector3d &min, const Eigen::Vector3d &max, const boost::uint64_t query_depth, AlignedPointTVector &dst) const
Get Points in BB, only points inside BB.
OutofcoreOctreeBaseNode Class internally representing nodes of an outofcore octree, with accessors to its data via the pcl::outofcore::OutofcoreOctreeDiskContainer class or pcl::outofcore::OutofcoreOctreeRamContainer class, whichever it is templated against.
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...
void writeVPythonVisual(const boost::filesystem::path filename)
Write a python script using the vpython module containing all the bounding boxes. ...
boost::shared_ptr< ::pcl::PCLPointCloud2 > Ptr
void convertToXYZ()
Save each .bin file as an XYZ file.
void printBoundingBox() const
Prints size of the bounding boxes to stdou.
void incrementPointsInLOD(boost::uint64_t depth, boost::uint64_t inc)
Increment current depths (LOD for branch nodes) point count; called by addDataAtMaxDepth in Outofcore...
void flushToDiskLazy()
DEPRECATED - Flush all non leaf nodes' cache.
void saveToFile()
Write octree definition ".octree" (defined by octree_extension_) to disk.
boost::uint64_t addDataToLeaf_and_genLOD(AlignedPointTVector &p)
Recursively add points to the tree subsampling LODs on the way.
void init(const boost::uint64_t &depth, const Eigen::Vector3d &min, const Eigen::Vector3d &max, const boost::filesystem::path &root_name, const std::string &coord_sys)
Filter represents the base filter class.
void buildLODRecursive(const std::vector< BranchNode * > ¤t_branch)
recursive portion of lod builder
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
void buildLOD()
Generate multi-resolution LODs for the tree, which are a uniform random sampling all child leafs belo...
bool getBinDimension(double &x, double &y) const
Computes the expected voxel dimensions at the leaves.
static const std::string TREE_EXTENSION_
defined as ".octree" to append to treepath files
boost::uint64_t addPointCloud_and_genLOD(pcl::PCLPointCloud2::Ptr &input_cloud)
Recursively add points to the tree.
void queryBBIntersects(const Eigen::Vector3d &min, const Eigen::Vector3d &max, const boost::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...
void getOccupiedVoxelCenters(AlignedPointTVector &voxel_centers, size_t query_depth) const
Returns the voxel centers of all existing voxels at query_depth.
OutofcoreNodeType * getBranchChildPtr(const BranchNode &branch_arg, unsigned char childIdx_arg) const
virtual ~OutofcoreOctreeBase()
boost::shared_ptr< const PointCloud > PointCloudConstPtr
pcl::Filter< pcl::PCLPointCloud2 >::Ptr getLODFilter()
OutofcoreNodeType * root_node_
Pointer to the root node of the octree data structure.
void setInputCloud(const PCLPointCloud2ConstPtr &cloud)
Provide a pointer to the input dataset.
This code defines the octree used for point storage at Urban Robotics.
boost::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...
RandomSample applies a random sampling with uniform probability.
virtual size_t getDepth() const
bool checkExtension(const boost::filesystem::path &path_name)
Auxiliary function to validate path_name extension is .octree.
boost::uint64_t size() const
Number of points in the payload.
OutofcoreOctreeBase< ContainerT, PointT > * m_tree_
The tree we belong to.