Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/octree/include/pcl/octree/octree_iterator.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2012, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id$
00037  */
00038 
00039 #ifndef PCL_OCTREE_ITERATOR_H
00040 #define PCL_OCTREE_ITERATOR_H
00041 
00042 #include <cstddef>
00043 #include <vector>
00044 #include <deque>
00045 
00046 #include "octree_nodes.h"
00047 #include "octree_key.h"
00048 
00049 #include <pcl/point_cloud.h>
00050 #include <pcl/point_types.h>
00051 
00052 #include <iterator>
00053 
00054 // Ignore warnings in the above headers
00055 #ifdef __GNUC__
00056 #pragma GCC system_header 
00057 #endif
00058 
00059 namespace pcl
00060 {
00061   namespace octree
00062   {
00063 
00064     // Octree iterator state pushed on stack/list
00065     struct IteratorState{
00066         OctreeNode* node_;
00067         OctreeKey key_;
00068         unsigned char depth_;
00069     };
00070 
00071 
00072     /** \brief @b Abstract octree iterator class
00073      * \note Octree iterator base class
00074      * \ingroup octree
00075      * \author Julius Kammerl (julius@kammerl.de)
00076      */
00077     template<typename OctreeT>
00078       class OctreeIteratorBase : public std::iterator<std::forward_iterator_tag, const OctreeNode, void,
00079           const OctreeNode*, const OctreeNode&>
00080       {
00081       public:
00082 
00083         typedef typename OctreeT::LeafNode LeafNode;
00084         typedef typename OctreeT::BranchNode BranchNode;
00085 
00086         typedef typename OctreeT::LeafContainer LeafContainer;
00087         typedef typename OctreeT::BranchContainer BranchContainer;
00088 
00089         /** \brief Empty constructor.
00090          */
00091         explicit
00092         OctreeIteratorBase (unsigned int max_depth_arg = 0) :
00093             octree_ (0), current_state_(0), max_octree_depth_(max_depth_arg)
00094         {
00095           this->reset ();
00096         }
00097 
00098         /** \brief Constructor.
00099          * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node.
00100          * \param[in] max_depth_arg Depth limitation during traversal
00101          */
00102         explicit
00103         OctreeIteratorBase (OctreeT* octree_arg, unsigned int max_depth_arg = 0) :
00104             octree_ (octree_arg), current_state_(0), max_octree_depth_(max_depth_arg)
00105         {
00106           this->reset ();
00107         }
00108 
00109         /** \brief Copy constructor.
00110          * \param[in] src the iterator to copy into this
00111          * \param[in] max_depth_arg Depth limitation during traversal
00112          */
00113         OctreeIteratorBase (const OctreeIteratorBase& src, unsigned int max_depth_arg = 0) :
00114             octree_ (src.octree_), current_state_(0), max_octree_depth_(max_depth_arg)
00115         {
00116           this->reset ();
00117         }
00118 
00119         /** \brief Copy operator.
00120          * \param[in] src the iterator to copy into this
00121          */
00122         inline OctreeIteratorBase&
00123         operator = (const OctreeIteratorBase& src)
00124         {
00125           octree_ = src.octree_;
00126           current_state_ = src.current_state_;
00127           max_octree_depth_ = src.max_octree_depth_;
00128           return (*this);
00129         }
00130 
00131         /** \brief Empty deconstructor. */
00132         virtual
00133         ~OctreeIteratorBase ()
00134         {
00135         }
00136 
00137         /** \brief Equal comparison operator
00138          * \param[in] OctreeIteratorBase to compare with
00139          */
00140         bool operator==(const OctreeIteratorBase& other) const
00141         {
00142           return (( octree_ ==other.octree_) &&
00143                   ( current_state_ == other.current_state_) &&
00144                   ( max_octree_depth_ == other.max_octree_depth_) );
00145         }
00146 
00147         /** \brief Inequal comparison operator
00148          * \param[in] OctreeIteratorBase to compare with
00149          */
00150         bool operator!=(const OctreeIteratorBase& other) const
00151         {
00152           return (( octree_ !=other.octree_) &&
00153                   ( current_state_ != other.current_state_) &&
00154                   ( max_octree_depth_ != other.max_octree_depth_) );
00155         }
00156 
00157         /** \brief Reset iterator */
00158         inline void reset ()
00159         {
00160           current_state_ = 0;
00161           if (octree_ && (!max_octree_depth_))
00162           {
00163             max_octree_depth_ = octree_->getTreeDepth();
00164           }
00165         }
00166 
00167         /** \brief Get octree key for the current iterator octree node
00168          * \return octree key of current node
00169          */
00170         inline const OctreeKey&
00171         getCurrentOctreeKey () const
00172         {
00173           assert(octree_!=0);
00174           assert(current_state_!=0);
00175 
00176           return (current_state_->key_);
00177         }
00178 
00179         /** \brief Get the current depth level of octree
00180          * \return depth level
00181          */
00182         inline unsigned int
00183         getCurrentOctreeDepth () const
00184         {
00185           assert(octree_!=0);
00186           assert(current_state_!=0);
00187 
00188           return (current_state_->depth_);
00189         }
00190 
00191         /** \brief Get the current octree node
00192          * \return pointer to current octree node
00193          */
00194         inline OctreeNode*
00195         getCurrentOctreeNode () const
00196         {
00197           assert(octree_!=0);
00198           assert(current_state_!=0);
00199 
00200           return (current_state_->node_);
00201         }
00202 
00203 
00204         /** \brief check if current node is a branch node
00205          * \return true if current node is a branch node, false otherwise
00206          */
00207         inline bool
00208         isBranchNode () const
00209         {
00210           assert(octree_!=0);
00211           assert(current_state_!=0);
00212 
00213           return (current_state_->node_->getNodeType () == BRANCH_NODE);
00214         }
00215 
00216         /** \brief check if current node is a branch node
00217          * \return true if current node is a branch node, false otherwise
00218          */
00219         inline bool
00220         isLeafNode () const
00221         {
00222           assert(octree_!=0);
00223           assert(current_state_!=0);
00224 
00225           return (current_state_->node_->getNodeType () == LEAF_NODE);
00226         }
00227 
00228         /** \brief *operator.
00229          * \return pointer to the current octree node
00230          */
00231         inline OctreeNode*
00232         operator* () const
00233         { // return designated object
00234           if (octree_ && current_state_)
00235           {
00236             return (current_state_->node_);
00237           } else
00238           {
00239             return 0;
00240           }
00241         }
00242 
00243         /** \brief Get bit pattern of children configuration of current node
00244          * \return bit pattern (byte) describing the existence of 8 children of the current node
00245          */
00246         inline char
00247         getNodeConfiguration () const
00248         {
00249           char ret = 0;
00250 
00251           assert(octree_!=0);
00252           assert(current_state_!=0);
00253 
00254           if (isBranchNode ())
00255           {
00256 
00257             // current node is a branch node
00258             const BranchNode* current_branch = static_cast<const BranchNode*> (current_state_->node_);
00259 
00260             // get child configuration bit pattern
00261             ret = octree_->getBranchBitPattern (*current_branch);
00262 
00263           }
00264 
00265           return (ret);
00266         }
00267 
00268         /** \brief Method for retrieving a single leaf container from the octree leaf node
00269          * \return Reference to container class of leaf node.
00270          */
00271         const LeafContainer&
00272         getLeafContainer () const
00273         {
00274           assert(octree_!=0);
00275           assert(current_state_!=0);
00276           assert(this->isLeafNode());
00277 
00278           LeafNode* leaf_node = static_cast<LeafNode*>(current_state_->node_);
00279 
00280           return leaf_node->getContainer();
00281         }
00282 
00283         /** \brief Method for retrieving a single leaf container from the octree leaf node
00284          * \return Reference to container class of leaf node.
00285          */
00286         LeafContainer&
00287         getLeafContainer ()
00288         {
00289           assert(octree_!=0);
00290           assert(current_state_!=0);
00291           assert(this->isLeafNode());
00292 
00293           LeafNode* leaf_node = static_cast<LeafNode*>(current_state_->node_);
00294 
00295           return leaf_node->getContainer();
00296         }
00297 
00298         /** \brief Method for retrieving the container from an octree branch node
00299          * \return BranchContainer.
00300          */
00301         const BranchContainer&
00302         getBranchContainer () const
00303         {
00304           assert(octree_!=0);
00305           assert(current_state_!=0);
00306           assert(this->isBranchNode());
00307 
00308           BranchNode* branch_node = static_cast<BranchNode*>(current_state_->node_);
00309 
00310           return branch_node->getContainer();
00311         }
00312 
00313         /** \brief Method for retrieving the container from an octree branch node
00314          * \return BranchContainer.
00315          */
00316         BranchContainer&
00317         getBranchContainer ()
00318         {
00319           assert(octree_!=0);
00320           assert(current_state_!=0);
00321           assert(this->isBranchNode());
00322 
00323           BranchNode* branch_node = static_cast<BranchNode*>(current_state_->node_);
00324 
00325           return branch_node->getContainer();
00326         }
00327 
00328         /** \brief get a integer identifier for current node (note: identifier depends on tree depth).
00329          * \return node id.
00330          */
00331         virtual unsigned long
00332         getNodeID () const
00333         {
00334           unsigned long id = 0;
00335 
00336           assert(octree_!=0);
00337           assert(current_state_!=0);
00338 
00339           if (current_state_)
00340           {
00341             const OctreeKey& key = getCurrentOctreeKey();
00342             // calculate integer id with respect to octree key
00343             unsigned int depth = octree_->getTreeDepth ();
00344             id = key.x << (depth * 2) | key.y << (depth * 1) | key.z << (depth * 0);
00345           }
00346 
00347           return id;
00348         }
00349 
00350       protected:
00351         /** \brief Reference to octree class. */
00352         OctreeT* octree_;
00353 
00354         /** \brief Pointer to current iterator state. */
00355         IteratorState* current_state_;
00356 
00357         /** \brief Maximum octree depth */
00358         unsigned int max_octree_depth_;
00359       };
00360 
00361     //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00362     /** \brief @b Octree iterator class
00363      * \note This class implements a forward iterator for traversing octrees in a depth-first manner.
00364      * \ingroup octree
00365      * \author Julius Kammerl (julius@kammerl.de)
00366      */
00367     template<typename OctreeT>
00368       class OctreeDepthFirstIterator : public OctreeIteratorBase<OctreeT>
00369       {
00370 
00371       public:
00372 
00373         typedef typename OctreeIteratorBase<OctreeT>::LeafNode LeafNode;
00374         typedef typename OctreeIteratorBase<OctreeT>::BranchNode BranchNode;
00375 
00376         /** \brief Empty constructor.
00377          * \param[in] max_depth_arg Depth limitation during traversal
00378          */
00379         explicit
00380         OctreeDepthFirstIterator (unsigned int max_depth_arg = 0);
00381 
00382         /** \brief Constructor.
00383          * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node.
00384          * \param[in] max_depth_arg Depth limitation during traversal
00385          */
00386         explicit
00387         OctreeDepthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg = 0);
00388 
00389         /** \brief Empty deconstructor. */
00390         virtual
00391         ~OctreeDepthFirstIterator ();
00392 
00393         /** \brief Copy operator.
00394          * \param[in] src the iterator to copy into this
00395          */
00396         inline OctreeDepthFirstIterator&
00397         operator = (const OctreeDepthFirstIterator& src)
00398         {
00399 
00400           OctreeIteratorBase<OctreeT>::operator=(src);
00401 
00402           stack_ = src.stack_;
00403 
00404           if (stack_.size())
00405           {
00406             this->current_state_ = &stack_.back();
00407           } else
00408           {
00409             this->current_state_ = 0;
00410           }
00411 
00412           return (*this);
00413         }
00414 
00415         /** \brief Reset the iterator to the root node of the octree
00416          */
00417         virtual void
00418         reset ();
00419 
00420         /** \brief Preincrement operator.
00421          * \note recursively step to next octree node
00422          */
00423         OctreeDepthFirstIterator&
00424         operator++ ();
00425 
00426         /** \brief postincrement operator.
00427          * \note recursively step to next octree node
00428          */
00429         inline OctreeDepthFirstIterator
00430         operator++ (int)
00431         {
00432           OctreeDepthFirstIterator _Tmp = *this;
00433           ++*this;
00434           return (_Tmp);
00435         }
00436 
00437         /** \brief Skip all child voxels of current node and return to parent node.
00438          */
00439         void
00440         skipChildVoxels ();
00441 
00442       protected:
00443         /** Stack structure. */
00444         std::vector<IteratorState> stack_;
00445       };
00446 
00447     //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00448     /** \brief @b Octree iterator class
00449      * \note This class implements a forward iterator for traversing octrees in a breadth-first manner.
00450      * \ingroup octree
00451      * \author Julius Kammerl (julius@kammerl.de)
00452      */
00453     template<typename OctreeT>
00454       class OctreeBreadthFirstIterator : public OctreeIteratorBase<OctreeT>
00455       {
00456         // public typedefs
00457         typedef typename OctreeIteratorBase<OctreeT>::BranchNode BranchNode;
00458         typedef typename OctreeIteratorBase<OctreeT>::LeafNode LeafNode;
00459 
00460 
00461       public:
00462         /** \brief Empty constructor.
00463          * \param[in] max_depth_arg Depth limitation during traversal
00464          */
00465         explicit
00466         OctreeBreadthFirstIterator (unsigned int max_depth_arg = 0);
00467 
00468         /** \brief Constructor.
00469          * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node.
00470          * \param[in] max_depth_arg Depth limitation during traversal
00471          */
00472         explicit
00473         OctreeBreadthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg = 0);
00474 
00475         /** \brief Empty deconstructor. */
00476         virtual
00477         ~OctreeBreadthFirstIterator ();
00478 
00479         /** \brief Copy operator.
00480          * \param[in] src the iterator to copy into this
00481          */
00482         inline OctreeBreadthFirstIterator&
00483         operator = (const OctreeBreadthFirstIterator& src)
00484         {
00485 
00486           OctreeIteratorBase<OctreeT>::operator=(src);
00487 
00488           FIFO_ = src.FIFO_;
00489 
00490           if (FIFO_.size())
00491           {
00492             this->current_state_ = &FIFO_.front();
00493           } else
00494           {
00495             this->current_state_ = 0;
00496           }
00497 
00498           return (*this);
00499         }
00500 
00501         /** \brief Reset the iterator to the root node of the octree
00502          */
00503         void
00504         reset ();
00505 
00506         /** \brief Preincrement operator.
00507          * \note step to next octree node
00508          */
00509         OctreeBreadthFirstIterator&
00510         operator++ ();
00511 
00512         /** \brief postincrement operator.
00513          * \note step to next octree node
00514          */
00515         inline OctreeBreadthFirstIterator
00516         operator++ (int)
00517         {
00518           OctreeBreadthFirstIterator _Tmp = *this;
00519           ++*this;
00520           return (_Tmp);
00521         }
00522 
00523       protected:
00524         /** FIFO list */
00525         std::deque<IteratorState> FIFO_;
00526       };
00527 
00528     //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00529     /** \brief Octree leaf node iterator class
00530      * \note This class implements a forward iterator for traversing the leaf nodes of an octree data structure.
00531      * \ingroup octree
00532      * \author Julius Kammerl (julius@kammerl.de)
00533      */
00534     //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00535     template<typename OctreeT>
00536       class OctreeLeafNodeIterator : public OctreeDepthFirstIterator<OctreeT>
00537       {
00538         typedef typename OctreeDepthFirstIterator<OctreeT>::BranchNode BranchNode;
00539         typedef typename OctreeDepthFirstIterator<OctreeT>::LeafNode LeafNode;
00540 
00541       public:
00542         /** \brief Empty constructor.
00543          * \param[in] max_depth_arg Depth limitation during traversal
00544          */
00545         explicit
00546         OctreeLeafNodeIterator (unsigned int max_depth_arg = 0) :
00547             OctreeDepthFirstIterator<OctreeT> (max_depth_arg)
00548         {
00549           reset ();
00550         }
00551 
00552         /** \brief Constructor.
00553          * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node.
00554          * \param[in] max_depth_arg Depth limitation during traversal
00555          */
00556         explicit
00557         OctreeLeafNodeIterator (OctreeT* octree_arg, unsigned int max_depth_arg = 0) :
00558             OctreeDepthFirstIterator<OctreeT> (octree_arg, max_depth_arg)
00559         {
00560           reset ();
00561         }
00562 
00563         /** \brief Empty deconstructor. */
00564         virtual
00565         ~OctreeLeafNodeIterator ()
00566         {
00567         }
00568 
00569         /** \brief Reset the iterator to the root node of the octree
00570          */
00571         inline void
00572         reset ()
00573         {
00574           OctreeDepthFirstIterator<OctreeT>::reset ();
00575           this->operator++ ();
00576         }
00577 
00578         /** \brief Preincrement operator.
00579          * \note recursively step to next octree leaf node
00580          */
00581         inline OctreeLeafNodeIterator&
00582         operator++ ()
00583         {
00584           do
00585           {
00586             OctreeDepthFirstIterator<OctreeT>::operator++ ();
00587           } while ((this->current_state_) && (this->current_state_->node_->getNodeType () != LEAF_NODE));
00588 
00589           return (*this);
00590         }
00591 
00592         /** \brief postincrement operator.
00593          * \note step to next octree node
00594          */
00595         inline OctreeLeafNodeIterator
00596         operator++ (int)
00597         {
00598           OctreeLeafNodeIterator _Tmp = *this;
00599           ++*this;
00600           return (_Tmp);
00601         }
00602 
00603         /** \brief *operator.
00604          * \return pointer to the current octree leaf node
00605          */
00606         OctreeNode*
00607         operator* () const
00608         {
00609           // return designated object
00610           OctreeNode* ret = 0;
00611 
00612           if (this->current_state_ && (this->current_state_->node_->getNodeType () == LEAF_NODE))
00613             ret = this->current_state_->node_;
00614           return (ret);
00615         }
00616       };
00617 
00618   }
00619 }
00620 
00621 #endif
00622