octree_iterator.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2011, Willow Garage, 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_OCTREE_ITERATOR_HPP_
#define PCL_OCTREE_ITERATOR_HPP_
namespace pcl
{
namespace octree
{
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename OctreeT>
OctreeDepthFirstIterator<OctreeT>::OctreeDepthFirstIterator (unsigned int max_depth_arg) :
OctreeIteratorBase<OctreeT> (max_depth_arg), stack_ ()
{
// initialize iterator
this->reset ();
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename OctreeT>
OctreeDepthFirstIterator<OctreeT>::OctreeDepthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg) :
OctreeIteratorBase<OctreeT> (octree_arg, max_depth_arg), stack_ ()
{
// initialize iterator
this->reset ();
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename OctreeT>
OctreeDepthFirstIterator<OctreeT>::~OctreeDepthFirstIterator ()
{
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename OctreeT>
void OctreeDepthFirstIterator<OctreeT>::reset ()
{
OctreeIteratorBase<OctreeT>::reset ();
if (this->octree_)
{
// allocate stack
stack_.reserve (this->max_octree_depth_);
// empty stack
stack_.clear ();
// pushing root node to stack
IteratorState stack_entry;
stack_entry.node_ = this->octree_->getRootNode ();
stack_entry.depth_ = 0;
stack_entry.key_.x = stack_entry.key_.y = stack_entry.key_.z = 0;
stack_.push_back(stack_entry);
this->current_state_ = &stack_.back();
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename OctreeT>
void OctreeDepthFirstIterator<OctreeT>::skipChildVoxels ()
{
if (stack_.size ())
{
// current depth
unsigned char current_depth = stack_.back ().depth_;
// pop from stack as long as we find stack elements with depth >= current depth
while (stack_.size () && (stack_.back ().depth_ >= current_depth))
stack_.pop_back ();
if (stack_.size ())
{
this->current_state_ = &stack_.back();
} else
{
this->current_state_ = 0;
}
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename OctreeT>
OctreeDepthFirstIterator<OctreeT>&
OctreeDepthFirstIterator<OctreeT>::operator++ ()
{
if (stack_.size ())
{
// get stack element
IteratorState stack_entry = stack_.back ();
stack_.pop_back ();
stack_entry.depth_ ++;
OctreeKey& current_key = stack_entry.key_;
if ( (this->max_octree_depth_>=stack_entry.depth_) &&
(stack_entry.node_->getNodeType () == BRANCH_NODE) )
{
unsigned char child_idx;
// current node is a branch node
BranchNode* current_branch =
static_cast<BranchNode*> (stack_entry.node_);
// add all children to stack
for (child_idx = 0; child_idx < 8; ++child_idx)
{
// if child exist
if (this->octree_->branchHasChild(*current_branch, child_idx))
{
// add child to stack
current_key.pushBranch (child_idx);
stack_entry.node_ = this->octree_->getBranchChildPtr(*current_branch, child_idx);
stack_.push_back(stack_entry);
current_key.popBranch();
}
}
}
if (stack_.size ())
{
this->current_state_ = &stack_.back();
} else
{
this->current_state_ = 0;
}
}
return (*this);
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename OctreeT>
OctreeBreadthFirstIterator<OctreeT>::OctreeBreadthFirstIterator (unsigned int max_depth_arg) :
OctreeIteratorBase<OctreeT> (max_depth_arg), FIFO_ ()
{
OctreeIteratorBase<OctreeT>::reset ();
// initialize iterator
this->reset ();
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename OctreeT>
OctreeBreadthFirstIterator<OctreeT>::OctreeBreadthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg) :
OctreeIteratorBase<OctreeT> (octree_arg, max_depth_arg), FIFO_ ()
{
OctreeIteratorBase<OctreeT>::reset ();
// initialize iterator
this->reset ();
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename OctreeT>
OctreeBreadthFirstIterator<OctreeT>::~OctreeBreadthFirstIterator ()
{
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename OctreeT>
void OctreeBreadthFirstIterator<OctreeT>::reset ()
{
OctreeIteratorBase<OctreeT>::reset ();
// init FIFO
FIFO_.clear ();
if (this->octree_)
{
// pushing root node to stack
IteratorState FIFO_entry;
FIFO_entry.node_ = this->octree_->getRootNode ();
FIFO_entry.depth_ = 0;
FIFO_entry.key_.x = FIFO_entry.key_.y = FIFO_entry.key_.z = 0;
FIFO_.push_back(FIFO_entry);
this->current_state_ = &FIFO_.front();
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename OctreeT>
OctreeBreadthFirstIterator<OctreeT>&
OctreeBreadthFirstIterator<OctreeT>::operator++ ()
{
if (FIFO_.size ())
{
// get stack element
IteratorState FIFO_entry = FIFO_.front ();
FIFO_.pop_front ();
FIFO_entry.depth_ ++;
OctreeKey& current_key = FIFO_entry.key_;
if ( (this->max_octree_depth_>=FIFO_entry.depth_) &&
(FIFO_entry.node_->getNodeType () == BRANCH_NODE) )
{
unsigned char child_idx;
// current node is a branch node
BranchNode* current_branch =
static_cast<BranchNode*> (FIFO_entry.node_);
// iterate over all children
for (child_idx = 0; child_idx < 8 ; ++child_idx)
{
// if child exist
if (this->octree_->branchHasChild(*current_branch, child_idx))
{
// add child to stack
current_key.pushBranch (static_cast<unsigned char> (child_idx));
FIFO_entry.node_ = this->octree_->getBranchChildPtr(*current_branch, child_idx);
FIFO_.push_back(FIFO_entry);
current_key.popBranch();
}
}
}
if (FIFO_.size ())
{
this->current_state_ = &FIFO_.front();
} else
{
this->current_state_ = 0;
}
}
return (*this);
}
}
}
#endif
pcl::octree::OctreeIteratorBase::reset
void reset()
Reset iterator.
Definition: octree_iterator.h:154
pcl::octree::BRANCH_NODE
Definition: octree_nodes.h:60
pcl::octree::IteratorState::depth_
unsigned char depth_
Definition: octree_iterator.h:65
pcl::octree::OctreeKey::x
uint32_t x
Definition: octree_key.h:142
pcl
Definition: convolution.h:45
pcl::octree::OctreeNode::getNodeType
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)
pcl::octree::OctreeBreadthFirstIterator::FIFO_
std::deque< IteratorState > FIFO_
FIFO list.
Definition: octree_iterator.h:523
pcl::octree::OctreeDepthFirstIterator::stack_
std::vector< IteratorState > stack_
Stack structure.
Definition: octree_iterator.h:442
pcl::octree::OctreeIteratorBase::current_state_
IteratorState * current_state_
Pointer to current iterator state.
Definition: octree_iterator.h:353
pcl::octree::OctreeIteratorBase::BranchNode
OctreeT::BranchNode BranchNode
Definition: octree_iterator.h:81
pcl::octree::OctreeKey::popBranch
void popBranch()
pop child node from octree key
Definition: octree_key.h:114
pcl::octree::OctreeBreadthFirstIterator
Octree iterator class
Definition: octree_iterator.h:452
pcl::octree::OctreeIteratorBase::max_octree_depth_
unsigned int max_octree_depth_
Maximum octree depth.
Definition: octree_iterator.h:356
pcl::octree::OctreeBreadthFirstIterator::reset
void reset()
Reset the iterator to the root node of the octree.
Definition: octree_iterator.hpp:208
pcl::octree::IteratorState
Definition: octree_iterator.h:62
pcl::octree::OctreeDepthFirstIterator::reset
virtual void reset()
Reset the iterator to the root node of the octree.
Definition: octree_iterator.hpp:72
pcl::octree::OctreeDepthFirstIterator::OctreeDepthFirstIterator
OctreeDepthFirstIterator(unsigned int max_depth_arg=0)
Empty constructor.
Definition: octree_iterator.hpp:48
pcl::octree::OctreeDepthFirstIterator::skipChildVoxels
void skipChildVoxels()
Skip all child voxels of current node and return to parent node.
Definition: octree_iterator.hpp:99
pcl::octree::OctreeBreadthFirstIterator::~OctreeBreadthFirstIterator
virtual ~OctreeBreadthFirstIterator()
Empty deconstructor.
Definition: octree_iterator.hpp:202
pcl::octree::OctreeDepthFirstIterator
Octree iterator class
Definition: octree_iterator.h:366
pcl::octree::OctreeKey
Octree key class
Definition: octree_key.h:51
pcl::octree::IteratorState::key_
OctreeKey key_
Definition: octree_iterator.h:64
pcl::octree::OctreeBreadthFirstIterator::operator++
OctreeBreadthFirstIterator & operator++()
Preincrement operator.
Definition: octree_iterator.hpp:232
pcl::octree::OctreeIteratorBase::octree_
OctreeT * octree_
Reference to octree class.
Definition: octree_iterator.h:350
pcl::octree::OctreeDepthFirstIterator::operator++
OctreeDepthFirstIterator & operator++()
Preincrement operator.
Definition: octree_iterator.hpp:125
pcl::octree::OctreeIteratorBase
Abstract octree iterator class
Definition: octree_iterator.h:75
pcl::octree::OctreeKey::y
uint32_t y
Definition: octree_key.h:143
pcl::octree::OctreeKey::pushBranch
void pushBranch(unsigned char childIndex)
push a child node to the octree key
Definition: octree_key.h:104
pcl::octree::OctreeDepthFirstIterator::~OctreeDepthFirstIterator
virtual ~OctreeDepthFirstIterator()
Empty deconstructor.
Definition: octree_iterator.hpp:66
pcl::octree::IteratorState::node_
OctreeNode * node_
Definition: octree_iterator.h:63
pcl::octree::OctreeKey::z
uint32_t z
Definition: octree_key.h:144
pcl::octree::OctreeBreadthFirstIterator::OctreeBreadthFirstIterator
OctreeBreadthFirstIterator(unsigned int max_depth_arg=0)
Empty constructor.
Definition: octree_iterator.hpp:180