octree_iterator.h
Go to the documentation of this file.
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: octree_iterator.h 6119 2012-07-03 18:50:04Z aichim $
00037  */
00038 
00039 #ifndef OCTREE_ITERATOR_H
00040 #define 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   {
00068     template<typename DataT, typename OctreeT>
00069       class OctreeIteratorBase : public std::iterator<std::forward_iterator_tag, const OctreeNode, void,
00070           const OctreeNode*, const OctreeNode&>
00071       {
00072       public:
00073 
00074         typedef typename OctreeT::LeafNode LeafNode;
00075         typedef typename OctreeT::BranchNode BranchNode;
00076 
00080         explicit
00081         OctreeIteratorBase (OctreeT& octree_arg) :
00082             octree_ (octree_arg), currentNode_ (NULL)
00083         {
00084           reset ();
00085         }
00086 
00090         OctreeIteratorBase (const OctreeIteratorBase& src) :
00091             octree_ (src.octree_), currentNode_ (src.currentNode_), currentOctreeDepth_ (src.currentOctreeDepth_), currentOctreeKey_ (
00092                 src.currentOctreeKey_)
00093         {
00094         }
00095 
00099         inline OctreeIteratorBase&
00100         operator = (const OctreeIteratorBase& src)
00101         {
00102           octree_ = src.octree_;
00103           currentNode_ = src.currentNode_;
00104           currentOctreeDepth_ = src.currentOctreeDepth_;
00105           currentOctreeKey_ = src.currentOctreeKey_;
00106           return (*this);
00107         }
00108 
00110         virtual
00111         ~OctreeIteratorBase ()
00112         {
00113         }
00114 
00116         inline void
00117         reset ()
00118         {
00119           // initialize iterator globals
00120           currentNode_ = static_cast<OctreeNode*> (octree_.getRootNode ());
00121           currentOctreeDepth_ = 0;
00122 
00123           // reset octree key
00124           currentOctreeKey_.x = currentOctreeKey_.y = currentOctreeKey_.z = 0;
00125         }
00126 
00130         inline const OctreeKey&
00131         getCurrentOctreeKey () const
00132         {
00133           return (currentOctreeKey_);
00134         }
00135 
00139         inline unsigned int
00140         getCurrentOctreeDepth () const
00141         {
00142           return (currentOctreeDepth_);
00143         }
00144 
00148         inline OctreeNode*
00149         getCurrentOctreeNode () const
00150         {
00151           return (currentNode_);
00152         }
00153 
00157         inline OctreeNode*
00158         operator* () const
00159         { // return designated object
00160           return (this->getCurrentOctreeNode ());
00161         }
00162 
00166         inline bool
00167         isBranchNode () const
00168         {
00169           return (currentNode_->getNodeType () == BRANCH_NODE);
00170         }
00171 
00175         inline bool
00176         isLeafNode () const
00177         {
00178           return (currentNode_->getNodeType () == LEAF_NODE);
00179         }
00180 
00184         inline char
00185         getNodeConfiguration () const
00186         {
00187           char ret = 0;
00188 
00189           if (isBranchNode ())
00190           {
00191 
00192             // current node is a branch node
00193             const BranchNode* currentBranch = static_cast<const BranchNode*> (this->currentNode_);
00194 
00195             // get child configuration bit pattern
00196             ret = octree_.getBranchBitPattern (*currentBranch);
00197 
00198           }
00199 
00200           return (ret);
00201         }
00202 
00206         virtual void
00207         getData (DataT& data_arg) const
00208         {
00209 
00210           assert(this->currentNode_ );
00211 
00212           octree_.getDataFromOctreeNode(this->currentNode_, data_arg);
00213         }
00214 
00218         virtual void
00219         getData (std::vector<DataT>& dataVector_arg) const
00220         {
00221           assert(this->currentNode_);
00222 
00223           octree_.getDataFromOctreeNode(this->currentNode_, dataVector_arg);
00224         }
00225 
00228         virtual std::size_t
00229         getSize () const
00230         {
00231           assert(this->currentNode_);
00232 
00233           return octree_.getDataSizeFromOctreeNode(this->currentNode_);
00234         }
00235 
00239         virtual unsigned long
00240         getNodeID () const
00241         {
00242           unsigned long id = 0;
00243 
00244           if (this->currentNode_)
00245           {
00246             // calculate integer id with respect to octree key
00247             unsigned int depth = octree_.getTreeDepth ();
00248             id = currentOctreeKey_.x << (depth * 2) | currentOctreeKey_.y << (depth * 1)
00249                 | currentOctreeKey_.z << (depth * 0);
00250           }
00251 
00252           return id;
00253         }
00254 
00255       protected:
00257         OctreeT& octree_;
00258 
00260         OctreeNode* currentNode_;
00261 
00263         unsigned int currentOctreeDepth_;
00264 
00266         OctreeKey currentOctreeKey_;
00267       };
00268 
00270 
00275     template<typename DataT, typename OctreeT>
00276       class OctreeDepthFirstIterator : public OctreeIteratorBase<DataT, OctreeT>
00277       {
00278 
00279       public:
00280 
00281         typedef typename OctreeIteratorBase<DataT, OctreeT>::LeafNode LeafNode;
00282         typedef typename OctreeIteratorBase<DataT, OctreeT>::BranchNode BranchNode;
00283 
00284 
00288         explicit
00289         OctreeDepthFirstIterator (OctreeT& octree_arg);
00290 
00292         virtual
00293         ~OctreeDepthFirstIterator ();
00294 
00295 
00298         virtual void
00299         reset ();
00300 
00304         OctreeDepthFirstIterator&
00305         operator++ ();
00306 
00310         inline OctreeDepthFirstIterator
00311         operator++ (int)
00312         {
00313           OctreeDepthFirstIterator _Tmp = *this;
00314           ++*this;
00315           return (_Tmp);
00316         }
00317 
00320         void
00321         skipChildVoxels ();
00322 
00323       protected:
00325         unsigned char currentChildIdx_;
00326 
00328         std::vector<std::pair<OctreeNode*, unsigned char> > stack_;
00329       };
00330 
00332 
00337     template<typename DataT, typename OctreeT>
00338       class OctreeBreadthFirstIterator : public OctreeIteratorBase<DataT, OctreeT>
00339       {
00340         // public typedefs
00341         typedef typename OctreeIteratorBase<DataT, OctreeT>::BranchNode BranchNode;
00342         typedef typename OctreeIteratorBase<DataT, OctreeT>::LeafNode LeafNode;
00343 
00344         struct FIFOElement
00345         {
00346           OctreeNode* node;
00347           OctreeKey key;
00348           unsigned int depth;
00349         };
00350 
00351       public:
00355         explicit
00356         OctreeBreadthFirstIterator (OctreeT& octree_arg);
00357 
00359         virtual
00360         ~OctreeBreadthFirstIterator ();
00361 
00364         void
00365         reset ();
00366 
00370         OctreeBreadthFirstIterator&
00371         operator++ ();
00372 
00376         inline OctreeBreadthFirstIterator
00377         operator++ (int)
00378         {
00379           OctreeBreadthFirstIterator _Tmp = *this;
00380           ++*this;
00381           return (_Tmp);
00382         }
00383 
00384       protected:
00388         void
00389         addChildNodesToFIFO (const OctreeNode* node);
00390 
00392         std::deque<FIFOElement> FIFO_;
00393       };
00394 
00396 
00401 
00402     template<typename DataT, typename OctreeT>
00403       class OctreeLeafNodeIterator : public OctreeDepthFirstIterator<DataT, OctreeT>
00404       {
00405         typedef typename OctreeDepthFirstIterator<DataT, OctreeT>::BranchNode BranchNode;
00406         typedef typename OctreeDepthFirstIterator<DataT, OctreeT>::LeafNode LeafNode;
00407 
00408       public:
00412         explicit
00413         OctreeLeafNodeIterator (OctreeT& octree_arg) :
00414             OctreeDepthFirstIterator<DataT, OctreeT> (octree_arg)
00415         {
00416           reset ();
00417         }
00418 
00420         virtual
00421         ~OctreeLeafNodeIterator ()
00422         {
00423         }
00424 
00427         inline void
00428         reset ()
00429         {
00430           OctreeDepthFirstIterator<DataT, OctreeT>::reset ();
00431         }
00432 
00436         inline OctreeLeafNodeIterator&
00437         operator++ ()
00438         {
00439           do
00440           {
00441             OctreeDepthFirstIterator<DataT, OctreeT>::operator++ ();
00442           } while ((this->currentNode_) && (this->currentNode_->getNodeType () != LEAF_NODE));
00443 
00444           return (*this);
00445         }
00446 
00450         inline OctreeLeafNodeIterator
00451         operator++ (int)
00452         {
00453           OctreeLeafNodeIterator _Tmp = *this;
00454           ++*this;
00455           return (_Tmp);
00456         }
00457 
00461         OctreeNode*
00462         operator* () const
00463         {
00464           // return designated object
00465           OctreeNode* ret = 0;
00466 
00467           if (this->currentNode_ && (this->currentNode_->getNodeType () == LEAF_NODE))
00468             ret = this->currentNode_;
00469           return (ret);
00470         }
00471       };
00472 
00473   }
00474 }
00475 
00476 #endif
00477 


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:15:57