Loading...
Searching...
No Matches
simple_octree.hpp
/*
* simple_octree.hpp
*
* Created on: Mar 12, 2013
* Author: papazov
*/
#ifndef SIMPLE_OCTREE_HPP_
#define SIMPLE_OCTREE_HPP_
#include <cmath>
namespace pcl
{
namespace recognition
{
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::Node ()
: data_ (nullptr),
parent_ (nullptr),
children_(nullptr)
{}
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::~Node ()
{
this->deleteChildren ();
this->deleteData ();
}
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::setCenter (const Scalar *c)
{
center_[0] = c[0];
center_[1] = c[1];
center_[2] = c[2];
}
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::setBounds (const Scalar *b)
{
bounds_[0] = b[0];
bounds_[1] = b[1];
bounds_[2] = b[2];
bounds_[3] = b[3];
bounds_[4] = b[4];
bounds_[5] = b[5];
}
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::computeRadius ()
{
Scalar v[3] = {static_cast<Scalar> (0.5)*(bounds_[1]-bounds_[0]),
static_cast<Scalar> (0.5)*(bounds_[3]-bounds_[2]),
static_cast<Scalar> (0.5)*(bounds_[5]-bounds_[4])};
radius_ = static_cast<Scalar> (std::sqrt (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]));
}
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline bool
SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::createChildren ()
{
if ( children_ )
return (false);
Scalar bounds[6], center[3], childside = static_cast<Scalar> (0.5)*(bounds_[1]-bounds_[0]);
children_ = new Node[8];
// Compute bounds and center for child 0, i.e., for (0,0,0)
bounds[0] = bounds_[0]; bounds[1] = center_[0];
bounds[2] = bounds_[2]; bounds[3] = center_[1];
bounds[4] = bounds_[4]; bounds[5] = center_[2];
// Compute the center of the new child
center[0] = 0.5f*(bounds[0] + bounds[1]);
center[1] = 0.5f*(bounds[2] + bounds[3]);
center[2] = 0.5f*(bounds[4] + bounds[5]);
// Save the results
children_[0].setBounds(bounds);
children_[0].setCenter(center);
// Compute bounds and center for child 1, i.e., for (0,0,1)
bounds[4] = center_[2]; bounds[5] = bounds_[5];
// Update the center
center[2] += childside;
// Save the results
children_[1].setBounds(bounds);
children_[1].setCenter(center);
// Compute bounds and center for child 3, i.e., for (0,1,1)
bounds[2] = center_[1]; bounds[3] = bounds_[3];
// Update the center
center[1] += childside;
// Save the results
children_[3].setBounds(bounds);
children_[3].setCenter(center);
// Compute bounds and center for child 2, i.e., for (0,1,0)
bounds[4] = bounds_[4]; bounds[5] = center_[2];
// Update the center
center[2] -= childside;
// Save the results
children_[2].setBounds(bounds);
children_[2].setCenter(center);
// Compute bounds and center for child 6, i.e., for (1,1,0)
bounds[0] = center_[0]; bounds[1] = bounds_[1];
// Update the center
center[0] += childside;
// Save the results
children_[6].setBounds(bounds);
children_[6].setCenter(center);
// Compute bounds and center for child 7, i.e., for (1,1,1)
bounds[4] = center_[2]; bounds[5] = bounds_[5];
// Update the center
center[2] += childside;
// Save the results
children_[7].setBounds(bounds);
children_[7].setCenter(center);
// Compute bounds and center for child 5, i.e., for (1,0,1)
bounds[2] = bounds_[2]; bounds[3] = center_[1];
// Update the center
center[1] -= childside;
// Save the results
children_[5].setBounds(bounds);
children_[5].setCenter(center);
// Compute bounds and center for child 4, i.e., for (1,0,0)
bounds[4] = bounds_[4]; bounds[5] = center_[2];
// Update the center
center[2] -= childside;
// Save the results
children_[4].setBounds(bounds);
children_[4].setCenter(center);
for ( int i = 0 ; i < 8 ; ++i )
{
children_[i].computeRadius();
children_[i].setParent(this);
}
return (true);
}
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::deleteChildren ()
{
delete[] children_;
children_ = nullptr;
}
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::deleteData ()
{
delete data_;
data_ = nullptr;
}
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::makeNeighbors (Node* node)
{
if ( !this->hasData () || !node->hasData () )
return;
this->full_leaf_neighbors_.insert (node);
node->full_leaf_neighbors_.insert (this);
}
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
SimpleOctree<NodeData, NodeDataCreator, Scalar>::SimpleOctree () = default;
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
SimpleOctree<NodeData, NodeDataCreator, Scalar>::~SimpleOctree ()
{
this->clear ();
}
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
SimpleOctree<NodeData, NodeDataCreator, Scalar>::clear ()
{
delete root_;
root_ = nullptr;
full_leaves_.clear();
}
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
SimpleOctree<NodeData, NodeDataCreator, Scalar>::build (const Scalar* bounds, Scalar voxel_size,
NodeDataCreator* node_data_creator)
{
if ( voxel_size <= 0 )
return;
this->clear();
voxel_size_ = voxel_size;
node_data_creator_ = node_data_creator;
Scalar extent = std::max (std::max (bounds[1]-bounds[0], bounds[3]-bounds[2]), bounds[5]-bounds[4]);
Scalar center[3] = {static_cast<Scalar> (0.5)*(bounds[0]+bounds[1]),
static_cast<Scalar> (0.5)*(bounds[2]+bounds[3]),
static_cast<Scalar> (0.5)*(bounds[4]+bounds[5])};
Scalar arg = extent/voxel_size;
// Compute the number of tree levels
if ( arg > 1 )
tree_levels_ = static_cast<int> (std::ceil (std::log (arg)/std::log (2.0)) + 0.5);
else
tree_levels_ = 0;
// Compute the number of octree levels and the bounds of the root
Scalar half_root_side = static_cast<Scalar> (0.5f*pow (2.0, tree_levels_)*voxel_size);
// Determine the bounding box of the octree
bounds_[0] = center[0] - half_root_side;
bounds_[1] = center[0] + half_root_side;
bounds_[2] = center[1] - half_root_side;
bounds_[3] = center[1] + half_root_side;
bounds_[4] = center[2] - half_root_side;
bounds_[5] = center[2] + half_root_side;
// Create and initialize the root
root_ = new Node ();
root_->setCenter (center);
root_->setBounds (bounds_);
root_->setParent (nullptr);
root_->computeRadius ();
}
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
typename SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node*
SimpleOctree<NodeData, NodeDataCreator, Scalar>::createLeaf (Scalar x, Scalar y, Scalar z)
{
// Make sure that the input point is within the octree bounds
if ( x < bounds_[0] || x > bounds_[1] ||
y < bounds_[2] || y > bounds_[3] ||
z < bounds_[4] || z > bounds_[5] )
{
return (nullptr);
}
Node* node = root_;
// Go down to the right leaf
for ( int l = 0 ; l < tree_levels_ ; ++l )
{
node->createChildren ();
const Scalar *c = node->getCenter ();
int id = 0;
if ( x >= c[0] ) id |= 4;
if ( y >= c[1] ) id |= 2;
if ( z >= c[2] ) id |= 1;
node = node->getChild (id);
}
if ( !node->hasData () )
{
node->setData (node_data_creator_->create (node));
this->insertNeighbors (node);
full_leaves_.push_back (node);
}
return (node);
}
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
typename SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node*
SimpleOctree<NodeData, NodeDataCreator, Scalar>::getFullLeaf (int i, int j, int k)
{
Scalar offset = 0.5f*voxel_size_;
Scalar p[3] = {bounds_[0] + offset + static_cast<Scalar> (i)*voxel_size_,
bounds_[2] + offset + static_cast<Scalar> (j)*voxel_size_,
bounds_[4] + offset + static_cast<Scalar> (k)*voxel_size_};
return (this->getFullLeaf (p[0], p[1], p[2]));
}
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
typename SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node*
SimpleOctree<NodeData, NodeDataCreator, Scalar>::getFullLeaf (Scalar x, Scalar y, Scalar z)
{
// Make sure that the input point is within the octree bounds
if ( x < bounds_[0] || x > bounds_[1] ||
y < bounds_[2] || y > bounds_[3] ||
z < bounds_[4] || z > bounds_[5] )
{
return (nullptr);
}
Node* node = root_;
// Go down to the right leaf
for ( int l = 0 ; l < tree_levels_ ; ++l )
{
if ( !node->hasChildren () )
return (nullptr);
const Scalar *c = node->getCenter ();
int id = 0;
if ( x >= c[0] ) id |= 4;
if ( y >= c[1] ) id |= 2;
if ( z >= c[2] ) id |= 1;
node = node->getChild (id);
}
if ( !node->hasData () )
return (nullptr);
return (node);
}
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
SimpleOctree<NodeData, NodeDataCreator, Scalar>::insertNeighbors (Node* node)
{
const Scalar* c = node->getCenter ();
Scalar s = static_cast<Scalar> (0.5)*voxel_size_;
Node *neigh;
neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
neigh = this->getFullLeaf (c[0]+s, c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
neigh = this->getFullLeaf (c[0]+s, c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh);
neigh = this->getFullLeaf (c[0]+s, c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
neigh = this->getFullLeaf (c[0] , c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
neigh = this->getFullLeaf (c[0] , c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
neigh = this->getFullLeaf (c[0] , c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
neigh = this->getFullLeaf (c[0] , c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
//neigh = this->getFullLeaf (c[0] , c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh);
neigh = this->getFullLeaf (c[0] , c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
neigh = this->getFullLeaf (c[0] , c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
neigh = this->getFullLeaf (c[0] , c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
neigh = this->getFullLeaf (c[0] , c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
neigh = this->getFullLeaf (c[0]-s, c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
neigh = this->getFullLeaf (c[0]-s, c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh);
neigh = this->getFullLeaf (c[0]-s, c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
}
} // namespace recognition
} // namespace pcl
#endif /* SIMPLE_OCTREE_HPP_ */
pcl::recognition::SimpleOctree::Node
Definition simple_octree.h:62
pcl::recognition::SimpleOctree< Hypothesis, HypothesisCreator, float >::Node::radius_
float radius_
Definition simple_octree.h:144
pcl::recognition::SimpleOctree::Node::full_leaf_neighbors_
std::set< Node * > full_leaf_neighbors_
Definition simple_octree.h:145
pcl::recognition::SimpleOctree::Node::center_
Scalar center_[3]
Definition simple_octree.h:142
pcl::recognition::SimpleOctree::Node::setBounds
void setBounds(const Scalar *b)
Definition simple_octree.hpp:46
pcl::recognition::SimpleOctree::Node::makeNeighbors
void makeNeighbors(Node *node)
Make this and 'node' neighbors by inserting each node in the others node neighbor set.
Definition simple_octree.hpp:172
pcl::recognition::SimpleOctree::Node::getCenter
const Scalar * getCenter() const
Definition simple_octree.h:75
pcl::recognition::SimpleOctree::Node::parent_
Node * parent_
Definition simple_octree.h:143
pcl::recognition::SimpleOctree::Node::~Node
virtual ~Node()
Definition simple_octree.hpp:29
pcl::recognition::SimpleOctree::Node::Node
Node()
Definition simple_octree.hpp:21
pcl::recognition::SimpleOctree::Node::computeRadius
void computeRadius()
Computes the "radius" of the node which is half the diagonal length.
Definition simple_octree.hpp:58
pcl::recognition::SimpleOctree::Node::children_
Node * children_
Definition simple_octree.h:143
pcl::recognition::SimpleOctree::Node::setData
void setData(const NodeData &src)
Definition simple_octree.h:90
pcl::recognition::SimpleOctree::Node::setCenter
void setCenter(const Scalar *c)
Definition simple_octree.hpp:37
pcl::recognition::SimpleOctree::Node::hasChildren
bool hasChildren()
Definition simple_octree.h:108
pcl::recognition::SimpleOctree::Node::hasData
bool hasData()
Definition simple_octree.h:105
pcl::recognition::SimpleOctree::Node::getChild
Node * getChild(int id)
Definition simple_octree.h:84
pcl::recognition::SimpleOctree::Node::deleteData
void deleteData()
Definition simple_octree.hpp:164
pcl::recognition::SimpleOctree::Node::data_
NodeData * data_
Definition simple_octree.h:141
pcl::recognition::SimpleOctree::Node::deleteChildren
void deleteChildren()
Definition simple_octree.hpp:156
pcl::recognition::SimpleOctree::Node::createChildren
bool createChildren()
Definition simple_octree.hpp:69
pcl::recognition::SimpleOctree::Node::bounds_
Scalar bounds_[6]
Definition simple_octree.h:142
pcl::recognition::SimpleOctree::root_
Node * root_
Definition simple_octree.h:203
pcl::recognition::SimpleOctree::insertNeighbors
void insertNeighbors(Node *node)
Definition simple_octree.hpp:338
pcl::recognition::SimpleOctree::bounds_
Scalar bounds_[6]
Definition simple_octree.h:201
pcl::recognition::SimpleOctree::node_data_creator_
NodeDataCreator * node_data_creator_
Definition simple_octree.h:205
pcl::recognition::SimpleOctree::createLeaf
Node * createLeaf(Scalar x, Scalar y, Scalar z)
Creates the leaf containing p = (x, y, z) and returns a pointer to it, however, only if p lies within...
Definition simple_octree.hpp:250
pcl::recognition::SimpleOctree::build
void build(const Scalar *bounds, Scalar voxel_size, NodeDataCreator *node_data_creator)
Creates an empty octree with bounds at least as large as the ones provided as input and with leaf siz...
Definition simple_octree.hpp:204
pcl::recognition::SimpleOctree::clear
void clear()
Definition simple_octree.hpp:194
pcl::recognition::SimpleOctree::full_leaves_
std::vector< Node * > full_leaves_
Definition simple_octree.h:204
pcl::recognition::SimpleOctree::~SimpleOctree
virtual ~SimpleOctree()
Definition simple_octree.hpp:187
pcl::recognition::SimpleOctree::voxel_size_
Scalar voxel_size_
Definition simple_octree.h:201
pcl::recognition::SimpleOctree::tree_levels_
int tree_levels_
Definition simple_octree.h:202
pcl::recognition::SimpleOctree::SimpleOctree
SimpleOctree()
pcl::recognition::SimpleOctree::getFullLeaf
Node * getFullLeaf(int i, int j, int k)
Since the leaves are aligned in a rectilinear grid, each leaf has a unique id.
Definition simple_octree.hpp:289
pcl::recognition
Definition hough_3d.h:52
pcl
Definition convolution.h:46