Point Cloud Library (PCL)  1.11.0
simple_octree.hpp
1 /*
2  * simple_octree.hpp
3  *
4  * Created on: Mar 12, 2013
5  * Author: papazov
6  */
7 
8 #ifndef SIMPLE_OCTREE_HPP_
9 #define SIMPLE_OCTREE_HPP_
10 
11 #include <algorithm>
12 #include <cmath>
13 
14 
15 namespace pcl
16 {
17 
18 namespace recognition
19 {
20 
21 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
23 : data_ (nullptr),
24  parent_ (nullptr),
25  children_(nullptr)
26 {}
27 
28 
29 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
31 {
32  this->deleteChildren ();
33  this->deleteData ();
34 }
35 
36 
37 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
39 {
40  center_[0] = c[0];
41  center_[1] = c[1];
42  center_[2] = c[2];
43 }
44 
45 
46 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
48 {
49  bounds_[0] = b[0];
50  bounds_[1] = b[1];
51  bounds_[2] = b[2];
52  bounds_[3] = b[3];
53  bounds_[4] = b[4];
54  bounds_[5] = b[5];
55 }
56 
57 
58 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
60 {
61  Scalar v[3] = {static_cast<Scalar> (0.5)*(bounds_[1]-bounds_[0]),
62  static_cast<Scalar> (0.5)*(bounds_[3]-bounds_[2]),
63  static_cast<Scalar> (0.5)*(bounds_[5]-bounds_[4])};
64 
65  radius_ = static_cast<Scalar> (std::sqrt (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]));
66 }
67 
68 
69 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline bool
71 {
72  if ( children_ )
73  return (false);
74 
75  Scalar bounds[6], center[3], childside = static_cast<Scalar> (0.5)*(bounds_[1]-bounds_[0]);
76  children_ = new Node[8];
77 
78  // Compute bounds and center for child 0, i.e., for (0,0,0)
79  bounds[0] = bounds_[0]; bounds[1] = center_[0];
80  bounds[2] = bounds_[2]; bounds[3] = center_[1];
81  bounds[4] = bounds_[4]; bounds[5] = center_[2];
82  // Compute the center of the new child
83  center[0] = 0.5f*(bounds[0] + bounds[1]);
84  center[1] = 0.5f*(bounds[2] + bounds[3]);
85  center[2] = 0.5f*(bounds[4] + bounds[5]);
86  // Save the results
87  children_[0].setBounds(bounds);
88  children_[0].setCenter(center);
89 
90  // Compute bounds and center for child 1, i.e., for (0,0,1)
91  bounds[4] = center_[2]; bounds[5] = bounds_[5];
92  // Update the center
93  center[2] += childside;
94  // Save the results
95  children_[1].setBounds(bounds);
96  children_[1].setCenter(center);
97 
98  // Compute bounds and center for child 3, i.e., for (0,1,1)
99  bounds[2] = center_[1]; bounds[3] = bounds_[3];
100  // Update the center
101  center[1] += childside;
102  // Save the results
103  children_[3].setBounds(bounds);
104  children_[3].setCenter(center);
105 
106  // Compute bounds and center for child 2, i.e., for (0,1,0)
107  bounds[4] = bounds_[4]; bounds[5] = center_[2];
108  // Update the center
109  center[2] -= childside;
110  // Save the results
111  children_[2].setBounds(bounds);
112  children_[2].setCenter(center);
113 
114  // Compute bounds and center for child 6, i.e., for (1,1,0)
115  bounds[0] = center_[0]; bounds[1] = bounds_[1];
116  // Update the center
117  center[0] += childside;
118  // Save the results
119  children_[6].setBounds(bounds);
120  children_[6].setCenter(center);
121 
122  // Compute bounds and center for child 7, i.e., for (1,1,1)
123  bounds[4] = center_[2]; bounds[5] = bounds_[5];
124  // Update the center
125  center[2] += childside;
126  // Save the results
127  children_[7].setBounds(bounds);
128  children_[7].setCenter(center);
129 
130  // Compute bounds and center for child 5, i.e., for (1,0,1)
131  bounds[2] = bounds_[2]; bounds[3] = center_[1];
132  // Update the center
133  center[1] -= childside;
134  // Save the results
135  children_[5].setBounds(bounds);
136  children_[5].setCenter(center);
137 
138  // Compute bounds and center for child 4, i.e., for (1,0,0)
139  bounds[4] = bounds_[4]; bounds[5] = center_[2];
140  // Update the center
141  center[2] -= childside;
142  // Save the results
143  children_[4].setBounds(bounds);
144  children_[4].setCenter(center);
145 
146  for ( int i = 0 ; i < 8 ; ++i )
147  {
148  children_[i].computeRadius();
149  children_[i].setParent(this);
150  }
151 
152  return (true);
153 }
154 
155 
156 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
158 {
159  delete[] children_;
160  children_ = nullptr;
161 }
162 
163 
164 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
166 {
167  delete data_;
168  data_ = nullptr;
169 }
170 
171 
172 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
174 {
175  if ( !this->hasData () || !node->hasData () )
176  return;
177 
178  this->full_leaf_neighbors_.insert (node);
179  node->full_leaf_neighbors_.insert (this);
180 }
181 
182 
183 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
185 : tree_levels_ (0),
186  root_ (nullptr)
187 {
188 }
189 
190 
191 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
193 {
194  this->clear ();
195 }
196 
197 
198 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
200 {
201  delete root_;
202  root_ = nullptr;
203 
204  full_leaves_.clear();
205 }
206 
207 
208 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
209 SimpleOctree<NodeData, NodeDataCreator, Scalar>::build (const Scalar* bounds, Scalar voxel_size,
210  NodeDataCreator* node_data_creator)
211 {
212  if ( voxel_size <= 0 )
213  return;
214 
215  this->clear();
216 
217  voxel_size_ = voxel_size;
218  node_data_creator_ = node_data_creator;
219 
220  Scalar extent = std::max (std::max (bounds[1]-bounds[0], bounds[3]-bounds[2]), bounds[5]-bounds[4]);
221  Scalar center[3] = {static_cast<Scalar> (0.5)*(bounds[0]+bounds[1]),
222  static_cast<Scalar> (0.5)*(bounds[2]+bounds[3]),
223  static_cast<Scalar> (0.5)*(bounds[4]+bounds[5])};
224 
225  Scalar arg = extent/voxel_size;
226 
227  // Compute the number of tree levels
228  if ( arg > 1 )
229  tree_levels_ = static_cast<int> (std::ceil (std::log (arg)/std::log (2.0)) + 0.5);
230  else
231  tree_levels_ = 0;
232 
233  // Compute the number of octree levels and the bounds of the root
234  Scalar half_root_side = static_cast<Scalar> (0.5f*pow (2.0, tree_levels_)*voxel_size);
235 
236  // Determine the bounding box of the octree
237  bounds_[0] = center[0] - half_root_side;
238  bounds_[1] = center[0] + half_root_side;
239  bounds_[2] = center[1] - half_root_side;
240  bounds_[3] = center[1] + half_root_side;
241  bounds_[4] = center[2] - half_root_side;
242  bounds_[5] = center[2] + half_root_side;
243 
244  // Create and initialize the root
245  root_ = new Node ();
246  root_->setCenter (center);
247  root_->setBounds (bounds_);
248  root_->setParent (nullptr);
249  root_->computeRadius ();
250 }
251 
252 
253 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
256 {
257  // Make sure that the input point is within the octree bounds
258  if ( x < bounds_[0] || x > bounds_[1] ||
259  y < bounds_[2] || y > bounds_[3] ||
260  z < bounds_[4] || z > bounds_[5] )
261  {
262  return (nullptr);
263  }
264 
265  Node* node = root_;
266 
267  // Go down to the right leaf
268  for ( int l = 0 ; l < tree_levels_ ; ++l )
269  {
270  node->createChildren ();
271  const Scalar *c = node->getCenter ();
272  int id = 0;
273 
274  if ( x >= c[0] ) id |= 4;
275  if ( y >= c[1] ) id |= 2;
276  if ( z >= c[2] ) id |= 1;
277 
278  node = node->getChild (id);
279  }
280 
281  if ( !node->hasData () )
282  {
283  node->setData (node_data_creator_->create (node));
284  this->insertNeighbors (node);
285  full_leaves_.push_back (node);
286  }
287 
288  return (node);
289 }
290 
291 
292 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
295 {
296  Scalar offset = 0.5f*voxel_size_;
297  Scalar p[3] = {bounds_[0] + offset + static_cast<Scalar> (i)*voxel_size_,
298  bounds_[2] + offset + static_cast<Scalar> (j)*voxel_size_,
299  bounds_[4] + offset + static_cast<Scalar> (k)*voxel_size_};
300 
301  return (this->getFullLeaf (p[0], p[1], p[2]));
302 }
303 
304 
305 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
308 {
309  // Make sure that the input point is within the octree bounds
310  if ( x < bounds_[0] || x > bounds_[1] ||
311  y < bounds_[2] || y > bounds_[3] ||
312  z < bounds_[4] || z > bounds_[5] )
313  {
314  return (nullptr);
315  }
316 
317  Node* node = root_;
318 
319  // Go down to the right leaf
320  for ( int l = 0 ; l < tree_levels_ ; ++l )
321  {
322  if ( !node->hasChildren () )
323  return (nullptr);
324 
325  const Scalar *c = node->getCenter ();
326  int id = 0;
327 
328  if ( x >= c[0] ) id |= 4;
329  if ( y >= c[1] ) id |= 2;
330  if ( z >= c[2] ) id |= 1;
331 
332  node = node->getChild (id);
333  }
334 
335  if ( !node->hasData () )
336  return (nullptr);
337 
338  return (node);
339 }
340 
341 
342 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
344 {
345  const Scalar* c = node->getCenter ();
346  Scalar s = static_cast<Scalar> (0.5)*voxel_size_;
347  Node *neigh;
348 
349  neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
350  neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
351  neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
352  neigh = this->getFullLeaf (c[0]+s, c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
353  neigh = this->getFullLeaf (c[0]+s, c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh);
354  neigh = this->getFullLeaf (c[0]+s, c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
355  neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
356  neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
357  neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
358 
359  neigh = this->getFullLeaf (c[0] , c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
360  neigh = this->getFullLeaf (c[0] , c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
361  neigh = this->getFullLeaf (c[0] , c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
362  neigh = this->getFullLeaf (c[0] , c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
363 //neigh = this->getFullLeaf (c[0] , c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh);
364  neigh = this->getFullLeaf (c[0] , c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
365  neigh = this->getFullLeaf (c[0] , c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
366  neigh = this->getFullLeaf (c[0] , c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
367  neigh = this->getFullLeaf (c[0] , c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
368 
369  neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
370  neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
371  neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
372  neigh = this->getFullLeaf (c[0]-s, c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
373  neigh = this->getFullLeaf (c[0]-s, c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh);
374  neigh = this->getFullLeaf (c[0]-s, c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
375  neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
376  neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
377  neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
378 }
379 
380 } // namespace recognition
381 } // namespace pcl
382 
383 #endif /* SIMPLE_OCTREE_HPP_ */
384 
pcl
Definition: convolution.h:46
pcl::recognition::SimpleOctree::Node::Node
Node()
Definition: simple_octree.hpp:22
pcl::recognition::SimpleOctree::Node::createChildren
bool createChildren()
Definition: simple_octree.hpp:70
pcl::recognition::SimpleOctree::Node::getChild
Node * getChild(int id)
Definition: simple_octree.h:84
pcl::recognition::SimpleOctree::bounds_
Scalar bounds_[6]
Definition: simple_octree.h:201
pcl::recognition::SimpleOctree::Node::deleteChildren
void deleteChildren()
Definition: simple_octree.hpp:157
pcl::recognition::SimpleOctree
Definition: simple_octree.h:59
pcl::recognition::SimpleOctree::Node::hasData
bool hasData()
Definition: simple_octree.h:105
pcl::recognition::SimpleOctree::insertNeighbors
void insertNeighbors(Node *node)
Definition: simple_octree.hpp:343
pcl::recognition::SimpleOctree::clear
void clear()
Definition: simple_octree.hpp:199
pcl::recognition::SimpleOctree::createLeaf
Node * createLeaf(Scalar x, Scalar y, Scalar z)
Creates the leaf containing p = (x, y, z) and returns a pointer to it, however, only if p lies within...
Definition: simple_octree.hpp:255
pcl::recognition::SimpleOctree::SimpleOctree
SimpleOctree()
Definition: simple_octree.hpp:184
pcl::recognition::SimpleOctree::Node::getCenter
const Scalar * getCenter() const
Definition: simple_octree.h:75
pcl::recognition::SimpleOctree::Node
Definition: simple_octree.h:62
pcl::recognition::SimpleOctree::tree_levels_
int tree_levels_
Definition: simple_octree.h:202
pcl::recognition::SimpleOctree::Node::full_leaf_neighbors_
std::set< Node * > full_leaf_neighbors_
Definition: simple_octree.h:145
pcl::recognition::SimpleOctree::Node::setBounds
void setBounds(const Scalar *b)
Definition: simple_octree.hpp:47
pcl::recognition::SimpleOctree::Node::makeNeighbors
void makeNeighbors(Node *node)
Make this and 'node' neighbors by inserting each node in the others node neighbor set.
Definition: simple_octree.hpp:173
pcl::recognition::SimpleOctree::Node::computeRadius
void computeRadius()
Computes the "radius" of the node which is half the diagonal length.
Definition: simple_octree.hpp:59
pcl::recognition::SimpleOctree::root_
Node * root_
Definition: simple_octree.h:203
pcl::recognition::SimpleOctree::getFullLeaf
Node * getFullLeaf(int i, int j, int k)
Since the leaves are aligned in a rectilinear grid, each leaf has a unique id.
Definition: simple_octree.hpp:294
pcl::recognition::SimpleOctree::build
void build(const Scalar *bounds, Scalar voxel_size, NodeDataCreator *node_data_creator)
Creates an empty octree with bounds at least as large as the ones provided as input and with leaf siz...
Definition: simple_octree.hpp:209
pcl::recognition::SimpleOctree::Node::hasChildren
bool hasChildren()
Definition: simple_octree.h:108
pcl::recognition::SimpleOctree::Node::setData
void setData(const NodeData &src)
Definition: simple_octree.h:90
pcl::recognition::SimpleOctree::~SimpleOctree
virtual ~SimpleOctree()
Definition: simple_octree.hpp:192
pcl::recognition::SimpleOctree::Node::deleteData
void deleteData()
Definition: simple_octree.hpp:165
pcl::recognition::SimpleOctree::Node::setCenter
void setCenter(const Scalar *c)
Definition: simple_octree.hpp:38