octree_base_node.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_NODE_IMPL_H_
#define PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_H_
// C++
#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <exception>
#include <pcl/common/common.h>
#include <pcl/visualization/common/common.h>
#include <pcl/outofcore/octree_base_node.h>
#include <pcl/filters/random_sample.h>
#include <pcl/filters/extract_indices.h>
// JSON
#include <pcl/outofcore/cJSON.h>
namespace pcl
{
namespace outofcore
{
template<typename ContainerT, typename PointT>
const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_basename = "node";
template<typename ContainerT, typename PointT>
const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::node_container_basename = "node";
template<typename ContainerT, typename PointT>
const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_extension = ".oct_idx";
template<typename ContainerT, typename PointT>
const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::node_container_extension = ".oct_dat";
template<typename ContainerT, typename PointT>
boost::mutex OutofcoreOctreeBaseNode<ContainerT, PointT>::rng_mutex_;
template<typename ContainerT, typename PointT>
boost::mt19937 OutofcoreOctreeBaseNode<ContainerT, PointT>::rand_gen_;
template<typename ContainerT, typename PointT>
const double OutofcoreOctreeBaseNode<ContainerT, PointT>::sample_percent_ = .125;
template<typename ContainerT, typename PointT>
const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::pcd_extension = ".pcd";
template<typename ContainerT, typename PointT>
OutofcoreOctreeBaseNode<ContainerT, PointT>::OutofcoreOctreeBaseNode ()
: m_tree_ ()
, root_node_ (NULL)
, parent_ (NULL)
, depth_ (0)
, children_ (std::vector <OutofcoreOctreeBaseNode<ContainerT, PointT>*> (8,static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>*>(0)))
, num_children_ (0)
, num_loaded_children_ (0)
, payload_ ()
, node_metadata_ ()
{
node_metadata_ = boost::shared_ptr<OutofcoreOctreeNodeMetadata> (new OutofcoreOctreeNodeMetadata ());
node_metadata_->setOutofcoreVersion (3);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT>
OutofcoreOctreeBaseNode<ContainerT, PointT>::OutofcoreOctreeBaseNode (const boost::filesystem::path& directory_path, OutofcoreOctreeBaseNode<ContainerT, PointT>* super, bool load_all)
: m_tree_ ()
, root_node_ ()
, parent_ (super)
, depth_ ()
, children_ (std::vector <OutofcoreOctreeBaseNode<ContainerT, PointT>*> (8,static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>*>(0)))
, num_children_ (0)
, num_loaded_children_ (0)
, payload_ ()
, node_metadata_ ()
{
node_metadata_ = boost::shared_ptr<OutofcoreOctreeNodeMetadata> (new OutofcoreOctreeNodeMetadata ());
node_metadata_->setOutofcoreVersion (3);
//Check if this is the first node created/loaded (this is true if super, i.e. node's parent is NULL)
if (super == NULL)
{
node_metadata_->setDirectoryPathname (directory_path.parent_path ());
node_metadata_->setMetadataFilename (directory_path);
depth_ = 0;
root_node_ = this;
//Check if the specified directory to load currently exists; if not, don't continue
if (!boost::filesystem::exists (node_metadata_->getDirectoryPathname ()))
{
PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find dir %s\n", node_metadata_->getDirectoryPathname ().c_str ());
PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Exception: missing directory");
}
}
else
{
node_metadata_->setDirectoryPathname (directory_path);
depth_ = super->getDepth () + 1;
root_node_ = super->root_node_;
boost::filesystem::directory_iterator directory_it_end; //empty constructor creates end of iterator
//flag to test if the desired metadata file was found
bool b_loaded = 0;
for (boost::filesystem::directory_iterator directory_it (node_metadata_->getDirectoryPathname ()); directory_it != directory_it_end; ++directory_it)
{
const boost::filesystem::path& file = *directory_it;
if (!boost::filesystem::is_directory (file))
{
if (boost::filesystem::extension (file) == node_index_extension)
{
b_loaded = node_metadata_->loadMetadataFromDisk (file);
break;
}
}
}
if (!b_loaded)
{
PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find index\n");
PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore: Could not find node index");
}
}
//load the metadata
loadFromFile (node_metadata_->getMetadataFilename (), super);
//set the number of children in this node
num_children_ = this->countNumChildren ();
if (load_all)
{
loadChildren (true);
}
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT>
OutofcoreOctreeBaseNode<ContainerT, PointT>::OutofcoreOctreeBaseNode (const Eigen::Vector3d& bb_min, const Eigen::Vector3d& bb_max, OutofcoreOctreeBase<ContainerT, PointT> * const tree, const boost::filesystem::path& root_name)
: m_tree_ (tree)
, root_node_ ()
, parent_ ()
, depth_ ()
, children_ (std::vector <OutofcoreOctreeBaseNode<ContainerT, PointT>*> (8,static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>*> (0)))
, num_children_ (0)
, num_loaded_children_ (0)
, payload_ ()
, node_metadata_ (new OutofcoreOctreeNodeMetadata ())
{
assert (tree != NULL);
node_metadata_->setOutofcoreVersion (3);
init_root_node (bb_min, bb_max, tree, root_name);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBaseNode<ContainerT, PointT>::init_root_node (const Eigen::Vector3d& bb_min, const Eigen::Vector3d& bb_max, OutofcoreOctreeBase<ContainerT, PointT> * const tree, const boost::filesystem::path& root_name)
{
assert (tree != NULL);
parent_ = NULL;
root_node_ = this;
m_tree_ = tree;
depth_ = 0;
//Mark the children as unallocated
num_children_ = 0;
Eigen::Vector3d tmp_max = bb_max;
Eigen::Vector3d tmp_min = bb_min;
// Need to make the bounding box slightly bigger so points that fall on the max side aren't excluded
double epsilon = 1e-8;
tmp_max = tmp_max + epsilon*Eigen::Vector3d (1.0, 1.0, 1.0);
node_metadata_->setBoundingBox (tmp_min, tmp_max);
node_metadata_->setDirectoryPathname (root_name.parent_path ());
node_metadata_->setOutofcoreVersion (3);
// If the root directory doesn't exist create it
if (!boost::filesystem::exists (node_metadata_->getDirectoryPathname ()))
{
boost::filesystem::create_directory (node_metadata_->getDirectoryPathname ());
}
// If the root directory is a file, do not continue
else if (!boost::filesystem::is_directory (node_metadata_->getDirectoryPathname ()))
{
PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Need empty directory structure. Dir %s exists and is a file.\n",node_metadata_->getDirectoryPathname ().c_str ());
PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Bad Path: Directory Already Exists");
}
// Create a unique id for node file name
std::string uuid;
OutofcoreOctreeDiskContainer<PointT>::getRandomUUIDString (uuid);
std::string node_container_name;
node_container_name = uuid + std::string ("_") + node_container_basename + pcd_extension;
node_metadata_->setMetadataFilename (node_metadata_->getDirectoryPathname () / root_name.filename ());
node_metadata_->setPCDFilename (node_metadata_->getDirectoryPathname () / boost::filesystem::path (node_container_name));
boost::filesystem::create_directory (node_metadata_->getDirectoryPathname ());
node_metadata_->serializeMetadataToDisk ();
// Create data container, ie octree_disk_container, octree_ram_container
payload_ = boost::shared_ptr<ContainerT> (new ContainerT (node_metadata_->getPCDFilename ()));
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT>
OutofcoreOctreeBaseNode<ContainerT, PointT>::~OutofcoreOctreeBaseNode ()
{
// Recursively delete all children and this nodes data
recFreeChildren ();
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> size_t
OutofcoreOctreeBaseNode<ContainerT, PointT>::countNumChildren () const
{
size_t child_count = 0;
for(size_t i=0; i<8; i++)
{
boost::filesystem::path child_path = this->node_metadata_->getDirectoryPathname () / boost::filesystem::path (boost::lexical_cast<std::string> (i));
if (boost::filesystem::exists (child_path))
child_count++;
}
return (child_count);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBaseNode<ContainerT, PointT>::saveIdx (bool recursive)
{
node_metadata_->serializeMetadataToDisk ();
if (recursive)
{
for (size_t i = 0; i < 8; i++)
{
if (children_[i])
children_[i]->saveIdx (true);
}
}
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> bool
OutofcoreOctreeBaseNode<ContainerT, PointT>::hasUnloadedChildren () const
{
if (this->getNumLoadedChildren () < this->getNumChildren ())
return (true);
else
return (false);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBaseNode<ContainerT, PointT>::loadChildren (bool recursive)
{
//if we have fewer children loaded than exist on disk, load them
if (num_loaded_children_ < this->getNumChildren ())
{
//check all 8 possible child directories
for (int i = 0; i < 8; i++)
{
boost::filesystem::path child_dir = node_metadata_->getDirectoryPathname () / boost::filesystem::path (boost::lexical_cast<std::string> (i));
//if the directory exists and the child hasn't been created (set to 0 by this node's constructor)
if (boost::filesystem::exists (child_dir) && this->children_[i] == 0)
{
//load the child node
this->children_[i] = new OutofcoreOctreeBaseNode<ContainerT, PointT> (child_dir, this, recursive);
//keep track of the children loaded
num_loaded_children_++;
}
}
}
assert (num_loaded_children_ == this->getNumChildren ());
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBaseNode<ContainerT, PointT>::recFreeChildren ()
{
if (num_children_ == 0)
{
return;
}
for (size_t i = 0; i < 8; i++)
{
if (children_[i])
{
OutofcoreOctreeBaseNode<ContainerT, PointT>* current = children_[i];
delete (current);
}
}
children_.resize (8, static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>* > (0));
num_children_ = 0;
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> uint64_t
OutofcoreOctreeBaseNode<ContainerT, PointT>::addDataToLeaf (const AlignedPointTVector& p, const bool skip_bb_check)
{
//quit if there are no points to add
if (p.empty ())
{
return (0);
}
//if this depth is the max depth of the tree, then add the points
if (this->depth_ == this->root_node_->m_tree_->getDepth ())
return (addDataAtMaxDepth( p, skip_bb_check));
if (hasUnloadedChildren ())
loadChildren (false);
std::vector < std::vector<const PointT*> > c;
c.resize (8);
for (size_t i = 0; i < 8; i++)
{
c[i].reserve (p.size () / 8);
}
const size_t len = p.size ();
for (size_t i = 0; i < len; i++)
{
const PointT& pt = p[i];
if (!skip_bb_check)
{
if (!this->pointInBoundingBox (pt))
{
PCL_ERROR ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Failed to place point within bounding box\n", __FUNCTION__ );
continue;
}
}
uint8_t box = 0;
Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
box = static_cast<uint8_t>(((pt.z >= mid_xyz[2]) << 2) | ((pt.y >= mid_xyz[1]) << 1) | ((pt.x >= mid_xyz[0]) << 0));
c[static_cast<size_t>(box)].push_back (&pt);
}
boost::uint64_t points_added = 0;
for (size_t i = 0; i < 8; i++)
{
if (c[i].empty ())
continue;
if (!children_[i])
createChild (i);
points_added += children_[i]->addDataToLeaf (c[i], true);
c[i].clear ();
}
return (points_added);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> boost::uint64_t
OutofcoreOctreeBaseNode<ContainerT, PointT>::addDataToLeaf (const std::vector<const PointT*>& p, const bool skip_bb_check)
{
if (p.empty ())
{
return (0);
}
if (this->depth_ == this->root_node_->m_tree_->getDepth ())
{
//trust me, just add the points
if (skip_bb_check)
{
root_node_->m_tree_->incrementPointsInLOD (this->depth_, p.size ());
payload_->insertRange (p.data (), p.size ());
return (p.size ());
}
else//check which points belong to this node, throw away the rest
{
std::vector<const PointT*> buff;
BOOST_FOREACH(const PointT* pt, p)
{
if(pointInBoundingBox(*pt))
{
buff.push_back(pt);
}
}
if (!buff.empty ())
{
root_node_->m_tree_->incrementPointsInLOD (this->depth_, buff.size ());
payload_->insertRange (buff.data (), buff.size ());
// payload_->insertRange ( buff );
}
return (buff.size ());
}
}
else
{
if (this->hasUnloadedChildren ())
{
loadChildren (false);
}
std::vector < std::vector<const PointT*> > c;
c.resize (8);
for (size_t i = 0; i < 8; i++)
{
c[i].reserve (p.size () / 8);
}
const size_t len = p.size ();
for (size_t i = 0; i < len; i++)
{
//const PointT& pt = p[i];
if (!skip_bb_check)
{
if (!this->pointInBoundingBox (*p[i]))
{
// std::cerr << "failed to place point!!!" << std::endl;
continue;
}
}
uint8_t box = 00;
Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
//hash each coordinate to the appropriate octant
box = static_cast<uint8_t> (((p[i]->z >= mid_xyz[2]) << 2) | ((p[i]->y >= mid_xyz[1]) << 1) | ((p[i]->x >= mid_xyz[0] )));
//3 bit, 8 octants
c[box].push_back (p[i]);
}
boost::uint64_t points_added = 0;
for (size_t i = 0; i < 8; i++)
{
if (c[i].empty ())
continue;
if (!children_[i])
createChild (i);
points_added += children_[i]->addDataToLeaf (c[i], true);
c[i].clear ();
}
return (points_added);
}
// std::cerr << "failed to place point!!!" << std::endl;
return (0);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> boost::uint64_t
OutofcoreOctreeBaseNode<ContainerT, PointT>::addPointCloud (const typename pcl::PCLPointCloud2::Ptr& input_cloud, const bool skip_bb_check)
{
assert (this->root_node_->m_tree_ != NULL);
if (input_cloud->height*input_cloud->width == 0)
return (0);
if (this->depth_ == this->root_node_->m_tree_->getDepth ())
return (addDataAtMaxDepth (input_cloud, true));
if( num_children_ < 8 )
if(hasUnloadedChildren ())
loadChildren (false);
if( skip_bb_check == false )
{
//indices to store the points for each bin
//these lists will be used to copy data to new point clouds and pass down recursively
std::vector < std::vector<int> > indices;
indices.resize (8);
this->sortOctantIndices (input_cloud, indices, node_metadata_->getVoxelCenter ());
for(size_t k=0; k<indices.size (); k++)
{
PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Computed %d indices in octact %d\n", __FUNCTION__, indices[k].size (), k);
}
boost::uint64_t points_added = 0;
for(size_t i=0; i<8; i++)
{
if ( indices[i].empty () )
continue;
if ( children_[i] == false )
{
createChild (i);
}
pcl::PCLPointCloud2::Ptr dst_cloud (new pcl::PCLPointCloud2 () );
PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Extracting indices to bins\n", __FUNCTION__);
//copy the points from extracted indices from input cloud to destination cloud
pcl::copyPointCloud ( *input_cloud, indices[i], *dst_cloud ) ;
//recursively add the new cloud to the data
points_added += children_[i]->addPointCloud (dst_cloud, false);
indices[i].clear ();
}
return (points_added);
}
PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Skipped bounding box check. Points not inserted\n");
return 0;
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBaseNode<ContainerT, PointT>::randomSample(const AlignedPointTVector& p, AlignedPointTVector& insertBuff, const bool skip_bb_check)
{
assert (this->root_node_->m_tree_ != NULL);
AlignedPointTVector sampleBuff;
if (!skip_bb_check)
{
BOOST_FOREACH (const PointT& pt, p)
if(pointInBoundingBox(pt))
sampleBuff.push_back(pt);
}
else
{
sampleBuff = p;
}
// Derive percentage from specified sample_percent and tree depth
const double percent = pow(sample_percent_, double((this->root_node_->m_tree_->getDepth () - depth_)));
const uint64_t samplesize = static_cast<uint64_t>(percent * static_cast<double>(sampleBuff.size()));
const uint64_t inputsize = sampleBuff.size();
if(samplesize > 0)
{
// Resize buffer to sample size
insertBuff.resize(samplesize);
// Create random number generator
boost::mutex::scoped_lock lock(rng_mutex_);
boost::uniform_int<boost::uint64_t> buffdist(0, inputsize-1);
boost::variate_generator<boost::mt19937&, boost::uniform_int<boost::uint64_t> > buffdie(rand_gen_, buffdist);
// Randomly pick sampled points
for(boost::uint64_t i = 0; i < samplesize; ++i)
{
boost::uint64_t buffstart = buffdie();
insertBuff[i] = ( sampleBuff[buffstart] );
}
}
// Have to do it the slow way
else
{
boost::mutex::scoped_lock lock(rng_mutex_);
boost::bernoulli_distribution<double> buffdist(percent);
boost::variate_generator<boost::mt19937&, boost::bernoulli_distribution<double> > buffcoin(rand_gen_, buffdist);
for(boost::uint64_t i = 0; i < inputsize; ++i)
if(buffcoin())
insertBuff.push_back( p[i] );
}
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> boost::uint64_t
OutofcoreOctreeBaseNode<ContainerT, PointT>::addDataAtMaxDepth (const AlignedPointTVector& p, const bool skip_bb_check)
{
assert (this->root_node_->m_tree_ != NULL);
// Trust me, just add the points
if (skip_bb_check)
{
// Increment point count for node
root_node_->m_tree_->incrementPointsInLOD (this->depth_, p.size ());
// Insert point data
payload_->insertRange ( p );
return (p.size ());
}
// Add points found within the current node's bounding box
else
{
AlignedPointTVector buff;
const size_t len = p.size ();
for (size_t i = 0; i < len; i++)
{
if (pointInBoundingBox (p[i]))
{
buff.push_back (p[i]);
}
}
if (!buff.empty ())
{
root_node_->m_tree_->incrementPointsInLOD (this->depth_, buff.size ());
payload_->insertRange ( buff );
}
return (buff.size ());
}
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> boost::uint64_t
OutofcoreOctreeBaseNode<ContainerT, PointT>::addDataAtMaxDepth (const pcl::PCLPointCloud2::Ptr input_cloud, const bool skip_bb_check)
{
//this assumes data is already in the correct bin
if(skip_bb_check == true)
{
PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Adding %u points at max depth, %u\n",__FUNCTION__, input_cloud->width*input_cloud->height, this->depth_);
this->root_node_->m_tree_->incrementPointsInLOD (this->depth_, input_cloud->width*input_cloud->height );
payload_->insertRange (input_cloud);
return (input_cloud->width*input_cloud->height);
}
else
{
PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Not implemented\n");
return (0);
}
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBaseNode<ContainerT, PointT>::subdividePoints (const AlignedPointTVector &p, std::vector< AlignedPointTVector > &c, const bool skip_bb_check)
{
// Reserve space for children nodes
c.resize(8);
for(size_t i = 0; i < 8; i++)
c[i].reserve(p.size() / 8);
const size_t len = p.size();
for(size_t i = 0; i < len; i++)
{
const PointT& pt = p[i];
if(!skip_bb_check)
if(!this->pointInBoundingBox(pt))
continue;
subdividePoint (pt, c);
}
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBaseNode<ContainerT, PointT>::subdividePoint (const PointT& point, std::vector< AlignedPointTVector >& c)
{
Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
size_t octant = 0;
octant = ((point.z >= mid_xyz[2]) << 2) | ((point.y >= mid_xyz[1]) << 1) | ((point.x >= mid_xyz[0]) << 0);
c[octant].push_back (point);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> boost::uint64_t
OutofcoreOctreeBaseNode<ContainerT, PointT>::addPointCloud_and_genLOD (const pcl::PCLPointCloud2::Ptr input_cloud) //, const bool skip_bb_check = false )
{
boost::uint64_t points_added = 0;
if ( input_cloud->width * input_cloud->height == 0 )
{
return (0);
}
if ( this->depth_ == this->root_node_->m_tree_->getDepth () || input_cloud->width*input_cloud->height < 8 )
{
uint64_t points_added = addDataAtMaxDepth (input_cloud, true);
assert (points_added > 0);
return (points_added);
}
if (num_children_ < 8 )
{
if ( hasUnloadedChildren () )
{
loadChildren (false);
}
}
//------------------------------------------------------------
//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)
//------------------------------------------------------------
pcl::RandomSample<pcl::PCLPointCloud2> random_sampler;
random_sampler.setInputCloud (input_cloud);
//set sample size to 1/8 of total points (12.5%)
uint64_t sample_size = input_cloud->width*input_cloud->height / 8;
random_sampler.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 > () );
random_sampler.filter (*downsampled_cloud_indices);
//extract the "random subset", size by setSampleSize
pcl::ExtractIndices<pcl::PCLPointCloud2> extractor;
extractor.setInputCloud (input_cloud);
extractor.setIndices (downsampled_cloud_indices);
extractor.filter (*downsampled_cloud);
//extract the complement of those points (i.e. everything remaining)
pcl::PCLPointCloud2::Ptr remaining_points ( new pcl::PCLPointCloud2 () );
extractor.setNegative (true);
extractor.filter (*remaining_points);
PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Random sampled: %lu of %lu\n", __FUNCTION__, downsampled_cloud->width * downsampled_cloud->height, input_cloud->width * input_cloud->height );
//insert subsampled data to the node's disk container payload
if ( downsampled_cloud->width * downsampled_cloud->height != 0 )
{
root_node_->m_tree_->incrementPointsInLOD ( this->depth_, downsampled_cloud->width * downsampled_cloud->height );
payload_->insertRange (downsampled_cloud);
points_added += downsampled_cloud->width*downsampled_cloud->height ;
}
PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Remaining points are %u\n",__FUNCTION__, remaining_points->width*remaining_points->height);
//subdivide remaining data by destination octant
std::vector<std::vector<int> > indices;
indices.resize (8);
this->sortOctantIndices (remaining_points, indices, node_metadata_->getVoxelCenter ());
//pass each set of points to the appropriate child octant
for(size_t i=0; i<8; i++)
{
if(indices[i].empty ())
continue;
if( children_[i] == false )
{
assert (i < 8);
createChild (i);
}
//copy correct indices into a temporary cloud
pcl::PCLPointCloud2::Ptr tmp_local_point_cloud (new pcl::PCLPointCloud2 ());
pcl::copyPointCloud (*remaining_points, indices[i], *tmp_local_point_cloud);
//recursively add points and keep track of how many were successfully added to the tree
points_added += children_[i]->addPointCloud_and_genLOD (tmp_local_point_cloud);
PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] points_added: %lu, indices[i].size: %lu, tmp_local_point_cloud size: %lu\n", __FUNCTION__, points_added, indices[i].size (), tmp_local_point_cloud->width*tmp_local_point_cloud->height);
}
assert (points_added == input_cloud->width*input_cloud->height);
return (points_added);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> boost::uint64_t
OutofcoreOctreeBaseNode<ContainerT, PointT>::addDataToLeaf_and_genLOD (const AlignedPointTVector& p, const bool skip_bb_check)
{
// If there are no points return
if (p.empty ())
return (0);
// when adding data and generating sampled LOD
// If the max depth has been reached
assert (this->root_node_->m_tree_ != NULL );
if (this->depth_ == this->root_node_->m_tree_->getDepth ())
{
PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::addDataToLeaf_and_genLOD] Adding data to the leaves\n");
return (addDataAtMaxDepth(p, false));
}
// Create child nodes of the current node but not grand children+
if (this->hasUnloadedChildren ())
loadChildren (false /*no recursive loading*/);
// Randomly sample data
AlignedPointTVector insertBuff;
randomSample(p, insertBuff, skip_bb_check);
if(!insertBuff.empty())
{
// Increment point count for node
root_node_->m_tree_->incrementPointsInLOD (this->depth_, insertBuff.size());
// Insert sampled point data
payload_->insertRange (insertBuff);
}
//subdivide vec to pass data down lower
std::vector<AlignedPointTVector> c;
subdividePoints(p, c, skip_bb_check);
boost::uint64_t points_added = 0;
for(size_t i = 0; i < 8; i++)
{
// If child doesn't have points
if(c[i].empty())
continue;
// If child doesn't exist
if(!children_[i])
createChild(i);
// Recursively build children
points_added += children_[i]->addDataToLeaf_and_genLOD(c[i], true);
c[i].clear();
}
return (points_added);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBaseNode<ContainerT, PointT>::createChild (const size_t idx)
{
assert (idx < 8);
//if already has 8 children, return
if (children_[idx] || (num_children_ == 8))
{
PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode::createChild] Not allowed to create a 9th child of %s",this->node_metadata_->getMetadataFilename ().c_str ());
return;
}
Eigen::Vector3d start = node_metadata_->getBoundingBoxMin ();
Eigen::Vector3d step = (node_metadata_->getBoundingBoxMax () - start)/static_cast<double>(2.0);
Eigen::Vector3d childbb_min;
Eigen::Vector3d childbb_max;
int x, y, z;
if (idx > 3)
{
x = ((idx == 5) || (idx == 7)) ? 1 : 0;
y = ((idx == 6) || (idx == 7)) ? 1 : 0;
z = 1;
}
else
{
x = ((idx == 1) || (idx == 3)) ? 1 : 0;
y = ((idx == 2) || (idx == 3)) ? 1 : 0;
z = 0;
}
childbb_min[2] = start[2] + static_cast<double> (z) * step[2];
childbb_max[2] = start[2] + static_cast<double> (z + 1) * step[2];
childbb_min[1] = start[1] + static_cast<double> (y) * step[1];
childbb_max[1] = start[1] + static_cast<double> (y + 1) * step[1];
childbb_min[0] = start[0] + static_cast<double> (x) * step[0];
childbb_max[0] = start[0] + static_cast<double> (x + 1) * step[0];
boost::filesystem::path childdir = node_metadata_->getDirectoryPathname () / boost::filesystem::path (boost::lexical_cast<std::string> (idx));
children_[idx] = new OutofcoreOctreeBaseNode<ContainerT, PointT> (childbb_min, childbb_max, childdir.string ().c_str (), this);
num_children_++;
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> bool
pointInBoundingBox (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, const Eigen::Vector3d& point)
{
if (((min_bb[0] <= point[0]) && (point[0] < max_bb[0])) &&
((min_bb[1] <= point[1]) && (point[1] < max_bb[1])) &&
((min_bb[2] <= point[2]) && (point[2] < max_bb[2])))
{
return (true);
}
return (false);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> bool
OutofcoreOctreeBaseNode<ContainerT, PointT>::pointInBoundingBox (const PointT& p) const
{
const Eigen::Vector3d& min = node_metadata_->getBoundingBoxMin ();
const Eigen::Vector3d& max = node_metadata_->getBoundingBoxMax ();
if (((min[0] <= p.x) && (p.x < max[0])) &&
((min[1] <= p.y) && (p.y < max[1])) &&
((min[2] <= p.z) && (p.z < max[2])))
{
return (true);
}
return (false);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBaseNode<ContainerT, PointT>::printBoundingBox (const size_t query_depth) const
{
Eigen::Vector3d min;
Eigen::Vector3d max;
node_metadata_->getBoundingBox (min, max);
if (this->depth_ < query_depth){
for (size_t i = 0; i < this->depth_; i++)
std::cout << " ";
std::cout << "[" << min[0] << ", " << min[1] << ", " << min[2] << "] - ";
std::cout << "[" << max[0] << ", " << max[1] << ", " << max[2] << "] - ";
std::cout << "[" << max[0] - min[0] << ", " << max[1] - min[1];
std::cout << ", " << max[2] - min[2] << "]" << std::endl;
if (num_children_ > 0)
{
for (size_t i = 0; i < 8; i++)
{
if (children_[i])
children_[i]->printBoundingBox (query_depth);
}
}
}
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBaseNode<ContainerT, PointT>::getOccupiedVoxelCentersRecursive (AlignedPointTVector &voxel_centers, const size_t query_depth)
{
if (this->depth_ < query_depth){
if (num_children_ > 0)
{
for (size_t i = 0; i < 8; i++)
{
if (children_[i])
children_[i]->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
}
}
}
else
{
PointT voxel_center;
Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
voxel_center.x = static_cast<float>(mid_xyz[0]);
voxel_center.y = static_cast<float>(mid_xyz[1]);
voxel_center.z = static_cast<float>(mid_xyz[2]);
voxel_centers.push_back(voxel_center);
}
}
////////////////////////////////////////////////////////////////////////////////
// Eigen::Vector3d cornerOffsets[] =
// {
// Eigen::Vector3d(-1.0, -1.0, -1.0), // - - -
// Eigen::Vector3d( 1.0, -1.0, -1.0), // - - +
// Eigen::Vector3d(-1.0, 1.0, -1.0), // - + -
// Eigen::Vector3d( 1.0, 1.0, -1.0), // - + +
// Eigen::Vector3d(-1.0, -1.0, 1.0), // + - -
// Eigen::Vector3d( 1.0, -1.0, 1.0), // + - +
// Eigen::Vector3d(-1.0, 1.0, 1.0), // + + -
// Eigen::Vector3d( 1.0, 1.0, 1.0) // + + +
// };
//
// // Note that the input vector must already be negated when using this code for halfplane tests
// int
// vectorToIndex(Eigen::Vector3d normal)
// {
// int index = 0;
//
// if (normal.z () >= 0) index |= 1;
// if (normal.y () >= 0) index |= 2;
// if (normal.x () >= 0) index |= 4;
//
// return index;
// }
//
// void
// get_np_vertices(Eigen::Vector3d normal, Eigen::Vector3d &p_vertex, Eigen::Vector3d &n_vertex, Eigen::Vector3d min_bb, Eigen::Vector3d max_bb)
// {
//
// p_vertex = min_bb;
// n_vertex = max_bb;
//
// if (normal.x () >= 0)
// {
// n_vertex.x () = min_bb.x ();
// p_vertex.x () = max_bb.x ();
// }
//
// if (normal.y () >= 0)
// {
// n_vertex.y () = min_bb.y ();
// p_vertex.y () = max_bb.y ();
// }
//
// if (normal.z () >= 0)
// {
// p_vertex.z () = max_bb.z ();
// n_vertex.z () = min_bb.z ();
// }
// }
template<typename Container, typename PointT> void
OutofcoreOctreeBaseNode<Container, PointT>::queryFrustum (const double planes[24], std::list<std::string>& file_names)
{
queryFrustum(planes, file_names, this->m_tree_->getTreeDepth());
}
template<typename Container, typename PointT> void
OutofcoreOctreeBaseNode<Container, PointT>::queryFrustum (const double planes[24], std::list<std::string>& file_names, const boost::uint32_t query_depth, const bool skip_vfc_check)
{
enum {INSIDE, INTERSECT, OUTSIDE};
int result = INSIDE;
if (this->depth_ > query_depth)
{
return;
}
// if (this->depth_ > query_depth)
// return;
if (!skip_vfc_check)
{
for(int i =0; i < 6; i++){
double a = planes[(i*4)];
double b = planes[(i*4)+1];
double c = planes[(i*4)+2];
double d = planes[(i*4)+3];
//cout << i << ": " << a << "x + " << b << "y + " << c << "z + " << d << endl;
Eigen::Vector3d normal(a, b, c);
Eigen::Vector3d min_bb;
Eigen::Vector3d max_bb;
node_metadata_->getBoundingBox(min_bb, max_bb);
// Basic VFC algorithm
Eigen::Vector3d center = node_metadata_->getVoxelCenter();
Eigen::Vector3d radius (fabs (static_cast<double> (max_bb.x () - center.x ())),
fabs (static_cast<double> (max_bb.y () - center.y ())),
fabs (static_cast<double> (max_bb.z () - center.z ())));
double m = (center.x () * a) + (center.y () * b) + (center.z () * c) + d;
double n = (radius.x () * fabs(a)) + (radius.y () * fabs(b)) + (radius.z () * fabs(c));
if (m + n < 0){
result = OUTSIDE;
break;
}
if (m - n < 0) result = INTERSECT;
// // n-p implementation
// Eigen::Vector3d p_vertex; //pos vertex
// Eigen::Vector3d n_vertex; //neg vertex
// get_np_vertices(normal, p_vertex, n_vertex, min_bb, max_bb);
//
// cout << "n_vertex: " << n_vertex.x () << ", " << n_vertex.y () << ", " << n_vertex.z () << endl;
// cout << "p_vertex: " << p_vertex.x () << ", " << p_vertex.y () << ", " << p_vertex.z () << endl;
// is the positive vertex outside?
// if (pl[i].distance(b.getVertexP(pl[i].normal)) < 0)
// {
// result = OUTSIDE;
// }
// // is the negative vertex outside?
// else if (pl[i].distance(b.getVertexN(pl[i].normal)) < 0)
// {
// result = INTERSECT;
// }
//
//
// // This should be the same as below
// if (normal.dot(n_vertex) + d > 0)
// {
// result = OUTSIDE;
// }
//
// if (normal.dot(p_vertex) + d >= 0)
// {
// result = INTERSECT;
// }
// This should be the same as above
// double m = (a * n_vertex.x ()) + (b * n_vertex.y ()) + (c * n_vertex.z ());
// cout << "m = " << m << endl;
// if (m > -d)
// {
// result = OUTSIDE;
// }
//
// double n = (a * p_vertex.x ()) + (b * p_vertex.y ()) + (c * p_vertex.z ());
// cout << "n = " << n << endl;
// if (n > -d)
// {
// result = INTERSECT;
// }
}
}
if (result == OUTSIDE)
{
return;
}
// switch(result){
// case OUTSIDE:
// //cout << this->depth_ << " [OUTSIDE]: " << node_metadata_->getPCDFilename() << endl;
// return;
// case INTERSECT:
// //cout << this->depth_ << " [INTERSECT]: " << node_metadata_->getPCDFilename() << endl;
// break;
// case INSIDE:
// //cout << this->depth_ << " [INSIDE]: " << node_metadata_->getPCDFilename() << endl;
// break;
// }
// Add files breadth first
if (this->depth_ == query_depth && payload_->getDataSize () > 0)
//if (payload_->getDataSize () > 0)
{
file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
}
if (hasUnloadedChildren ())
{
loadChildren (false);
}
if (this->getNumChildren () > 0)
{
for (size_t i = 0; i < 8; i++)
{
if (children_[i])
children_[i]->queryFrustum (planes, file_names, query_depth, (result == INSIDE) /*skip_vfc_check*/);
}
}
// else if (hasUnloadedChildren ())
// {
// loadChildren (false);
//
// for (size_t i = 0; i < 8; i++)
// {
// if (children_[i])
// children_[i]->queryFrustum (planes, file_names, query_depth);
// }
// }
//}
}
////////////////////////////////////////////////////////////////////////////////
template<typename Container, typename PointT> void
OutofcoreOctreeBaseNode<Container, PointT>::queryFrustum (const double planes[24], const Eigen::Vector3d &eye, const Eigen::Matrix4d &view_projection_matrix, std::list<std::string>& file_names, const boost::uint32_t query_depth, const bool skip_vfc_check)
{
// If we're above our query depth
if (this->depth_ > query_depth)
{
return;
}
// Bounding Box
Eigen::Vector3d min_bb;
Eigen::Vector3d max_bb;
node_metadata_->getBoundingBox(min_bb, max_bb);
// Frustum Culling
enum {INSIDE, INTERSECT, OUTSIDE};
int result = INSIDE;
if (!skip_vfc_check)
{
for(int i =0; i < 6; i++){
double a = planes[(i*4)];
double b = planes[(i*4)+1];
double c = planes[(i*4)+2];
double d = planes[(i*4)+3];
//cout << i << ": " << a << "x + " << b << "y + " << c << "z + " << d << endl;
Eigen::Vector3d normal(a, b, c);
// Basic VFC algorithm
Eigen::Vector3d center = node_metadata_->getVoxelCenter();
Eigen::Vector3d radius (fabs (static_cast<double> (max_bb.x () - center.x ())),
fabs (static_cast<double> (max_bb.y () - center.y ())),
fabs (static_cast<double> (max_bb.z () - center.z ())));
double m = (center.x () * a) + (center.y () * b) + (center.z () * c) + d;
double n = (radius.x () * fabs(a)) + (radius.y () * fabs(b)) + (radius.z () * fabs(c));
if (m + n < 0){
result = OUTSIDE;
break;
}
if (m - n < 0) result = INTERSECT;
}
}
if (result == OUTSIDE)
{
return;
}
// Bounding box projection
// 3--------2
// /| /| Y 0 = xmin, ymin, zmin
// / | / | | 6 = xmax, ymax. zmax
// 7--------6 | |
// | | | | |
// | 0-----|--1 +------X
// | / | / /
// |/ |/ /
// 4--------5 Z
// bounding_box[0] = Eigen::Vector4d(min_bb.x (), min_bb.y (), min_bb.z (), 1.0);
// bounding_box[1] = Eigen::Vector4d(max_bb.x (), min_bb.y (), min_bb.z (), 1.0);
// bounding_box[2] = Eigen::Vector4d(max_bb.x (), max_bb.y (), min_bb.z (), 1.0);
// bounding_box[3] = Eigen::Vector4d(min_bb.x (), max_bb.y (), min_bb.z (), 1.0);
// bounding_box[4] = Eigen::Vector4d(min_bb.x (), min_bb.y (), max_bb.z (), 1.0);
// bounding_box[5] = Eigen::Vector4d(max_bb.x (), min_bb.y (), max_bb.z (), 1.0);
// bounding_box[6] = Eigen::Vector4d(max_bb.x (), max_bb.y (), max_bb.z (), 1.0);
// bounding_box[7] = Eigen::Vector4d(min_bb.x (), max_bb.y (), max_bb.z (), 1.0);
int width = 500;
int height = 500;
float coverage = pcl::visualization::viewScreenArea(eye, min_bb, max_bb, view_projection_matrix, width, height);
//float coverage = pcl::visualization::viewScreenArea(eye, bounding_box, view_projection_matrix);
// for (int i=0; i < this->depth_; i++) std::cout << " ";
// std::cout << this->depth_ << ": " << coverage << std::endl;
// Add files breadth first
if (this->depth_ <= query_depth && payload_->getDataSize () > 0)
//if (payload_->getDataSize () > 0)
{
file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
}
//if (coverage <= 0.075)
if (coverage <= 10000)
return;
if (hasUnloadedChildren ())
{
loadChildren (false);
}
if (this->getNumChildren () > 0)
{
for (size_t i = 0; i < 8; i++)
{
if (children_[i])
children_[i]->queryFrustum (planes, eye, view_projection_matrix, file_names, query_depth, (result == INSIDE) /*skip_vfc_check*/);
}
}
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBaseNode<ContainerT, PointT>::getOccupiedVoxelCentersRecursive (std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &voxel_centers, const size_t query_depth)
{
if (this->depth_ < query_depth){
if (num_children_ > 0)
{
for (size_t i = 0; i < 8; i++)
{
if (children_[i])
children_[i]->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
}
}
}
else
{
Eigen::Vector3d voxel_center = node_metadata_->getVoxelCenter ();
voxel_centers.push_back(voxel_center);
}
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBaseNode<ContainerT, PointT>::queryBBIntersects (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, const boost::uint32_t query_depth, std::list<std::string>& file_names)
{
Eigen::Vector3d my_min = min_bb;
Eigen::Vector3d my_max = max_bb;
if (intersectsWithBoundingBox (my_min, my_max))
{
if (this->depth_ < query_depth)
{
if (this->getNumChildren () > 0)
{
for (size_t i = 0; i < 8; i++)
{
if (children_[i])
children_[i]->queryBBIntersects (my_min, my_max, query_depth, file_names);
}
}
else if (hasUnloadedChildren ())
{
loadChildren (false);
for (size_t i = 0; i < 8; i++)
{
if (children_[i])
children_[i]->queryBBIntersects (my_min, my_max, query_depth, file_names);
}
}
return;
}
if (payload_->getDataSize () > 0)
{
file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
}
}
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBaseNode<ContainerT, PointT>::queryBBIncludes (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, size_t query_depth, const pcl::PCLPointCloud2::Ptr& dst_blob)
{
uint64_t startingSize = dst_blob->width*dst_blob->height;
PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Starting points in destination blob: %ul\n", __FUNCTION__, startingSize );
// If the queried bounding box has any intersection with this node's bounding box
if (intersectsWithBoundingBox (min_bb, max_bb))
{
// If we aren't at the max desired depth
if (this->depth_ < query_depth)
{
//if this node doesn't have any children, we are at the max depth for this query
if ((num_children_ == 0) && (hasUnloadedChildren ()))
loadChildren (false);
//if this node has children
if (num_children_ > 0)
{
//recursively store any points that fall into the queried bounding box into v and return
for (size_t i = 0; i < 8; i++)
{
if (children_[i])
children_[i]->queryBBIncludes (min_bb, max_bb, query_depth, dst_blob);
}
PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in dst_blob: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height );
return;
}
}
else //otherwise if we are at the max depth
{
//get all the points from the payload and return (easy with PCLPointCloud2)
pcl::PCLPointCloud2::Ptr tmp_blob (new pcl::PCLPointCloud2 ());
pcl::PCLPointCloud2::Ptr tmp_dst_blob (new pcl::PCLPointCloud2 ());
//load all the data in this node from disk
payload_->readRange (0, payload_->size (), tmp_blob);
if( tmp_blob->width*tmp_blob->height == 0 )
return;
//if this node's bounding box falls completely within the queried bounding box, keep all the points
if (inBoundingBox (min_bb, max_bb))
{
//concatenate all of what was just read into the main dst_blob
//(is it safe to do in place?)
//if there is already something in the destination blob (remember this method is recursive)
if( dst_blob->width*dst_blob->height != 0 )
{
PCL_DEBUG ("[pcl::outofocre::OutofcoreOctreeBaseNode::%s] Size of cloud before: %lu\n", __FUNCTION__, dst_blob->width*dst_blob->height );
PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Concatenating point cloud\n", __FUNCTION__);
int res = pcl::concatenatePointCloud (*dst_blob, *tmp_blob, *dst_blob);
(void)res;
assert (res == 1);
PCL_DEBUG ("[pcl::outofocre::OutofcoreOctreeBaseNode::%s] Size of cloud after: %lu\n", __FUNCTION__, dst_blob->width*dst_blob->height );
}
//otherwise, just copy the tmp_blob into the dst_blob
else
{
PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode] Copying point cloud into the destination blob\n");
pcl::copyPointCloud (*tmp_blob, *dst_blob);
assert (tmp_blob->width*tmp_blob->height == dst_blob->width*dst_blob->height);
}
PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in dst_blob: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height );
return;
}
else
{
//otherwise queried bounding box only partially intersects this
//node's bounding box, so we have to check all the points in
//this box for intersection with queried bounding box
// PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Partial extraction of points in bounding box. Desired: %.2lf %.2lf %2lf, %.2lf %.2lf %.2lf; This node BB: %.2lf %.2lf %.2lf, %.2lf %.2lf %.2lf\n", __FUNCTION__, min_bb[0], min_bb[1], min_bb[2], max_bb[0], max_bb[1], max_bb[2], min_[0], min_[1], min_[2], max_[0], max_[1], max_[2] );
//put the ros message into a pointxyz point cloud (just to get the indices by using getPointsInBox)
typename pcl::PointCloud<PointT>::Ptr tmp_cloud ( new pcl::PointCloud<PointT> () );
pcl::fromPCLPointCloud2 ( *tmp_blob, *tmp_cloud );
assert (tmp_blob->width*tmp_blob->height == tmp_cloud->width*tmp_cloud->height );
Eigen::Vector4f min_pt ( static_cast<float> ( min_bb[0] ), static_cast<float> ( min_bb[1] ), static_cast<float> ( min_bb[2] ), 1.0f);
Eigen::Vector4f max_pt ( static_cast<float> ( max_bb[0] ), static_cast<float> ( max_bb[1] ) , static_cast<float>( max_bb[2] ), 1.0f );
std::vector<int> indices;
pcl::getPointsInBox ( *tmp_cloud, min_pt, max_pt, indices );
PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in box: %d\n", __FUNCTION__, indices.size () );
PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points remaining: %d\n", __FUNCTION__, tmp_cloud->width*tmp_cloud->height - indices.size () );
if ( indices.size () > 0 )
{
if( dst_blob->width*dst_blob->height > 0 )
{
//need a new tmp destination with extracted points within BB
pcl::PCLPointCloud2::Ptr tmp_blob_within_bb (new pcl::PCLPointCloud2 ());
//copy just the points marked in indices
pcl::copyPointCloud ( *tmp_blob, indices, *tmp_blob_within_bb );
assert ( tmp_blob_within_bb->width*tmp_blob_within_bb->height == indices.size () );
assert ( tmp_blob->fields.size () == tmp_blob_within_bb->fields.size () );
//concatenate those points into the returned dst_blob
PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Concatenating point cloud in place\n", __FUNCTION__);
boost::uint64_t orig_points_in_destination = dst_blob->width*dst_blob->height;
(void)orig_points_in_destination;
int res = pcl::concatenatePointCloud (*dst_blob, *tmp_blob_within_bb, *dst_blob);
(void)res;
assert (res == 1);
assert (dst_blob->width*dst_blob->height == indices.size () + orig_points_in_destination);
}
else
{
pcl::copyPointCloud ( *tmp_blob, indices, *dst_blob );
assert ( dst_blob->width*dst_blob->height == indices.size () );
}
}
}
}
}
PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points added by function call: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height - startingSize );
}
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBaseNode<ContainerT, PointT>::queryBBIncludes (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, size_t query_depth, AlignedPointTVector& v)
{
//if the queried bounding box has any intersection with this node's bounding box
if (intersectsWithBoundingBox (min_bb, max_bb))
{
//if we aren't at the max desired depth
if (this->depth_ < query_depth)
{
//if this node doesn't have any children, we are at the max depth for this query
if (this->hasUnloadedChildren ())
{
this->loadChildren (false);
}
//if this node has children
if (getNumChildren () > 0)
{
if(hasUnloadedChildren ())
loadChildren (false);
//recursively store any points that fall into the queried bounding box into v and return
for (size_t i = 0; i < 8; i++)
{
if (children_[i])
children_[i]->queryBBIncludes (min_bb, max_bb, query_depth, v);
}
return;
}
}
//otherwise if we are at the max depth
else
{
//if this node's bounding box falls completely within the queried bounding box
if (inBoundingBox (min_bb, max_bb))
{
//get all the points from the payload and return
AlignedPointTVector payload_cache;
payload_->readRange (0, payload_->size (), payload_cache);
v.insert (v.end (), payload_cache.begin (), payload_cache.end ());
return;
}
//otherwise queried bounding box only partially intersects this
//node's bounding box, so we have to check all the points in
//this box for intersection with queried bounding box
else
{
//read _all_ the points in from the disk container
AlignedPointTVector payload_cache;
payload_->readRange (0, payload_->size (), payload_cache);
uint64_t len = payload_->size ();
//iterate through each of them
for (uint64_t i = 0; i < len; i++)
{
const PointT& p = payload_cache[i];
//if it falls within this bounding box
if (pointInBoundingBox (min_bb, max_bb, p))
{
//store it in the list
v.push_back (p);
}
else
{
PCL_DEBUG ("[pcl::outofcore::queryBBIncludes] Point %.2lf %.2lf %.2lf not in bounding box %.2lf %.2lf %.2lf", p.x, p.y, p.z, min_bb[0], min_bb[1], min_bb[2], max_bb[0], max_bb[1], max_bb[2]);
}
}
}
}
}
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBaseNode<ContainerT, PointT>::queryBBIncludes_subsample (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, boost::uint64_t query_depth, const pcl::PCLPointCloud2::Ptr& dst_blob, double percent)
{
if (intersectsWithBoundingBox (min_bb, max_bb))
{
if (this->depth_ < query_depth)
{
if (this->hasUnloadedChildren ())
this->loadChildren (false);
if (this->getNumChildren () > 0)
{
for (size_t i=0; i<8; i++)
{
//recursively traverse (depth first)
if (children_[i]!=0)
children_[i]->queryBBIncludes_subsample (min_bb, max_bb, query_depth, dst_blob, percent);
}
return;
}
}
//otherwise, at max depth --> read from disk, subsample, concatenate
else
{
if (inBoundingBox (min_bb, max_bb))
{
pcl::PCLPointCloud2::Ptr tmp_blob;
this->payload_->read (tmp_blob);
uint64_t num_pts = tmp_blob->width*tmp_blob->height;
double sample_points = static_cast<double>(num_pts) * percent;
if (num_pts > 0)
{
//always sample at least one point
sample_points = sample_points > 0 ? sample_points : 1;
}
else
{
return;
}
pcl::RandomSample<pcl::PCLPointCloud2> random_sampler;
random_sampler.setInputCloud (tmp_blob);
pcl::PCLPointCloud2::Ptr downsampled_points (new pcl::PCLPointCloud2 ());
//set sample size as percent * number of points read
random_sampler.setSample (static_cast<unsigned int> (sample_points));
pcl::ExtractIndices<pcl::PCLPointCloud2> extractor;
pcl::IndicesPtr downsampled_cloud_indices (new std::vector<int> ());
random_sampler.filter (*downsampled_cloud_indices);
extractor.setIndices (downsampled_cloud_indices);
extractor.filter (*downsampled_points);
//concatenate the result into the destination cloud
pcl::concatenatePointCloud (*dst_blob, *downsampled_points, *dst_blob);
}
}
}
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBaseNode<ContainerT, PointT>::queryBBIncludes_subsample (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, boost::uint64_t query_depth, const double percent, AlignedPointTVector& dst)
{
//check if the queried bounding box has any intersection with this node's bounding box
if (intersectsWithBoundingBox (min_bb, max_bb))
{
//if we are not at the max depth for queried nodes
if (this->depth_ < query_depth)
{
//check if we don't have children
if ((num_children_ == 0) && (hasUnloadedChildren ()))
{
loadChildren (false);
}
//if we do have children
if (num_children_ > 0)
{
//recursively add their valid points within the queried bounding box to the list v
for (size_t i = 0; i < 8; i++)
{
if (children_[i])
children_[i]->queryBBIncludes_subsample (min_bb, max_bb, query_depth, percent, dst);
}
return;
}
}
//otherwise we are at the max depth, so we add all our points or some of our points
else
{
//if this node's bounding box falls completely within the queried bounding box
if (inBoundingBox (min_bb, max_bb))
{
//add a random sample of all the points
AlignedPointTVector payload_cache;
payload_->readRangeSubSample (0, payload_->size (), percent, payload_cache);
dst.insert (dst.end (), payload_cache.begin (), payload_cache.end ());
return;
}
//otherwise the queried bounding box only partially intersects with this node's bounding box
else
{
//brute force selection of all valid points
AlignedPointTVector payload_cache_within_region;
{
AlignedPointTVector payload_cache;
payload_->readRange (0, payload_->size (), payload_cache);
for (size_t i = 0; i < payload_->size (); i++)
{
const PointT& p = payload_cache[i];
if (pointInBoundingBox (min_bb, max_bb, p))
{
payload_cache_within_region.push_back (p);
}
}
}//force the payload cache to deconstruct here
//use STL random_shuffle and push back a random selection of the points onto our list
std::random_shuffle (payload_cache_within_region.begin (), payload_cache_within_region.end ());
size_t numpick = static_cast<size_t> (percent * static_cast<double> (payload_cache_within_region.size ()));;
for (size_t i = 0; i < numpick; i++)
{
dst.push_back (payload_cache_within_region[i]);
}
}
}
}
}
////////////////////////////////////////////////////////////////////////////////
//dir is current level. we put this nodes files into it
template<typename ContainerT, typename PointT>
OutofcoreOctreeBaseNode<ContainerT, PointT>::OutofcoreOctreeBaseNode (const Eigen::Vector3d& bb_min, const Eigen::Vector3d& bb_max, const char* dir, OutofcoreOctreeBaseNode<ContainerT,PointT>* super)
: m_tree_ ()
, root_node_ ()
, parent_ ()
, depth_ ()
, children_ (std::vector <OutofcoreOctreeBaseNode<ContainerT, PointT>*> (8,static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>*>(0)))
, num_children_ ()
, num_loaded_children_ (0)
, payload_ ()
, node_metadata_ ()
{
node_metadata_ = boost::shared_ptr<OutofcoreOctreeNodeMetadata> (new OutofcoreOctreeNodeMetadata ());
node_metadata_->setOutofcoreVersion (3);
if (super == NULL)
{
PCL_ERROR ( "[pc::outofcore::OutofcoreOctreeBaseNode] Super is null - don't make a root node this way!\n" );
PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Exception: Bad parent");
}
this->parent_ = super;
root_node_ = super->root_node_;
m_tree_ = super->root_node_->m_tree_;
assert (m_tree_ != NULL);
depth_ = super->depth_ + 1;
num_children_ = 0;
node_metadata_->setBoundingBox (bb_min, bb_max);
std::string uuid_idx;
std::string uuid_cont;
OutofcoreOctreeDiskContainer<PointT>::getRandomUUIDString (uuid_idx);
OutofcoreOctreeDiskContainer<PointT>::getRandomUUIDString (uuid_cont);
std::string node_index_name = uuid_idx + std::string ("_") + node_index_basename + node_index_extension;
std::string node_container_name;
node_container_name = uuid_cont + std::string ("_") + node_container_basename + pcd_extension;
node_metadata_->setDirectoryPathname (boost::filesystem::path (dir));
node_metadata_->setPCDFilename (node_metadata_->getDirectoryPathname () / boost::filesystem::path (node_container_name));
node_metadata_->setMetadataFilename ( node_metadata_->getDirectoryPathname ()/boost::filesystem::path (node_index_name));
boost::filesystem::create_directory (node_metadata_->getDirectoryPathname ());
payload_ = boost::shared_ptr<ContainerT> (new ContainerT (node_metadata_->getPCDFilename ()));
this->saveIdx (false);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBaseNode<ContainerT, PointT>::copyAllCurrentAndChildPointsRec (std::list<PointT>& v)
{
if ((num_children_ == 0) && (hasUnloadedChildren ()))
{
loadChildren (false);
}
for (size_t i = 0; i < num_children_; i++)
{
children_[i]->copyAllCurrentAndChildPointsRec (v);
}
AlignedPointTVector payload_cache;
payload_->readRange (0, payload_->size (), payload_cache);
{
//boost::mutex::scoped_lock lock(queryBBIncludes_vector_mutex);
v.insert (v.end (), payload_cache.begin (), payload_cache.end ());
}
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBaseNode<ContainerT, PointT>::copyAllCurrentAndChildPointsRec_sub (std::list<PointT>& v, const double percent)
{
if ((num_children_ == 0) && (hasUnloadedChildren ()))
{
loadChildren (false);
}
for (size_t i = 0; i < 8; i++)
{
if (children_[i])
children_[i]->copyAllCurrentAndChildPointsRec_sub (v, percent);
}
std::vector<PointT> payload_cache;
payload_->readRangeSubSample (0, payload_->size (), percent, payload_cache);
for (size_t i = 0; i < payload_cache.size (); i++)
{
v.push_back (payload_cache[i]);
}
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> inline bool
OutofcoreOctreeBaseNode<ContainerT, PointT>::intersectsWithBoundingBox (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb) const
{
Eigen::Vector3d min, max;
node_metadata_->getBoundingBox (min, max);
//Check whether any portion of min_bb, max_bb falls within min,max
if (((min[0] <= min_bb[0]) && (min_bb[0] <= max[0])) || ((min_bb[0] <= min[0]) && (min[0] <= max_bb[0])))
{
if (((min[1] <= min_bb[1]) && (min_bb[1] <= max[1])) || ((min_bb[1] <= min[1]) && (min[1] <= max_bb[1])))
{
if (((min[2] <= min_bb[2]) && (min_bb[2] <= max[2])) || ((min_bb[2] <= min[2]) && (min[2] <= max_bb[2])))
{
return (true);
}
}
}
return (false);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> inline bool
OutofcoreOctreeBaseNode<ContainerT, PointT>::inBoundingBox (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb) const
{
Eigen::Vector3d min, max;
node_metadata_->getBoundingBox (min, max);
if ((min_bb[0] <= min[0]) && (max[0] <= max_bb[0]))
{
if ((min_bb[1] <= min[1]) && (max[1] <= max_bb[1]))
{
if ((min_bb[2] <= min[2]) && (max[2] <= max_bb[2]))
{
return (true);
}
}
}
return (false);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> inline bool
OutofcoreOctreeBaseNode<ContainerT, PointT>::pointInBoundingBox (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb,
const PointT& p)
{
//by convention, minimum boundary is included; maximum boundary is not
if ((min_bb[0] <= p.x) && (p.x < max_bb[0]))
{
if ((min_bb[1] <= p.y) && (p.y < max_bb[1]))
{
if ((min_bb[2] <= p.z) && (p.z < max_bb[2]))
{
return (true);
}
}
}
return (false);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBaseNode<ContainerT, PointT>::writeVPythonVisual (std::ofstream& file)
{
Eigen::Vector3d min;
Eigen::Vector3d max;
node_metadata_->getBoundingBox (min, max);
double l = max[0] - min[0];
double h = max[1] - min[1];
double w = max[2] - min[2];
file << "box( pos=(" << min[0] << ", " << min[1] << ", " << min[2] << "), length=" << l << ", height=" << h
<< ", width=" << w << " )\n";
for (size_t i = 0; i < num_children_; i++)
{
children_[i]->writeVPythonVisual (file);
}
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> int
OutofcoreOctreeBaseNode<ContainerT, PointT>::read (pcl::PCLPointCloud2::Ptr &output_cloud)
{
return (this->payload_->read (output_cloud));
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> OutofcoreOctreeBaseNode<ContainerT, PointT>*
OutofcoreOctreeBaseNode<ContainerT, PointT>::getChildPtr (size_t index_arg) const
{
PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] %d", __FUNCTION__, index_arg);
return (children_[index_arg]);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> boost::uint64_t
OutofcoreOctreeBaseNode<ContainerT, PointT>::getDataSize () const
{
return (this->payload_->getDataSize ());
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> size_t
OutofcoreOctreeBaseNode<ContainerT, PointT>::countNumLoadedChildren () const
{
size_t loaded_children_count = 0;
for (size_t i=0; i<8; i++)
{
if (children_[i] != 0)
loaded_children_count++;
}
return (loaded_children_count);
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBaseNode<ContainerT, PointT>::loadFromFile (const boost::filesystem::path& path, OutofcoreOctreeBaseNode<ContainerT, PointT>* super)
{
PCL_DEBUG ("[pcl:outofcore::OutofcoreOctreeBaseNode] Loading metadata from %s\n", path.filename ().c_str ());
node_metadata_->loadMetadataFromDisk (path);
//this shouldn't be part of 'loadFromFile'
this->parent_ = super;
if (num_children_ > 0)
recFreeChildren ();
this->num_children_ = 0;
this->payload_ = boost::shared_ptr<ContainerT> (new ContainerT (node_metadata_->getPCDFilename ()));
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBaseNode<ContainerT, PointT>::convertToXYZRecursive ()
{
std::string fname = boost::filesystem::basename (node_metadata_->getPCDFilename ()) + std::string (".dat.xyz");
boost::filesystem::path xyzfile = node_metadata_->getDirectoryPathname () / fname;
payload_->convertToXYZ (xyzfile);
if (hasUnloadedChildren ())
{
loadChildren (false);
}
for (size_t i = 0; i < 8; i++)
{
if (children_[i])
children_[i]->convertToXYZ ();
}
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBaseNode<ContainerT, PointT>::flushToDiskRecursive ()
{
for (size_t i = 0; i < 8; i++)
{
if (children_[i])
children_[i]->flushToDiskRecursive ();
}
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
OutofcoreOctreeBaseNode<ContainerT, PointT>::sortOctantIndices (const pcl::PCLPointCloud2::Ptr &input_cloud, std::vector< std::vector<int> > &indices, const Eigen::Vector3d &mid_xyz)
{
if (indices.size () < 8)
indices.resize (8);
int x_idx = pcl::getFieldIndex (*input_cloud , std::string ("x") );
int y_idx = pcl::getFieldIndex (*input_cloud, std::string ("y") );
int z_idx = pcl::getFieldIndex (*input_cloud, std::string ("z") );
int x_offset = input_cloud->fields[x_idx].offset;
int y_offset = input_cloud->fields[y_idx].offset;
int z_offset = input_cloud->fields[z_idx].offset;
for ( size_t point_idx =0; point_idx < input_cloud->data.size (); point_idx +=input_cloud->point_step )
{
PointT local_pt;
local_pt.x = * (reinterpret_cast<float*>(&input_cloud->data[point_idx + x_offset]));
local_pt.y = * (reinterpret_cast<float*>(&input_cloud->data[point_idx + y_offset]));
local_pt.z = * (reinterpret_cast<float*>(&input_cloud->data[point_idx + z_offset]));
if (!pcl_isfinite (local_pt.x) || !pcl_isfinite (local_pt.y) || !pcl_isfinite (local_pt.z))
continue;
if(!this->pointInBoundingBox (local_pt))
{
PCL_ERROR ("pcl::outofcore::OutofcoreOctreeBaseNode::%s] Point %2.lf %.2lf %.2lf not in bounding box", __FUNCTION__, local_pt.x, local_pt.y, local_pt.z);
}
assert (this->pointInBoundingBox (local_pt) == true);
//compute the box we are in
size_t box = 0;
box = ((local_pt.z >= mid_xyz[2]) << 2) | ((local_pt.y >= mid_xyz[1]) << 1) | ((local_pt.x >= mid_xyz[0]) << 0);
assert (box < 8);
//insert to the vector of indices
indices[box].push_back (static_cast<int> (point_idx/input_cloud->point_step));
}
}
////////////////////////////////////////////////////////////////////////////////
#if 0 //A bunch of non-class methods left from the Urban Robotics code that has been deactivated
template<typename ContainerT, typename PointT> OutofcoreOctreeBaseNode<ContainerT, PointT>*
makenode_norec (const boost::filesystem::path& path, OutofcoreOctreeBaseNode<ContainerT, PointT>* super)
{
OutofcoreOctreeBaseNode<ContainerT, PointT>* thisnode = new OutofcoreOctreeBaseNode<OutofcoreOctreeDiskContainer < PointT > , PointT > ();
//octree_disk_node ();
if (super == NULL)
{
thisnode->thisdir_ = path.parent_path ();
if (!boost::filesystem::exists (thisnode->thisdir_))
{
PCL_ERROR ( "[pcl::outofcore::OutofcoreOctreeBaseNode] could not find dir %s\n",thisnode->thisdir_.c_str () );
PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Octree Exception: Could not find directory");
}
thisnode->thisnodeindex_ = path;
thisnode->depth_ = 0;
thisnode->root_node_ = thisnode;
}
else
{
thisnode->thisdir_ = path;
thisnode->depth_ = super->depth_ + 1;
thisnode->root_node_ = super->root_node_;
if (thisnode->depth_ > thisnode->root->max_depth_)
{
thisnode->root->max_depth_ = thisnode->depth_;
}
boost::filesystem::directory_iterator diterend;
bool loaded = false;
for (boost::filesystem::directory_iterator diter (thisnode->thisdir_); diter != diterend; ++diter)
{
const boost::filesystem::path& file = *diter;
if (!boost::filesystem::is_directory (file))
{
if (boost::filesystem::extension (file) == OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_extension)
{
thisnode->thisnodeindex_ = file;
loaded = true;
break;
}
}
}
if (!loaded)
{
PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find index!\n");
PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find node metadata index file");
}
}
thisnode->max_depth_ = 0;
{
std::ifstream f (thisnode->thisnodeindex_.string ().c_str (), std::ios::in);
f >> thisnode->min_[0];
f >> thisnode->min_[1];
f >> thisnode->min_[2];
f >> thisnode->max_[0];
f >> thisnode->max_[1];
f >> thisnode->max_[2];
std::string filename;
f >> filename;
thisnode->thisnodestorage_ = thisnode->thisdir_ / filename;
f.close ();
thisnode->payload_ = boost::shared_ptr<ContainerT> (new ContainerT (thisnode->thisnodestorage_));
}
thisnode->parent_ = super;
children_.clear ();
children_.resize (8, static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>* > (0));
thisnode->num_children_ = 0;
return (thisnode);
}
////////////////////////////////////////////////////////////////////////////////
//accelerate search
template<typename ContainerT, typename PointT> void
queryBBIntersects_noload (const boost::filesystem::path& root_node, const Eigen::Vector3d& min, const Eigen::Vector3d& max, const boost::uint32_t query_depth, std::list<std::string>& bin_name)
{
OutofcoreOctreeBaseNode<ContainerT, PointT>* root = makenode_norec<ContainerT, PointT> (root_node, NULL);
if (root == NULL)
{
std::cout << "test";
}
if (root->intersectsWithBoundingBox (min, max))
{
if (query_depth == root->max_depth_)
{
if (!root->payload_->empty ())
{
bin_name.push_back (root->thisnodestorage_.string ());
}
return;
}
for (int i = 0; i < 8; i++)
{
boost::filesystem::path child_dir = root->thisdir_
/ boost::filesystem::path (boost::lexical_cast<std::string> (i));
if (boost::filesystem::exists (child_dir))
{
root->children_[i] = makenode_norec (child_dir, root);
root->num_children_++;
queryBBIntersects_noload (root->children_[i], min, max, root->max_depth_ - query_depth, bin_name);
}
}
}
delete root;
}
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
queryBBIntersects_noload (OutofcoreOctreeBaseNode<ContainerT, PointT>* current, const Eigen::Vector3d& min, const Eigen::Vector3d& max, const boost::uint32_t query_depth, std::list<std::string>& bin_name)
{
if (current->intersectsWithBoundingBox (min, max))
{
if (current->depth_ == query_depth)
{
if (!current->payload_->empty ())
{
bin_name.push_back (current->thisnodestorage_.string ());
}
}
else
{
for (int i = 0; i < 8; i++)
{
boost::filesystem::path child_dir = current->thisdir_ / boost::filesystem::path (boost::lexical_cast<std::string> (i));
if (boost::filesystem::exists (child_dir))
{
current->children_[i] = makenode_norec<ContainerT, PointT> (child_dir, current);
current->num_children_++;
queryBBIntersects_noload (current->children_[i], min, max, query_depth, bin_name);
}
}
}
}
}
#endif
////////////////////////////////////////////////////////////////////////////////
}//namespace outofcore
}//namespace pcl
//#define PCL_INSTANTIATE....
#endif //PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_H_
pcl::outofcore::OutofcoreOctreeBaseNode::num_children_
uint64_t num_children_
Number of children on disk.
Definition: octree_base_node.h:545
pcl::getPointsInBox
void getPointsInBox(const pcl::PointCloud< PointT > &cloud, Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, std::vector< int > &indices)
Get a set of points residing in a box given its bounds.
Definition: common.hpp:73
pcl::fromPCLPointCloud2
void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud<T> object using a field_map...
Definition: conversions.h:169
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::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::OutofcoreOctreeBaseNode::root_node_
OutofcoreOctreeBaseNode * root_node_
The root node of the tree we belong to.
Definition: octree_base_node.h:536
pcl::getFieldIndex
int getFieldIndex(const pcl::PCLPointCloud2 &cloud, const std::string &field_name)
Get the index of a specified field (i.e., dimension/channel)
Definition: io.h:59
pcl::outofcore::OutofcoreOctreeBaseNode::subdividePoints
void subdividePoints(const AlignedPointTVector &p, std::vector< AlignedPointTVector > &c, const bool skip_bb_check)
Subdivide points to pass to child nodes.
Definition: octree_base_node.hpp:688
pcl::outofcore::OutofcoreOctreeBaseNode::copyAllCurrentAndChildPointsRec
void copyAllCurrentAndChildPointsRec(std::list< PointT > &v)
Copies points from this and all children into a single point container (std::list) ...
Definition: octree_base_node.hpp:1782
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::outofcore::OutofcoreOctreeBaseNode::hasUnloadedChildren
bool hasUnloadedChildren() const
Returns whether or not a node has unloaded children data.
Definition: octree_base_node.hpp:301
pcl::IndicesPtr
boost::shared_ptr< std::vector< int > > IndicesPtr
Definition: pcl_base.h:60
pcl::RandomSample< pcl::PCLPointCloud2 >
RandomSample applies a random sampling with uniform probability.
Definition: random_sample.h:155
pcl
Definition: cloud_iterator.h:46
pcl::FilterIndices< pcl::PCLPointCloud2 >::filter
virtual void filter(PCLPointCloud2 &output)
Definition: filter_indices.h:228
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
std
Definition: multi_grid_octree_data.hpp:45
pcl::outofcore::OutofcoreOctreeBaseNode::makenode_norec
friend OutofcoreOctreeBaseNode< ContainerT, PointT > * makenode_norec(const boost::filesystem::path &path, OutofcoreOctreeBaseNode< ContainerT, PointT > *super)
Non-class function which creates a single child leaf; used with queryBBIntersects_noload to avoid loa...
pcl::FilterIndices< pcl::PCLPointCloud2 >::setNegative
void setNegative(bool negative)
Set whether the regular conditions for points filtering should apply, or the inverted conditions...
Definition: filter_indices.h:243
pcl::outofcore::OutofcoreOctreeDiskContainer::getRandomUUIDString
static void getRandomUUIDString(std::string &s)
Generate a universally unique identifier (UUID)
Definition: octree_disk_container.hpp:85
pcl::copyPointCloud
PCL_EXPORTS void copyPointCloud(const pcl::PCLPointCloud2 &cloud_in, const std::vector< int > &indices, pcl::PCLPointCloud2 &cloud_out)
Extract the indices of a given point cloud as a new point cloud.
pcl::outofcore::OutofcoreOctreeBaseNode::sample_percent_
static const double sample_percent_
Definition: octree_base_node.h:118
pcl::outofcore::OutofcoreOctreeBaseNode::children_
std::vector< OutofcoreOctreeBaseNode * > children_
The children of this node.
Definition: octree_base_node.h:542
pcl::outofcore::OutofcoreOctreeBaseNode::~OutofcoreOctreeBaseNode
virtual ~OutofcoreOctreeBaseNode()
Will recursively delete all children calling recFreeChildrein.
Definition: octree_base_node.hpp:259
pcl::outofcore::OutofcoreOctreeBaseNode::pointInBoundingBox
bool pointInBoundingBox(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const Eigen::Vector3d &point)
Tests whether point falls within the input bounding box.
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::OutofcoreOctreeBaseNode::node_index_basename
static const std::string node_index_basename
Definition: octree_base_node.h:114
pcl::outofcore::OutofcoreOctreeBaseNode::recFreeChildren
void recFreeChildren()
Method which recursively free children of this node.
Definition: octree_base_node.hpp:335
pcl::outofcore::OutofcoreOctreeBaseNode::OutofcoreOctreeBaseNode
OutofcoreOctreeBaseNode()
Empty constructor; sets pointers for children and for bounding boxes to 0.
Definition: octree_base_node.hpp:90
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::OutofcoreOctreeBaseNode::subdividePoint
void subdividePoint(const PointT &point, std::vector< AlignedPointTVector > &c)
Subdivide a single point into a specific child node.
Definition: octree_base_node.hpp:710
pcl::PCLPointCloud2::Ptr
boost::shared_ptr< ::pcl::PCLPointCloud2 > Ptr
Definition: PCLPointCloud2.h:51
pcl::PointCloud::height
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
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::OutofcoreOctreeBaseNode::rng_mutex_
static boost::mutex rng_mutex_
Random number generator mutex.
Definition: octree_base_node.h:561
pcl::outofcore::OutofcoreOctreeBaseNode::init_root_node
void init_root_node(const Eigen::Vector3d &bb_min, const Eigen::Vector3d &bb_max, OutofcoreOctreeBase< ContainerT, PointT > *const tree, const boost::filesystem::path &rootname)
Create root node and directory.
Definition: octree_base_node.hpp:202
pcl::PointCloud::Ptr
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
pcl::PointCloud::width
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
pcl::outofcore::OutofcoreOctreeBaseNode::countNumLoadedChildren
virtual size_t countNumLoadedChildren() const
Counts the number of loaded chilren by testing the children_ array; used to update num_loaded_chilren...
Definition: octree_base_node.hpp:1940
pcl::outofcore::OutofcoreOctreeBaseNode::queryBBIntersects_noload
friend void queryBBIntersects_noload(const boost::filesystem::path &rootnode, const Eigen::Vector3d &min, const Eigen::Vector3d &max, const boost::uint32_t query_depth, std::list< std::string > &bin_name)
Non-class method which performs a bounding box query without loading any of the point cloud data from...
pcl::outofcore::OutofcoreOctreeBaseNode::createChild
void createChild(const std::size_t idx)
Creates child node idx.
Definition: octree_base_node.hpp:878
pcl::outofcore::OutofcoreOctreeBaseNode::queryFrustum
void queryFrustum(const double planes[24], std::list< std::string > &file_names)
Definition: octree_base_node.hpp:1063
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::outofcore::OutofcoreOctreeBaseNode::randomSample
void randomSample(const AlignedPointTVector &p, AlignedPointTVector &insertBuff, const bool skip_bb_check)
Randomly sample point data.
Definition: octree_base_node.hpp:572
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::rand_gen_
static boost::mt19937 rand_gen_
Mersenne Twister: A 623-dimensionally equidistributed uniform pseudo-random number generator...
Definition: octree_base_node.h:565
pcl::outofcore::OutofcoreOctreeBaseNode::payload_
boost::shared_ptr< ContainerT > payload_
what holds the points.
Definition: octree_base_node.h:558
pcl::outofcore::OutofcoreOctreeBaseNode::getNumChildren
virtual size_t getNumChildren() const
Returns the total number of children on disk.
Definition: octree_base_node.h:279
pcl::outofcore::OutofcoreOctreeBaseNode::loadFromFile
void loadFromFile(const boost::filesystem::path &path, OutofcoreOctreeBaseNode *super)
Loads the nodes metadata from the JSON file.
Definition: octree_base_node.hpp:1956
pcl::outofcore::OutofcoreOctreeBaseNode::getDataSize
virtual boost::uint64_t getDataSize() const
Gets the number of points available in the PCD file.
Definition: octree_base_node.hpp:1932
pcl::outofcore::OutofcoreOctreeBaseNode::convertToXYZRecursive
void convertToXYZRecursive()
Recursively converts data files to ascii XZY files.
Definition: octree_base_node.hpp:1974
pcl::visualization::viewScreenArea
PCL_EXPORTS float viewScreenArea(const Eigen::Vector3d &eye, const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const Eigen::Matrix4d &view_projection_matrix, int width, int height)
pcl::outofcore::OutofcoreOctreeBaseNode::loadChildren
virtual void loadChildren(bool recursive)
Load nodes child data creating new nodes for each.
Definition: octree_base_node.hpp:311
pcl::outofcore::OutofcoreOctreeBaseNode::pcd_extension
static const std::string pcd_extension
Extension for this class to find the pcd files on disk.
Definition: octree_base_node.h:570
pcl::outofcore::OutofcoreOctreeBaseNode::sortOctantIndices
void sortOctantIndices(const pcl::PCLPointCloud2::Ptr &input_cloud, std::vector< std::vector< int > > &indices, const Eigen::Vector3d &mid_xyz)
Sorts the indices based on x,y,z fields and pushes the index into the proper octant&#39;s vector; This co...
Definition: octree_base_node.hpp:2007
pcl::outofcore::OutofcoreOctreeBaseNode::node_index_extension
static const std::string node_index_extension
Definition: octree_base_node.h:116
pcl::outofcore::OutofcoreOctreeBaseNode::intersectsWithBoundingBox
bool intersectsWithBoundingBox(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb) const
Tests whether the input bounding box intersects with the current node&#39;s bounding box.
Definition: octree_base_node.hpp:1831
pcl::outofcore::OutofcoreOctreeBaseNode::countNumChildren
virtual size_t countNumChildren() const
Counts the number of child directories on disk; used to update num_children_.
Definition: octree_base_node.hpp:268
pcl::outofcore::OutofcoreOctreeBaseNode::node_container_basename
static const std::string node_container_basename
Definition: octree_base_node.h:115
pcl::outofcore::OutofcoreOctreeBaseNode::AlignedPointTVector
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
Definition: octree_base_node.h:110
pcl::outofcore::OutofcoreOctreeBaseNode::saveIdx
void saveIdx(bool recursive)
Save node&#39;s metadata to file.
Definition: octree_base_node.hpp:284
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Definition: projection_matrix.h:53
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::OutofcoreOctreeBaseNode::parent_
OutofcoreOctreeBaseNode * parent_
super-node
Definition: octree_base_node.h:538
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::ExtractIndices< pcl::PCLPointCloud2 >
ExtractIndices extracts a set of indices from a point cloud.
Definition: extract_indices.h:161
pcl::concatenatePointCloud
PCL_EXPORTS bool concatenatePointCloud(const pcl::PCLPointCloud2 &cloud1, const pcl::PCLPointCloud2 &cloud2, pcl::PCLPointCloud2 &cloud_out)
Concatenate two pcl::PCLPointCloud2.
pcl::outofcore::OutofcoreOctreeBaseNode::copyAllCurrentAndChildPointsRec_sub
void copyAllCurrentAndChildPointsRec_sub(std::list< PointT > &v, const double percent)
Definition: octree_base_node.hpp:1806
pcl::PCLBase< pcl::PCLPointCloud2 >::setInputCloud
void setInputCloud(const PCLPointCloud2ConstPtr &cloud)
Provide a pointer to the input dataset.
pcl::outofcore::OutofcoreOctreeBaseNode::node_container_extension
static const std::string node_container_extension
Definition: octree_base_node.h:117
pcl::outofcore::OutofcoreOctreeBaseNode::addDataAtMaxDepth
boost::uint64_t addDataAtMaxDepth(const AlignedPointTVector &p, const bool skip_bb_check=true)
Add data to the leaf when at max depth of tree.
Definition: octree_base_node.hpp:625
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::read
virtual int read(pcl::PCLPointCloud2::Ptr &output_cloud)
Definition: octree_base_node.hpp:1916
pcl::outofcore::OutofcoreOctreeBaseNode::getDepth
virtual size_t getDepth() const
Definition: octree_base_node.h:272
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:594
pcl::outofcore::OutofcoreOctreeBaseNode::inBoundingBox
bool inBoundingBox(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb) const
Tests whether the input bounding box falls inclusively within this node&#39;s bounding box...
Definition: octree_base_node.hpp:1853
pcl::outofcore::OutofcoreOctreeBase
This code defines the octree used for point storage at Urban Robotics.
Definition: octree_base.h:149
pcl::RandomSample< pcl::PCLPointCloud2 >::setSample
void setSample(unsigned int sample)
Set number of indices to be sampled.
Definition: random_sample.h:179
pcl::outofcore::OutofcoreOctreeNodeMetadata
Encapsulated class to read JSON metadata into memory, and write the JSON metadata for each node...
Definition: outofcore_node_data.h:83
pcl::outofcore::OutofcoreOctreeBaseNode::num_loaded_children_
uint64_t num_loaded_children_
Number of loaded children this node has.
Definition: octree_base_node.h:553
pcl::outofcore::OutofcoreOctreeBaseNode::getNumLoadedChildren
virtual size_t getNumLoadedChildren() const
Count loaded chilren.
Definition: octree_base_node.h:287
pcl::outofcore::OutofcoreOctreeBaseNode::depth_
size_t depth_
Depth in the tree, root is 0, root&#39;s children are 1, ...
Definition: octree_base_node.h:540
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::OutofcoreOctreeBaseNode::node_metadata_
OutofcoreOctreeNodeMetadata::Ptr node_metadata_
Definition: octree_base_node.h:572