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