Point Cloud Library (PCL)
1.3.1
|
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