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>
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>
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 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);
178 metadata_->setMetadataFilename (treepath);
188 template<
typename ContainerT,
typename Po
intT>
199 template<
typename ContainerT,
typename Po
intT>
void 202 this->
metadata_->serializeMetadataToDisk ();
207 template<
typename ContainerT,
typename Po
intT> boost::uint64_t
212 const bool _FORCE_BB_CHECK =
true;
216 assert (p.size () == pt_added);
223 template<
typename ContainerT,
typename Po
intT> boost::uint64_t
231 template<
typename ContainerT,
typename Po
intT> boost::uint64_t
242 template<
typename ContainerT,
typename Po
intT> boost::uint64_t
253 template<
typename ContainerT,
typename Po
intT> boost::uint64_t
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
280 template<
typename Container,
typename Po
intT>
void 289 template<
typename Container,
typename Po
intT>
void 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 312 template<
typename ContainerT,
typename Po
intT>
void 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 328 dst_blob->data.clear ();
337 template<
typename ContainerT,
typename Po
intT>
void 346 template<
typename ContainerT,
typename Po
intT>
void 361 template<
typename ContainerT,
typename Po
intT>
bool 374 template<
typename ContainerT,
typename Po
intT>
void 383 template<
typename ContainerT,
typename Po
intT>
void 387 if (query_depth >
metadata_->getDepth ())
399 template<
typename ContainerT,
typename Po
intT>
void 403 if (query_depth >
metadata_->getDepth ())
415 template<
typename ContainerT,
typename Po
intT>
void 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";
444 template<
typename ContainerT,
typename Po
intT>
void 452 template<
typename ContainerT,
typename Po
intT>
void 460 template<
typename ContainerT,
typename Po
intT>
void 469 template<
typename ContainerT,
typename Po
intT>
void 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++)
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 534 Eigen::Vector3d 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;
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 564 PCL_ERROR (
"Root node is null; aborting buildLOD.\n");
570 const int number_of_nodes = 1;
572 std::vector<BranchNode*> current_branch (number_of_nodes, static_cast<BranchNode*>(0));
574 assert (current_branch.back () != 0);
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);
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);
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)
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
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_ 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
virtual void printBoundingBox(const size_t query_depth) const
Write the voxel size to stdout at query_depth.
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.
boost::uint64_t getTreeDepth() const
boost::uint64_t size() const
Number of points in the payload.
void flushToDiskRecursive()
A base class for all pcl exceptions which inherits from std::runtime_error.
OutofcoreOctreeBase< ContainerT, PointT > * m_tree_
The tree we belong to.
boost::shared_ptr< std::vector< int > > IndicesPtr
double getVoxelSideLength() const
Gets the smallest (assumed) cubic voxel side lengths.
virtual void filter(PCLPointCloud2 &output)
boost::shared_ptr< OutofcoreOctreeBaseMetadata > metadata_
boost::uint64_t addDataToLeaf(const AlignedPointTVector &p)
Recursively add points to the tree.
virtual void queryBBIntersects(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const boost::uint32_t query_depth, std::list< std::string > &file_names)
Recursive acquires PCD paths to any node with which the queried bounding box intersects (at query_dep...
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.
boost::uint64_t getDepth() const
Get number of LODs, which is the height of the tree.
virtual void queryBBIncludes_subsample(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, boost::uint64_t query_depth, const double percent, AlignedPointTVector &v)
Recursively add points that fall into the queried bounding box up to the query_depth.
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.
void getOccupiedVoxelCentersRecursive(AlignedPointTVector &voxel_centers, const size_t query_depth)
Gets a vector of occupied voxel centers.
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
virtual boost::uint64_t addDataToLeaf_and_genLOD(const AlignedPointTVector &p, const bool skip_bb_check)
Recursively add points to the leaf and children subsampling LODs on the way down. ...
virtual void queryBBIncludes(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, size_t query_depth, AlignedPointTVector &dst)
Recursively add points that fall into the queried bounding box up to the query_depth.
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 queryFrustum(const double planes[24], std::list< std::string > &file_names)
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)
virtual OutofcoreOctreeBaseNode * getChildPtr(size_t index_arg) const
Returns a pointer to the child in octant index_arg.
Filter represents the base filter class.
void writeVPythonVisual(std::ofstream &file)
Write a python visual script to file.
boost::shared_ptr< ContainerT > payload_
what holds the points.
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...
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.
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
virtual boost::uint64_t addPointCloud_and_genLOD(const pcl::PCLPointCloud2::Ptr input_cloud)
Add a single PCLPointCloud2 into the octree and build the subsampled LOD during construction; this me...
pcl::Filter< pcl::PCLPointCloud2 >::Ptr getLODFilter()
OutofcoreNodeType * root_node_
Pointer to the root node of the octree data structure.
static const int OUTOFCORE_VERSION_
void setInputCloud(const PCLPointCloud2ConstPtr &cloud)
Provide a pointer to the input dataset.
virtual boost::uint64_t addDataToLeaf(const AlignedPointTVector &p, const bool skip_bb_check=true)
add point to this node if we are a leaf, or find the leaf below us that is supposed to take the point...
virtual size_t getDepth() const
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
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.
void setSample(unsigned int sample)
Set number of indices to be sampled.
boost::shared_mutex read_write_mutex_
shared mutex for controlling read/write access to disk
virtual boost::uint64_t addPointCloud(const pcl::PCLPointCloud2::Ptr &input_cloud, const bool skip_bb_check=false)
Add a single PCLPointCloud2 object into the octree.
bool checkExtension(const boost::filesystem::path &path_name)
Auxiliary function to validate path_name extension is .octree.