Point Cloud Library (PCL)  1.7.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_ */
Node * getFullLeaf(int i, int j, int k)
Since the leaves are aligned in a rectilinear grid, each leaf has a unique id.
void setData(const NodeData &src)
Definition: simple_octree.h:89
const Scalar * getCenter() const
Definition: simple_octree.h:74
void makeNeighbors(Node *node)
Make this and 'node' neighbors by inserting each node in the others node neighbor set...
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 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...
void computeRadius()
Computes the "radius" of the node which is half the diagonal length.