octree_base.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2012, Willow Garage, Inc.
* Copyright (c) 2012, Urban Robotics, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id$
*/
#ifndef PCL_OUTOFCORE_OCTREE_BASE_IMPL_H_
#define PCL_OUTOFCORE_OCTREE_BASE_IMPL_H_
#include <pcl/outofcore/octree_base.h>
// JSON
#include <pcl/outofcore/cJSON.h>
#include <pcl/filters/random_sample.h>
#include <pcl/filters/extract_indices.h>
// C++
#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <exception>
namespace pcl
{
namespace outofcore
{
////////////////////////////////////////////////////////////////////////////////
//Static constants
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT>
const std::string OutofcoreOctreeBase<ContainerT, PointT>::TREE_EXTENSION_ = ".octree";
template<typename ContainerT, typename PointT>
const int OutofcoreOctreeBase<ContainerT, PointT>::OUTOFCORE_VERSION_ = static_cast<int>(3);
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT>
OutofcoreOctreeBase<ContainerT, PointT>::OutofcoreOctreeBase (const boost::filesystem::path& root_name, const bool load_all)
: root_node_ ()
, read_write_mutex_ ()
, metadata_ (new OutofcoreOctreeBaseMetadata ())
, sample_percent_ (0.125)
, lod_filter_ptr_ (new pcl::RandomSample<pcl::PCLPointCloud2> ())
{
//validate the root filename
if (!this->checkExtension (root_name))
{
PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBase] Bad extension. Outofcore Octrees must have a root node ending in .oct_idx\n");
}
// Create root_node_node
root_node_ = new OutofcoreOctreeBaseNode<ContainerT, PointT> (root_name, NULL, load_all);
// Set root_node_nodes tree to the newly created tree
root_node_->m_tree_ = this;
// Set the path to the outofcore octree metadata (unique to the root folder) ending in .octree
boost::filesystem::path treepath = root_name.parent_path () / (boost::filesystem::basename (root_name) + TREE_EXTENSION_);
//Load the JSON metadata
metadata_->loadMetadataFromDisk (treepath);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT>
OutofcoreOctreeBase<ContainerT, PointT>::OutofcoreOctreeBase (const Eigen::Vector3d& min, const Eigen::Vector3d& max, const double resolution_arg, const boost::filesystem::path& root_node_name, const std::string& coord_sys)
: root_node_()
, read_write_mutex_ ()
, metadata_ (new OutofcoreOctreeBaseMetadata ())
, sample_percent_ (0.125)
, lod_filter_ptr_ (new pcl::RandomSample<pcl::PCLPointCloud2> ())
{
//Enlarge the bounding box to a cube so our voxels will be cubes
Eigen::Vector3d tmp_min = min;
Eigen::Vector3d tmp_max = max;
this->enlargeToCube (tmp_min, tmp_max);
//Compute the depth of the tree given the resolution
boost::uint64_t depth = this->calculateDepth (tmp_min, tmp_max, resolution_arg);
//Create a new outofcore tree
this->init (depth, tmp_min, tmp_max, root_node_name, coord_sys);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT>
OutofcoreOctreeBase<ContainerT, PointT>::OutofcoreOctreeBase (const boost::uint64_t max_depth, const Eigen::Vector3d& min, const Eigen::Vector3d& max, const boost::filesystem::path& root_node_name, const std::string& coord_sys)
: root_node_()
, read_write_mutex_ ()
, metadata_ (new OutofcoreOctreeBaseMetadata ())
, sample_percent_ (0.125)
, lod_filter_ptr_ (new pcl::RandomSample<pcl::PCLPointCloud2> ())
{
//Create a new outofcore tree
this->init (max_depth, min, max, root_node_name, coord_sys);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBase<ContainerT, PointT>::init (const uint64_t& depth, const Eigen::Vector3d& min, const Eigen::Vector3d& max, const boost::filesystem::path& root_name, const std::string& coord_sys)
{
//Validate the extension of the pathname
if (!this->checkExtension (root_name))
{
PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBase] Bad extension. Outofcore Octrees must have a root node ending in .oct_idx\n");
}
//Check to make sure that we are not overwriting existing data
if (boost::filesystem::exists (root_name.parent_path ()))
{
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 () );
PCL_THROW_EXCEPTION ( PCLException, "[pcl::outofcore::OutofcoreOctreeBase] Directory exists; Overwriting an existing tree is not supported\n");
}
// Get fullpath and recreate directories
boost::filesystem::path dir = root_name.parent_path ();
if (!boost::filesystem::exists (dir))
{
boost::filesystem::create_directory (dir);
}
Eigen::Vector3d tmp_min = min;
Eigen::Vector3d tmp_max = max;
this->enlargeToCube (tmp_min, tmp_max);
// Create root node
root_node_= new OutofcoreOctreeBaseNode<ContainerT, PointT> (tmp_min, tmp_max, this, root_name);
root_node_->m_tree_ = this;
// Set root nodes file path
boost::filesystem::path treepath = dir / (boost::filesystem::basename (root_name) + TREE_EXTENSION_);
//fill the fields of the metadata
metadata_->setCoordinateSystem (coord_sys);
metadata_->setDepth (depth);
metadata_->setLODPoints (depth+1);
metadata_->setMetadataFilename (treepath);
metadata_->setOutofcoreVersion (OUTOFCORE_VERSION_);
//metadata_->setPointType ( <point type string here> );
//save to disk
metadata_->serializeMetadataToDisk ();
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT>
OutofcoreOctreeBase<ContainerT, PointT>::~OutofcoreOctreeBase ()
{
root_node_->flushToDiskRecursive ();
saveToFile ();
delete (root_node_);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBase<ContainerT, PointT>::saveToFile ()
{
this->metadata_->serializeMetadataToDisk ();
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> boost::uint64_t
OutofcoreOctreeBase<ContainerT, PointT>::addDataToLeaf (const AlignedPointTVector& p)
{
boost::unique_lock < boost::shared_mutex > lock (read_write_mutex_);
const bool _FORCE_BB_CHECK = true;
uint64_t pt_added = root_node_->addDataToLeaf (p, _FORCE_BB_CHECK);
assert (p.size () == pt_added);
return (pt_added);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> boost::uint64_t
OutofcoreOctreeBase<ContainerT, PointT>::addPointCloud (PointCloudConstPtr point_cloud)
{
return (addDataToLeaf (point_cloud->points));
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> boost::uint64_t
OutofcoreOctreeBase<ContainerT, PointT>::addPointCloud (pcl::PCLPointCloud2::Ptr &input_cloud, const bool skip_bb_check)
{
uint64_t pt_added = this->root_node_->addPointCloud (input_cloud, skip_bb_check) ;
// assert (input_cloud->width*input_cloud->height == pt_added);
return (pt_added);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> boost::uint64_t
OutofcoreOctreeBase<ContainerT, PointT>::addPointCloud_and_genLOD (PointCloudConstPtr point_cloud)
{
// Lock the tree while writing
boost::unique_lock < boost::shared_mutex > lock (read_write_mutex_);
boost::uint64_t pt_added = root_node_->addDataToLeaf_and_genLOD (point_cloud->points, false);
return (pt_added);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> boost::uint64_t
OutofcoreOctreeBase<ContainerT, PointT>::addPointCloud_and_genLOD (pcl::PCLPointCloud2::Ptr &input_cloud)
{
// Lock the tree while writing
boost::unique_lock < boost::shared_mutex > lock (read_write_mutex_);
boost::uint64_t pt_added = root_node_->addPointCloud_and_genLOD (input_cloud);
PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBase::%s] Points added %lu, points in input cloud, %lu\n",__FUNCTION__, pt_added, input_cloud->width*input_cloud->height );
assert ( input_cloud->width*input_cloud->height == pt_added );
return (pt_added);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> boost::uint64_t
OutofcoreOctreeBase<ContainerT, PointT>::addDataToLeaf_and_genLOD (AlignedPointTVector& src)
{
// Lock the tree while writing
boost::unique_lock < boost::shared_mutex > lock (read_write_mutex_);
boost::uint64_t pt_added = root_node_->addDataToLeaf_and_genLOD (src, false);
return (pt_added);
}
////////////////////////////////////////////////////////////////////////////////
template<typename Container, typename PointT> void
OutofcoreOctreeBase<Container, PointT>::queryFrustum (const double planes[24], std::list<std::string>& file_names) const
{
boost::shared_lock < boost::shared_mutex > lock (read_write_mutex_);
root_node_->queryFrustum (planes, file_names, this->getTreeDepth());
}
////////////////////////////////////////////////////////////////////////////////
template<typename Container, typename PointT> void
OutofcoreOctreeBase<Container, PointT>::queryFrustum(const double *planes, std::list<std::string>& file_names, const boost::uint32_t query_depth) const
{
boost::shared_lock < boost::shared_mutex > lock (read_write_mutex_);
root_node_->queryFrustum (planes, file_names, query_depth);
}
////////////////////////////////////////////////////////////////////////////////
template<typename Container, typename PointT> void
OutofcoreOctreeBase<Container, PointT>::queryFrustum (
const double *planes,
const Eigen::Vector3d &eye,
const Eigen::Matrix4d &view_projection_matrix,
std::list<std::string>& file_names,
const boost::uint32_t query_depth) const
{
boost::shared_lock < boost::shared_mutex > lock (read_write_mutex_);
root_node_->queryFrustum (planes, eye, view_projection_matrix, file_names, query_depth);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBase<ContainerT, PointT>::queryBBIncludes (const Eigen::Vector3d& min, const Eigen::Vector3d& max, const boost::uint64_t query_depth, AlignedPointTVector& dst) const
{
boost::shared_lock < boost::shared_mutex > lock (read_write_mutex_);
dst.clear ();
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]);
root_node_->queryBBIncludes (min, max, query_depth, dst);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBase<ContainerT, PointT>::queryBBIncludes (const Eigen::Vector3d& min, const Eigen::Vector3d& max, const boost::uint64_t query_depth, const pcl::PCLPointCloud2::Ptr& dst_blob) const
{
boost::shared_lock < boost::shared_mutex > lock (read_write_mutex_);
dst_blob->data.clear ();
dst_blob->width = 0;
dst_blob->height =1;
root_node_->queryBBIncludes ( min, max, query_depth, dst_blob );
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBase<ContainerT, PointT>::queryBBIncludes_subsample (const Eigen::Vector3d& min, const Eigen::Vector3d& max, const boost::uint64_t query_depth, const double percent, AlignedPointTVector& dst) const
{
boost::shared_lock < boost::shared_mutex > lock (read_write_mutex_);
dst.clear ();
root_node_->queryBBIncludes_subsample (min, max, query_depth, percent, dst);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBase<ContainerT, PointT>::queryBoundingBox (const Eigen::Vector3d &min, const Eigen::Vector3d &max, const int query_depth, const pcl::PCLPointCloud2::Ptr &dst_blob, double percent)
{
if (percent==1.0)
{
root_node_->queryBBIncludes (min, max, query_depth, dst_blob);
}
else
{
root_node_->queryBBIncludes_subsample (min, max, query_depth, dst_blob, percent);
}
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> bool
OutofcoreOctreeBase<ContainerT, PointT>::getBoundingBox (Eigen::Vector3d &min, Eigen::Vector3d &max) const
{
if (root_node_!= NULL)
{
root_node_->getBoundingBox (min, max);
return true;
}
return false;
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBase<ContainerT, PointT>::printBoundingBox(const size_t query_depth) const
{
boost::shared_lock < boost::shared_mutex > lock (read_write_mutex_);
root_node_->printBoundingBox (query_depth);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBase<ContainerT, PointT>::getOccupiedVoxelCenters(AlignedPointTVector &voxel_centers, const size_t query_depth) const
{
boost::shared_lock < boost::shared_mutex > lock (read_write_mutex_);
if (query_depth > metadata_->getDepth ())
{
root_node_->getOccupiedVoxelCentersRecursive (voxel_centers, metadata_->getDepth ());
}
else
{
root_node_->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
}
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBase<ContainerT, PointT>::getOccupiedVoxelCenters(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &voxel_centers, const size_t query_depth) const
{
boost::shared_lock < boost::shared_mutex > lock (read_write_mutex_);
if (query_depth > metadata_->getDepth ())
{
root_node_->getOccupiedVoxelCentersRecursive (voxel_centers, metadata_->getDepth ());
}
else
{
root_node_->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
}
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBase<ContainerT, PointT>::queryBBIntersects (const Eigen::Vector3d& min, const Eigen::Vector3d& max, const boost::uint32_t query_depth, std::list<std::string>& bin_name) const
{
boost::shared_lock < boost::shared_mutex > lock (read_write_mutex_);
bin_name.clear ();
#if defined _MSC_VER
#pragma warning(push)
#pragma warning(disable : 4267)
#endif
root_node_->queryBBIntersects (min, max, query_depth, bin_name);
#if defined _MSC_VER
#pragma warning(pop)
#endif
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBase<ContainerT, PointT>::writeVPythonVisual (const boost::filesystem::path filename)
{
std::ofstream f (filename.c_str ());
f << "from visual import *\n\n";
root_node_->writeVPythonVisual (f);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBase<ContainerT, PointT>::flushToDisk ()
{
root_node_->flushToDisk ();
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBase<ContainerT, PointT>::flushToDiskLazy ()
{
root_node_->flushToDiskLazy ();
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBase<ContainerT, PointT>::convertToXYZ ()
{
saveToFile ();
root_node_->convertToXYZ ();
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBase<ContainerT, PointT>::DeAllocEmptyNodeCache ()
{
DeAllocEmptyNodeCache (root_node_);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBase<ContainerT, PointT>::DeAllocEmptyNodeCache (OutofcoreOctreeBaseNode<ContainerT, PointT>* current)
{
if (current->size () == 0)
{
current->flush_DeAlloc_this_only ();
}
for (int i = 0; i < current->numchildren (); i++)
{
DeAllocEmptyNodeCache (current->children[i]);
}
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> OutofcoreOctreeBaseNode<ContainerT, PointT>*
OutofcoreOctreeBase<ContainerT, PointT>::getBranchChildPtr (const BranchNode& branch_arg, unsigned char childIdx_arg) const
{
return (branch_arg.getChildPtr (childIdx_arg));
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> pcl::Filter<pcl::PCLPointCloud2>::Ptr
OutofcoreOctreeBase<ContainerT, PointT>::getLODFilter ()
{
return (lod_filter_ptr_);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> const pcl::Filter<pcl::PCLPointCloud2>::ConstPtr
OutofcoreOctreeBase<ContainerT, PointT>::getLODFilter () const
{
return (lod_filter_ptr_);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBase<ContainerT, PointT>::setLODFilter (const pcl::Filter<pcl::PCLPointCloud2>::Ptr& filter_arg)
{
lod_filter_ptr_ = filter_arg;
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> bool
OutofcoreOctreeBase<ContainerT, PointT>::getBinDimension (double& x, double& y) const
{
if (root_node_== NULL)
{
x = 0;
y = 0;
return (false);
}
Eigen::Vector3d min, max;
this->getBoundingBox (min, max);
double depth = static_cast<double> (metadata_->getDepth ());
Eigen::Vector3d diff = max-min;
y = diff[1] * pow (.5, depth);
x = diff[0] * pow (.5, depth);
return (true);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> double
OutofcoreOctreeBase<ContainerT, PointT>::getVoxelSideLength (const boost::uint64_t& depth) const
{
Eigen::Vector3d min, max;
this->getBoundingBox (min, max);
double result = (max[0] - min[0]) * pow (.5, static_cast<double> (metadata_->getDepth ())) * static_cast<double> (1 << (metadata_->getDepth () - depth));
return (result);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBase<ContainerT, PointT>::buildLOD ()
{
if (root_node_== NULL)
{
PCL_ERROR ("Root node is null; aborting buildLOD.\n");
return;
}
boost::unique_lock < boost::shared_mutex > lock (read_write_mutex_);
const int number_of_nodes = 1;
std::vector<BranchNode*> current_branch (number_of_nodes, static_cast<BranchNode*>(0));
current_branch[0] = root_node_;
assert (current_branch.back () != 0);
this->buildLODRecursive (current_branch);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBase<ContainerT, PointT>::printBoundingBox (OutofcoreOctreeBaseNode<ContainerT, PointT>& node) const
{
Eigen::Vector3d min, max;
node.getBoundingBox (min,max);
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]);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBase<ContainerT, PointT>::buildLODRecursive (const std::vector<BranchNode*>& current_branch)
{
PCL_DEBUG ("%s Building LOD at depth %d",__PRETTY_FUNCTION__, current_branch.size ());
if (!current_branch.back ())
{
return;
}
if (current_branch.back ()->getNodeType () == pcl::octree::LEAF_NODE)
{
assert (current_branch.back ()->getDepth () == this->getDepth ());
BranchNode* leaf = current_branch.back ();
pcl::PCLPointCloud2::Ptr leaf_input_cloud (new pcl::PCLPointCloud2 ());
//read the data from the PCD file associated with the leaf; it is full resolution
leaf->read (leaf_input_cloud);
assert (leaf_input_cloud->width*leaf_input_cloud->height > 0);
//go up the tree, re-downsampling the full resolution leaf cloud at lower and lower resolution
for (int64_t level = static_cast<int64_t>(current_branch.size ()-1); level >= 1; level--)
{
BranchNode* target_parent = current_branch[level-1];
assert (target_parent != 0);
double exponent = static_cast<double>(current_branch.size () - target_parent->getDepth ());
double current_depth_sample_percent = pow (sample_percent_, exponent);
assert (current_depth_sample_percent > 0.0);
//------------------------------------------------------------
//subsample data:
// 1. Get indices from a random sample
// 2. Extract those indices with the extract indices class (in order to also get the complement)
//------------------------------------------------------------
lod_filter_ptr_->setInputCloud (leaf_input_cloud);
//set sample size to 1/8 of total points (12.5%)
uint64_t sample_size = static_cast<uint64_t> (static_cast<double> (leaf_input_cloud->width*leaf_input_cloud->height) * current_depth_sample_percent);
if (sample_size == 0)
sample_size = 1;
lod_filter_ptr_->setSample (static_cast<unsigned int>(sample_size));
//create our destination
pcl::PCLPointCloud2::Ptr downsampled_cloud (new pcl::PCLPointCloud2 ());
//create destination for indices
pcl::IndicesPtr downsampled_cloud_indices (new std::vector< int > ());
lod_filter_ptr_->filter (*downsampled_cloud_indices);
//extract the "random subset", size by setSampleSize
pcl::ExtractIndices<pcl::PCLPointCloud2> extractor;
extractor.setInputCloud (leaf_input_cloud);
extractor.setIndices (downsampled_cloud_indices);
extractor.filter (*downsampled_cloud);
//write to the target
if (downsampled_cloud->width*downsampled_cloud->height > 0)
{
target_parent->payload_->insertRange (downsampled_cloud);
this->incrementPointsInLOD (target_parent->getDepth (), downsampled_cloud->width*downsampled_cloud->height);
}
}
}
else//not at leaf, keep going down
{
//clear this node while walking down the tree in case we are updating the LOD
current_branch.back ()->clearData ();
std::vector<BranchNode*> next_branch (current_branch);
if (current_branch.back ()->hasUnloadedChildren ())
{
current_branch.back ()->loadChildren (false);
}
for (size_t i = 0; i < 8; i++)
{
next_branch.push_back (current_branch.back ()->getChildPtr (i));
//skip that child if it doesn't exist
if (next_branch.back () != 0)
buildLODRecursive (next_branch);
next_branch.pop_back ();
}
}
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBase<ContainerT, PointT>::incrementPointsInLOD (boost::uint64_t depth, boost::uint64_t new_point_count)
{
if (std::numeric_limits<uint64_t>::max () - metadata_->getLODPoints (depth) < new_point_count)
{
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());
PCL_THROW_EXCEPTION (PCLException, "Overflow error");
}
metadata_->setLODPoints (depth, new_point_count, true /*true->increment*/);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> bool
OutofcoreOctreeBase<ContainerT, PointT>::checkExtension (const boost::filesystem::path& path_name)
{
if (boost::filesystem::extension (path_name) != OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_extension)
{
PCL_ERROR ( "[pcl::outofcore::OutofcoreOctreeBase] Wrong root node file extension: %s. The tree must have a root node ending in %s\n", boost::filesystem::extension (path_name).c_str (), OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_extension.c_str () );
return (0);
}
return (1);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBase<ContainerT, PointT>::enlargeToCube (Eigen::Vector3d& bb_min, Eigen::Vector3d& bb_max)
{
Eigen::Vector3d diff = bb_max - bb_min;
assert (diff[0] > 0);
assert (diff[1] > 0);
assert (diff[2] > 0);
Eigen::Vector3d center = (bb_max + bb_min)/2.0;
double max_sidelength = std::max (std::max (fabs (diff[0]), fabs (diff[1])), fabs (diff[2]));
assert (max_sidelength > 0);
bb_min = center - Eigen::Vector3d (1.0, 1.0, 1.0)*(max_sidelength/2.0);
bb_max = center + Eigen::Vector3d (1.0, 1.0, 1.0)*(max_sidelength/2.0);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> boost::uint64_t
OutofcoreOctreeBase<ContainerT, PointT>::calculateDepth (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, const double leaf_resolution)
{
//Assume cube
double side_length = max_bb[0] - min_bb[0];
if (side_length < leaf_resolution)
return (0);
boost::uint64_t res = static_cast<boost::uint64_t> (std::ceil (log2f (static_cast<float> (side_length / leaf_resolution))));
PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBase::calculateDepth] Setting depth to %d\n",res);
return (res);
}
}//namespace outofcore
}//namespace pcl
#endif //PCL_OUTOFCORE_OCTREE_BASE_IMPL_H_
pcl::PCLBase< pcl::PCLPointCloud2 >::setIndices
void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
pcl::outofcore::OutofcoreOctreeBase::DeAllocEmptyNodeCache
void DeAllocEmptyNodeCache()
DEPRECATED - Flush empty nodes only.
Definition: octree_base.hpp:470
pcl::outofcore::OutofcoreOctreeBase::queryFrustum
void queryFrustum(const double *planes, std::list< std::string > &file_names) const
pcl::outofcore::OutofcoreOctreeBaseNode::printBoundingBox
virtual void printBoundingBox(const size_t query_depth) const
Write the voxel size to stdout at query_depth.
Definition: octree_base_node.hpp:958
pcl::outofcore::OutofcoreOctreeBase::setLODFilter
void setLODFilter(const pcl::Filter< pcl::PCLPointCloud2 >::Ptr &filter_arg)
Sets the filter to use when building the levels of depth.
Definition: octree_base.hpp:517
pcl::outofcore::OutofcoreOctreeBase::flushToDisk
void flushToDisk()
DEPRECATED - Flush all nodes&#39; cache.
Definition: octree_base.hpp:445
pcl::outofcore::OutofcoreOctreeBase::getTreeDepth
boost::uint64_t getTreeDepth() const
Definition: octree_base.h:439
pcl::outofcore::OutofcoreOctreeBaseNode::size
boost::uint64_t size() const
Number of points in the payload.
Definition: octree_base_node.h:467
pcl::outofcore::OutofcoreOctreeBaseNode::flushToDiskRecursive
void flushToDiskRecursive()
Definition: octree_base_node.hpp:1995
pcl::PCLException
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition: exceptions.h:65
pcl::outofcore::OutofcoreOctreeBaseNode::m_tree_
OutofcoreOctreeBase< ContainerT, PointT > * m_tree_
The tree we belong to.
Definition: octree_base_node.h:534
pcl::IndicesPtr
boost::shared_ptr< std::vector< int > > IndicesPtr
Definition: pcl_base.h:60
pcl::outofcore::OutofcoreOctreeBase::getVoxelSideLength
double getVoxelSideLength() const
Gets the smallest (assumed) cubic voxel side lengths.
Definition: octree_base.h:460
pcl
Definition: cloud_iterator.h:46
pcl::FilterIndices< pcl::PCLPointCloud2 >::filter
virtual void filter(PCLPointCloud2 &output)
Definition: filter_indices.h:228
pcl::outofcore::OutofcoreOctreeBase::metadata_
boost::shared_ptr< OutofcoreOctreeBaseMetadata > metadata_
Definition: octree_base.h:637
pcl::outofcore::OutofcoreOctreeBase::addDataToLeaf
boost::uint64_t addDataToLeaf(const AlignedPointTVector &p)
Recursively add points to the tree.
Definition: octree_base.hpp:208
pcl::outofcore::OutofcoreOctreeBaseNode::queryBBIntersects
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...
Definition: octree_base_node.hpp:1353
pcl::outofcore::OutofcoreOctreeBase::queryBBIncludes_subsample
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.
Definition: octree_base.hpp:338
pcl::outofcore::OutofcoreOctreeBase::OutofcoreOctreeBase
OutofcoreOctreeBase(const boost::filesystem::path &root_node_name, const bool load_all)
Load an existing tree.
Definition: octree_base.hpp:77
pcl::outofcore::OutofcoreOctreeBase::getDepth
boost::uint64_t getDepth() const
Get number of LODs, which is the height of the tree.
Definition: octree_base.h:433
pcl::outofcore::OutofcoreOctreeBaseNode::queryBBIncludes_subsample
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.
Definition: octree_base_node.hpp:1658
pcl::outofcore::OutofcoreOctreeBase::queryBBIncludes
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.
Definition: octree_base.hpp:313
pcl::outofcore::OutofcoreOctreeBaseNode::getOccupiedVoxelCentersRecursive
void getOccupiedVoxelCentersRecursive(AlignedPointTVector &voxel_centers, const size_t query_depth)
Gets a vector of occupied voxel centers.
Definition: octree_base_node.hpp:986
pcl::outofcore::OutofcoreOctreeBase::queryBoundingBox
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...
Definition: octree_base.hpp:347
pcl::outofcore::OutofcoreOctreeBase::writeVPythonVisual
void writeVPythonVisual(const boost::filesystem::path filename)
Write a python script using the vpython module containing all the bounding boxes. ...
Definition: octree_base.hpp:433
pcl::PCLPointCloud2::Ptr
boost::shared_ptr< ::pcl::PCLPointCloud2 > Ptr
Definition: PCLPointCloud2.h:51
pcl::outofcore::OutofcoreOctreeBaseNode::addDataToLeaf_and_genLOD
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. ...
Definition: octree_base_node.hpp:820
pcl::outofcore::OutofcoreOctreeBaseNode::queryBBIncludes
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.
Definition: octree_base_node.hpp:1517
pcl::outofcore::OutofcoreOctreeBase::convertToXYZ
void convertToXYZ()
Save each .bin file as an XYZ file.
Definition: octree_base.hpp:461
pcl::outofcore::OutofcoreOctreeBase::printBoundingBox
void printBoundingBox() const
Prints size of the bounding boxes to stdou.
Definition: octree_base.h:493
pcl::outofcore::OutofcoreOctreeBase::incrementPointsInLOD
void incrementPointsInLOD(boost::uint64_t depth, boost::uint64_t inc)
Increment current depths (LOD for branch nodes) point count; called by addDataAtMaxDepth in Outofcore...
Definition: octree_base.hpp:684
pcl::outofcore::OutofcoreOctreeBase::flushToDiskLazy
void flushToDiskLazy()
DEPRECATED - Flush all non leaf nodes&#39; cache.
Definition: octree_base.hpp:453
pcl::outofcore::OutofcoreOctreeBase::saveToFile
void saveToFile()
Write octree definition ".octree" (defined by octree_extension_) to disk.
Definition: octree_base.hpp:200
pcl::outofcore::OutofcoreOctreeBase::addDataToLeaf_and_genLOD
boost::uint64_t addDataToLeaf_and_genLOD(AlignedPointTVector &p)
Recursively add points to the tree subsampling LODs on the way.
Definition: octree_base.hpp:270
pcl::outofcore::OutofcoreOctreeBaseNode::queryFrustum
void queryFrustum(const double planes[24], std::list< std::string > &file_names)
Definition: octree_base_node.hpp:1063
pcl::outofcore::OutofcoreOctreeBase::init
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)
Definition: octree_base.hpp:140
pcl::outofcore::OutofcoreOctreeBaseNode::getChildPtr
virtual OutofcoreOctreeBaseNode * getChildPtr(size_t index_arg) const
Returns a pointer to the child in octant index_arg.
Definition: octree_base_node.hpp:1924
pcl::Filter
Filter represents the base filter class.
Definition: filter.h:83
pcl::outofcore::OutofcoreOctreeBaseNode::writeVPythonVisual
void writeVPythonVisual(std::ofstream &file)
Write a python visual script to file.
Definition: octree_base_node.hpp:1895
pcl::outofcore::OutofcoreOctreeBaseNode::payload_
boost::shared_ptr< ContainerT > payload_
what holds the points.
Definition: octree_base_node.h:558
pcl::outofcore::OutofcoreOctreeBase::buildLODRecursive
void buildLODRecursive(const std::vector< BranchNode * > &current_branch)
recursive portion of lod builder
Definition: octree_base.hpp:592
pcl::outofcore::OutofcoreOctreeBase::AlignedPointTVector
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
Definition: octree_base.h:188
pcl::outofcore::OutofcoreOctreeBase::buildLOD
void buildLOD()
Generate multi-resolution LODs for the tree, which are a uniform random sampling all child leafs belo...
Definition: octree_base.hpp:560
pcl::octree::LEAF_NODE
Definition: octree_nodes.h:63
pcl::outofcore::OutofcoreOctreeBase::getBinDimension
bool getBinDimension(double &x, double &y) const
Computes the expected voxel dimensions at the leaves.
Definition: octree_base.hpp:525
pcl::outofcore::OutofcoreOctreeBase::TREE_EXTENSION_
static const std::string TREE_EXTENSION_
defined as ".octree" to append to treepath files
Definition: octree_base.h:642
pcl::outofcore::OutofcoreOctreeBase::addPointCloud_and_genLOD
boost::uint64_t addPointCloud_and_genLOD(pcl::PCLPointCloud2::Ptr &input_cloud)
Recursively add points to the tree.
Definition: octree_base.hpp:254
pcl::outofcore::OutofcoreOctreeBase::queryBBIntersects
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...
Definition: octree_base.hpp:416
pcl::outofcore::OutofcoreOctreeBase::getBoundingBox
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...
Definition: octree_base.hpp:362
pcl::outofcore::OutofcoreOctreeBaseNode
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.
Definition: octree_base_node.h:59
pcl::outofcore::OutofcoreOctreeBase::getOccupiedVoxelCenters
void getOccupiedVoxelCenters(AlignedPointTVector &voxel_centers, size_t query_depth) const
Returns the voxel centers of all existing voxels at query_depth.
Definition: octree_base.hpp:384
pcl::outofcore::OutofcoreOctreeBase::getBranchChildPtr
OutofcoreNodeType * getBranchChildPtr(const BranchNode &branch_arg, unsigned char childIdx_arg) const
Definition: octree_base.hpp:494
pcl::outofcore::OutofcoreOctreeBase::~OutofcoreOctreeBase
virtual ~OutofcoreOctreeBase()
Definition: octree_base.hpp:189
pcl::outofcore::OutofcoreOctreeBase::PointCloudConstPtr
boost::shared_ptr< const PointCloud > PointCloudConstPtr
Definition: octree_base.h:186
pcl::outofcore::OutofcoreOctreeBaseNode::addPointCloud_and_genLOD
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...
Definition: octree_base_node.hpp:720
pcl::outofcore::OutofcoreOctreeBase::getLODFilter
pcl::Filter< pcl::PCLPointCloud2 >::Ptr getLODFilter()
Definition: octree_base.hpp:501
pcl::outofcore::OutofcoreOctreeBase::root_node_
OutofcoreNodeType * root_node_
Pointer to the root node of the octree data structure.
Definition: octree_base.h:632
pcl::outofcore::OutofcoreOctreeBase::OUTOFCORE_VERSION_
static const int OUTOFCORE_VERSION_
Definition: octree_base.h:643
pcl::ExtractIndices< pcl::PCLPointCloud2 >
ExtractIndices extracts a set of indices from a point cloud.
Definition: extract_indices.h:161
pcl::PCLBase< pcl::PCLPointCloud2 >::setInputCloud
void setInputCloud(const PCLPointCloud2ConstPtr &cloud)
Provide a pointer to the input dataset.
pcl::PCLPointCloud2
Definition: PCLPointCloud2.h:20
pcl::outofcore::OutofcoreOctreeBaseNode::addDataToLeaf
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...
Definition: octree_base_node.hpp:356
pcl::outofcore::OutofcoreOctreeBaseNode::getDepth
virtual size_t getDepth() const
Definition: octree_base_node.h:272
pcl::outofcore::OutofcoreOctreeBaseNode::getBoundingBox
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
Definition: octree_base_node.h:137
pcl::outofcore::OutofcoreOctreeBase
This code defines the octree used for point storage at Urban Robotics.
Definition: octree_base.h:149
pcl::outofcore::OutofcoreOctreeBase::addPointCloud
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...
Definition: octree_base.hpp:224
pcl::RandomSample
RandomSample applies a random sampling with uniform probability.
Definition: random_sample.h:56
pcl::outofcore::OutofcoreOctreeBaseMetadata
Encapsulated class to read JSON metadata into memory, and write the JSON metadata associated with the...
Definition: outofcore_base_data.h:97
pcl::RandomSample< pcl::PCLPointCloud2 >::setSample
void setSample(unsigned int sample)
Set number of indices to be sampled.
Definition: random_sample.h:179
pcl::outofcore::OutofcoreOctreeBase::read_write_mutex_
boost::shared_mutex read_write_mutex_
shared mutex for controlling read/write access to disk
Definition: octree_base.h:635
pcl::outofcore::OutofcoreOctreeBaseNode::addPointCloud
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.
Definition: octree_base_node.hpp:508
pcl::outofcore::OutofcoreOctreeBase::checkExtension
bool checkExtension(const boost::filesystem::path &path_name)
Auxiliary function to validate path_name extension is .octree.
Definition: octree_base.hpp:698