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-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