Point Cloud Library (PCL)
1.7.0
|
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$ 00037 */ 00038 00039 #ifndef PCL_OCTREE_ITERATOR_HPP_ 00040 #define PCL_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 template<typename OctreeT> 00053 OctreeDepthFirstIterator<OctreeT>::OctreeDepthFirstIterator (unsigned int max_depth_arg) : 00054 OctreeIteratorBase<OctreeT> (max_depth_arg), stack_ () 00055 { 00056 // initialize iterator 00057 this->reset (); 00058 } 00059 00060 ////////////////////////////////////////////////////////////////////////////////////////////// 00061 template<typename OctreeT> 00062 OctreeDepthFirstIterator<OctreeT>::OctreeDepthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg) : 00063 OctreeIteratorBase<OctreeT> (octree_arg, max_depth_arg), stack_ () 00064 { 00065 // initialize iterator 00066 this->reset (); 00067 } 00068 00069 ////////////////////////////////////////////////////////////////////////////////////////////// 00070 template<typename OctreeT> 00071 OctreeDepthFirstIterator<OctreeT>::~OctreeDepthFirstIterator () 00072 { 00073 } 00074 00075 ////////////////////////////////////////////////////////////////////////////////////////////// 00076 template<typename OctreeT> 00077 void OctreeDepthFirstIterator<OctreeT>::reset () 00078 { 00079 OctreeIteratorBase<OctreeT>::reset (); 00080 00081 if (this->octree_) 00082 { 00083 // allocate stack 00084 stack_.reserve (this->max_octree_depth_); 00085 00086 // empty stack 00087 stack_.clear (); 00088 00089 // pushing root node to stack 00090 IteratorState stack_entry; 00091 stack_entry.node_ = this->octree_->getRootNode (); 00092 stack_entry.depth_ = 0; 00093 stack_entry.key_.x = stack_entry.key_.y = stack_entry.key_.z = 0; 00094 00095 stack_.push_back(stack_entry); 00096 00097 this->current_state_ = &stack_.back(); 00098 } 00099 00100 } 00101 00102 ////////////////////////////////////////////////////////////////////////////////////////////// 00103 template<typename OctreeT> 00104 void OctreeDepthFirstIterator<OctreeT>::skipChildVoxels () 00105 { 00106 00107 if (stack_.size ()) 00108 { 00109 // current depth 00110 unsigned char current_depth = stack_.back ().depth_; 00111 00112 // pop from stack as long as we find stack elements with depth >= current depth 00113 while (stack_.size () && (stack_.back ().depth_ >= current_depth)) 00114 stack_.pop_back (); 00115 00116 if (stack_.size ()) 00117 { 00118 this->current_state_ = &stack_.back(); 00119 } else 00120 { 00121 this->current_state_ = 0; 00122 } 00123 } 00124 00125 } 00126 00127 ////////////////////////////////////////////////////////////////////////////////////////////// 00128 template<typename OctreeT> 00129 OctreeDepthFirstIterator<OctreeT>& 00130 OctreeDepthFirstIterator<OctreeT>::operator++ () 00131 { 00132 00133 if (stack_.size ()) 00134 { 00135 // get stack element 00136 IteratorState stack_entry = stack_.back (); 00137 stack_.pop_back (); 00138 00139 stack_entry.depth_ ++; 00140 OctreeKey& current_key = stack_entry.key_; 00141 00142 if ( (this->max_octree_depth_>=stack_entry.depth_) && 00143 (stack_entry.node_->getNodeType () == BRANCH_NODE) ) 00144 { 00145 unsigned char child_idx; 00146 00147 // current node is a branch node 00148 BranchNode* current_branch = 00149 static_cast<BranchNode*> (stack_entry.node_); 00150 00151 // add all children to stack 00152 for (child_idx = 0; child_idx < 8; ++child_idx) 00153 { 00154 00155 // if child exist 00156 00157 if (this->octree_->branchHasChild(*current_branch, child_idx)) 00158 { 00159 // add child to stack 00160 current_key.pushBranch (child_idx); 00161 00162 stack_entry.node_ = this->octree_->getBranchChildPtr(*current_branch, child_idx); 00163 00164 stack_.push_back(stack_entry); 00165 00166 current_key.popBranch(); 00167 } 00168 } 00169 } 00170 00171 if (stack_.size ()) 00172 { 00173 this->current_state_ = &stack_.back(); 00174 } else 00175 { 00176 this->current_state_ = 0; 00177 } 00178 } 00179 00180 return (*this); 00181 } 00182 00183 ////////////////////////////////////////////////////////////////////////////////////////////// 00184 template<typename OctreeT> 00185 OctreeBreadthFirstIterator<OctreeT>::OctreeBreadthFirstIterator (unsigned int max_depth_arg) : 00186 OctreeIteratorBase<OctreeT> (max_depth_arg), FIFO_ () 00187 { 00188 OctreeIteratorBase<OctreeT>::reset (); 00189 00190 // initialize iterator 00191 this->reset (); 00192 } 00193 00194 ////////////////////////////////////////////////////////////////////////////////////////////// 00195 template<typename OctreeT> 00196 OctreeBreadthFirstIterator<OctreeT>::OctreeBreadthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg) : 00197 OctreeIteratorBase<OctreeT> (octree_arg, max_depth_arg), FIFO_ () 00198 { 00199 OctreeIteratorBase<OctreeT>::reset (); 00200 00201 // initialize iterator 00202 this->reset (); 00203 } 00204 00205 ////////////////////////////////////////////////////////////////////////////////////////////// 00206 template<typename OctreeT> 00207 OctreeBreadthFirstIterator<OctreeT>::~OctreeBreadthFirstIterator () 00208 { 00209 } 00210 00211 ////////////////////////////////////////////////////////////////////////////////////////////// 00212 template<typename OctreeT> 00213 void OctreeBreadthFirstIterator<OctreeT>::reset () 00214 { 00215 OctreeIteratorBase<OctreeT>::reset (); 00216 00217 // init FIFO 00218 FIFO_.clear (); 00219 00220 if (this->octree_) 00221 { 00222 // pushing root node to stack 00223 IteratorState FIFO_entry; 00224 FIFO_entry.node_ = this->octree_->getRootNode (); 00225 FIFO_entry.depth_ = 0; 00226 FIFO_entry.key_.x = FIFO_entry.key_.y = FIFO_entry.key_.z = 0; 00227 00228 FIFO_.push_back(FIFO_entry); 00229 00230 this->current_state_ = &FIFO_.front(); 00231 } 00232 } 00233 00234 ////////////////////////////////////////////////////////////////////////////////////////////// 00235 template<typename OctreeT> 00236 OctreeBreadthFirstIterator<OctreeT>& 00237 OctreeBreadthFirstIterator<OctreeT>::operator++ () 00238 { 00239 00240 if (FIFO_.size ()) 00241 { 00242 // get stack element 00243 IteratorState FIFO_entry = FIFO_.front (); 00244 FIFO_.pop_front (); 00245 00246 FIFO_entry.depth_ ++; 00247 OctreeKey& current_key = FIFO_entry.key_; 00248 00249 if ( (this->max_octree_depth_>=FIFO_entry.depth_) && 00250 (FIFO_entry.node_->getNodeType () == BRANCH_NODE) ) 00251 { 00252 unsigned char child_idx; 00253 00254 // current node is a branch node 00255 BranchNode* current_branch = 00256 static_cast<BranchNode*> (FIFO_entry.node_); 00257 00258 // iterate over all children 00259 for (child_idx = 0; child_idx < 8 ; ++child_idx) 00260 { 00261 00262 // if child exist 00263 if (this->octree_->branchHasChild(*current_branch, child_idx)) 00264 { 00265 // add child to stack 00266 current_key.pushBranch (static_cast<unsigned char> (child_idx)); 00267 00268 FIFO_entry.node_ = this->octree_->getBranchChildPtr(*current_branch, child_idx); 00269 00270 FIFO_.push_back(FIFO_entry); 00271 00272 current_key.popBranch(); 00273 } 00274 } 00275 } 00276 00277 if (FIFO_.size ()) 00278 { 00279 this->current_state_ = &FIFO_.front(); 00280 } else 00281 { 00282 this->current_state_ = 0; 00283 } 00284 00285 } 00286 00287 return (*this); 00288 } 00289 } 00290 } 00291 00292 #endif