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