Point Cloud Library (PCL)  1.3.1
octree_iterator.hpp
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-2011, 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.hpp 3018 2011-11-01 03:24:12Z svn $
00037  */
00038 
00039 #ifndef OCTREE_ITERATOR_HPP_
00040 #define OCTREE_ITERATOR_HPP_
00041 
00042 #include <vector>
00043 #include <assert.h>
00044 
00045 #include "pcl/common/common.h"
00046 
00047 namespace pcl
00048 {
00049   namespace octree
00050   {
00051 
00052     using namespace std;
00053 
00055     template<typename DataT, typename LeafT, typename OctreeT>
00056       OctreeNodeIterator<DataT, LeafT, OctreeT>::OctreeNodeIterator (const OctreeT& octree_arg) :
00057         octree_ (octree_arg), currentNode_ (NULL), currentChildIdx_ (0)
00058       {
00059 
00060         // allocate stack
00061         stack_ .reserve (octree_.getTreeDepth ());
00062 
00063         // initialize iterator
00064         reset ();
00065 
00066       }
00067 
00069     template<typename DataT, typename LeafT, typename OctreeT>
00070       OctreeNodeIterator<DataT, LeafT, OctreeT>::~OctreeNodeIterator ()
00071       {
00072 
00073       }
00074 
00076     template<typename DataT, typename LeafT, typename OctreeT>
00077       void
00078       OctreeNodeIterator<DataT, LeafT, OctreeT>::reset ()
00079       {
00080         // initialize iterator globals
00081         currentNode_ = (OctreeNode*)octree_.getRootNode ();
00082         currentChildIdx_ = 0;
00083         currentOctreeDepth_ = 0;
00084 
00085         // reset octree key
00086         currentOctreeKey_.x = currentOctreeKey_.y = currentOctreeKey_.z = 0;
00087 
00088         // empty stack
00089         stack_.clear ();
00090       }
00091 
00093     template<typename DataT, typename LeafT, typename OctreeT>
00094       void
00095       OctreeNodeIterator<DataT, LeafT, OctreeT>::skipChildVoxels ()
00096       {
00097         if (currentNode_)
00098         {
00099           // make sure, we are not at the root node
00100           if (stack_.size () > 0)
00101           {
00102             // pop stack
00103             std::pair<OctreeNode const*, unsigned char>& stackEntry = stack_.back ();
00104             stack_.pop_back ();
00105 
00106             // assign parent node and child index
00107             currentNode_ = stackEntry.first;
00108             currentChildIdx_ = stackEntry.second;
00109 
00110             // update octree key
00111             currentOctreeKey_.x = (currentOctreeKey_.x >> 1);
00112             currentOctreeKey_.y = (currentOctreeKey_.y >> 1);
00113             currentOctreeKey_.z = (currentOctreeKey_.z >> 1);
00114 
00115             // update octree depth
00116             currentOctreeDepth_--;
00117 
00118           }
00119           else
00120           {
00121             // we are at root node level - finish
00122             currentNode_ = NULL;
00123           }
00124 
00125         }
00126 
00127       }
00128 
00130     template<typename DataT, typename LeafT, typename OctreeT>
00131       OctreeNodeIterator<DataT, LeafT, OctreeT>&
00132       OctreeNodeIterator<DataT, LeafT, OctreeT>::operator++ ()
00133       {
00134         if (currentNode_)
00135         {
00136 
00137           bool bTreeUp = false;
00138           const OctreeNode* itNode = NULL;
00139 
00140           if (currentNode_->getNodeType () == BRANCH_NODE)
00141           {
00142             // current node is a branch node
00143             const OctreeBranch* currentBranch = (const OctreeBranch*)currentNode_;
00144 
00145             // find next existing child node
00146             while ((currentChildIdx_ < 8) && !(itNode = octree_.getBranchChild (*currentBranch, currentChildIdx_)))
00147             {
00148               currentChildIdx_++;
00149             };
00150 
00151             if (currentChildIdx_ == 8)
00152             {
00153               // all childs traversed -> back to parent node
00154               bTreeUp = true;
00155             }
00156           }
00157           else
00158           {
00159             // at leaf node level, we need to return to parent node
00160             bTreeUp = true;
00161           }
00162 
00163           if (bTreeUp)
00164           {
00165             // return to parent node
00166 
00167             if (stack_.size () > 0)
00168             {
00169               // pop the stack
00170               std::pair<OctreeNode const*, unsigned char>& stackEntry = stack_.back ();
00171               stack_.pop_back ();
00172 
00173               // assign parent node and child index
00174               currentNode_ = stackEntry.first;
00175               currentChildIdx_ = stackEntry.second;
00176 
00177               // update octree key
00178               currentOctreeKey_.x = (currentOctreeKey_.x >> 1);
00179               currentOctreeKey_.y = (currentOctreeKey_.y >> 1);
00180               currentOctreeKey_.z = (currentOctreeKey_.z >> 1);
00181 
00182               // update octree depth
00183               currentOctreeDepth_--;
00184             }
00185             else
00186             {
00187               // root level -> finish
00188               currentNode_ = NULL;
00189             }
00190 
00191           }
00192           else
00193           {
00194             // traverse child node
00195 
00196             // new stack entry
00197             std::pair<OctreeNode const*, unsigned char> newStackEntry;
00198 
00199             // assign current node and child index to stack entry
00200             newStackEntry.first = currentNode_;
00201             newStackEntry.second = currentChildIdx_ + 1;
00202 
00203             // push stack entry to stack
00204             stack_.push_back (newStackEntry);
00205 
00206             // update octree key
00207             currentOctreeKey_.x = (currentOctreeKey_.x << 1) | (!!(currentChildIdx_ & (1 << 2)));
00208             currentOctreeKey_.y = (currentOctreeKey_.y << 1) | (!!(currentChildIdx_ & (1 << 1)));
00209             currentOctreeKey_.z = (currentOctreeKey_.z << 1) | (!!(currentChildIdx_ & (1 << 0)));
00210 
00211             // update octree depth
00212             currentOctreeDepth_++;
00213 
00214             // traverse to child node
00215             currentChildIdx_ = 0;
00216             currentNode_ = itNode;
00217           }
00218         }
00219 
00220         return (*this);
00221       }
00222   }
00223 }
00224 
00225 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines