Point Cloud Library (PCL)  1.14.1-dev
outofcore_depth_first_iterator.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  */
38 
39 #ifndef PCL_OUTOFCORE_DEPTH_FIRST_ITERATOR_IMPL_H_
40 #define PCL_OUTOFCORE_DEPTH_FIRST_ITERATOR_IMPL_H_
41 
42 namespace pcl
43 {
44  namespace outofcore
45  {
46 
47  template<typename PointT, typename ContainerT>
49  : OutofcoreIteratorBase<PointT, ContainerT> (octree_arg)
50  ,
51  stack_ (0)
52  {
53  stack_.reserve (this->octree_.getTreeDepth ());
55  }
56 
57  ////////////////////////////////////////////////////////////////////////////////
58 
59  template<typename PointT, typename ContainerT>
61 
62  ////////////////////////////////////////////////////////////////////////////////
63 
64  template<typename PointT, typename ContainerT>
67  {
68  //when currentNode_ is 0, skip incrementing because it is already at the end
69  if (this->currentNode_)
70  {
71  bool bTreeUp = false;
73 
74  if (this->currentNode_->getNodeType () == pcl::octree::BRANCH_NODE)
75  {
76  auto* currentBranch = static_cast<BranchNode*> (this->currentNode_);
77 
78  if (currentChildIdx_ < 8)
79  {
80  itNode = this->octree_.getBranchChildPtr (*currentBranch, currentChildIdx_);
81 
82  //keep looking for a valid child until we've run out of children or a valid one is found
83  while ((currentChildIdx_ < 7) && !(itNode))
84  {
85  //find next existing child node
86  currentChildIdx_++;
87  itNode = this->octree_.getBranchChildPtr (*currentBranch, currentChildIdx_);
88  }
89  //if no valid one was found, set flag to move back up the tree to the parent node
90  if (!itNode)
91  {
92  bTreeUp = true;
93  }
94  }
95  else
96  {
97  bTreeUp = true;
98  }
99  }
100  else
101  {
102  bTreeUp = true;
103  }
104 
105  if (bTreeUp)
106  {
107  if (!stack_.empty ())
108  {
109  std::pair<OutofcoreOctreeBaseNode<ContainerT, PointT>*, unsigned char>& stackEntry = stack_.back ();
110  stack_.pop_back ();
111 
112  this->currentNode_ = stackEntry.first;
113  currentChildIdx_ = stackEntry.second;
114 
115  //don't do anything with the keys here...
116  this->currentOctreeDepth_--;
117  }
118  else
119  {
120  this->currentNode_ = nullptr;
121  }
122 
123  }
124  else
125  {
126  std::pair<OutofcoreOctreeBaseNode<ContainerT, PointT>*, unsigned char> newStackEntry;
127  newStackEntry.first = this->currentNode_;
128  newStackEntry.second = static_cast<unsigned char> (currentChildIdx_+1);
129 
130  stack_.push_back (newStackEntry);
131 
132  //don't do anything with the keys here...
133 
134  this->currentOctreeDepth_++;
135  currentChildIdx_= 0;
136  this->currentNode_ = itNode;
137  }
138  }
139 
140  return (*this);
141  }
142 
143  ////////////////////////////////////////////////////////////////////////////////
144 
145  }//namespace pcl
146 }//namespace outofcore
147 
148 #endif //PCL_OUTOFCORE_DEPTH_FIRST_ITERATOR_IMPL_H_
149 
std::vector< std::pair< OctreeDiskNode *, unsigned char > > stack_
This code defines the octree used for point storage at Urban Robotics.
Definition: octree_base.h:151
std::uint64_t getTreeDepth() const
Definition: octree_base.h:441
OutofcoreOctreeBaseNode Class internally representing nodes of an outofcore octree,...
A point structure representing Euclidean xyz coordinates, and the RGB color.