Point Cloud Library (PCL)  1.14.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 
185 
186 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
188 {
189  this->clear ();
190 }
191 
192 
193 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
195 {
196  delete root_;
197  root_ = nullptr;
198 
199  full_leaves_.clear();
200 }
201 
202 
203 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
204 SimpleOctree<NodeData, NodeDataCreator, Scalar>::build (const Scalar* bounds, Scalar voxel_size,
205  NodeDataCreator* node_data_creator)
206 {
207  if ( voxel_size <= 0 )
208  return;
209 
210  this->clear();
211 
212  voxel_size_ = voxel_size;
213  node_data_creator_ = node_data_creator;
214 
215  Scalar extent = std::max (std::max (bounds[1]-bounds[0], bounds[3]-bounds[2]), bounds[5]-bounds[4]);
216  Scalar center[3] = {static_cast<Scalar> (0.5)*(bounds[0]+bounds[1]),
217  static_cast<Scalar> (0.5)*(bounds[2]+bounds[3]),
218  static_cast<Scalar> (0.5)*(bounds[4]+bounds[5])};
219 
220  Scalar arg = extent/voxel_size;
221 
222  // Compute the number of tree levels
223  if ( arg > 1 )
224  tree_levels_ = static_cast<int> (std::ceil (std::log (arg)/std::log (2.0)) + 0.5);
225  else
226  tree_levels_ = 0;
227 
228  // Compute the number of octree levels and the bounds of the root
229  Scalar half_root_side = static_cast<Scalar> (0.5f*pow (2.0, tree_levels_)*voxel_size);
230 
231  // Determine the bounding box of the octree
232  bounds_[0] = center[0] - half_root_side;
233  bounds_[1] = center[0] + half_root_side;
234  bounds_[2] = center[1] - half_root_side;
235  bounds_[3] = center[1] + half_root_side;
236  bounds_[4] = center[2] - half_root_side;
237  bounds_[5] = center[2] + half_root_side;
238 
239  // Create and initialize the root
240  root_ = new Node ();
241  root_->setCenter (center);
243  root_->setParent (nullptr);
244  root_->computeRadius ();
245 }
246 
247 
248 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
251 {
252  // Make sure that the input point is within the octree bounds
253  if ( x < bounds_[0] || x > bounds_[1] ||
254  y < bounds_[2] || y > bounds_[3] ||
255  z < bounds_[4] || z > bounds_[5] )
256  {
257  return (nullptr);
258  }
259 
260  Node* node = root_;
261 
262  // Go down to the right leaf
263  for ( int l = 0 ; l < tree_levels_ ; ++l )
264  {
265  node->createChildren ();
266  const Scalar *c = node->getCenter ();
267  int id = 0;
268 
269  if ( x >= c[0] ) id |= 4;
270  if ( y >= c[1] ) id |= 2;
271  if ( z >= c[2] ) id |= 1;
272 
273  node = node->getChild (id);
274  }
275 
276  if ( !node->hasData () )
277  {
278  node->setData (node_data_creator_->create (node));
279  this->insertNeighbors (node);
280  full_leaves_.push_back (node);
281  }
282 
283  return (node);
284 }
285 
286 
287 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
290 {
291  Scalar offset = 0.5f*voxel_size_;
292  Scalar p[3] = {bounds_[0] + offset + static_cast<Scalar> (i)*voxel_size_,
293  bounds_[2] + offset + static_cast<Scalar> (j)*voxel_size_,
294  bounds_[4] + offset + static_cast<Scalar> (k)*voxel_size_};
295 
296  return (this->getFullLeaf (p[0], p[1], p[2]));
297 }
298 
299 
300 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
303 {
304  // Make sure that the input point is within the octree bounds
305  if ( x < bounds_[0] || x > bounds_[1] ||
306  y < bounds_[2] || y > bounds_[3] ||
307  z < bounds_[4] || z > bounds_[5] )
308  {
309  return (nullptr);
310  }
311 
312  Node* node = root_;
313 
314  // Go down to the right leaf
315  for ( int l = 0 ; l < tree_levels_ ; ++l )
316  {
317  if ( !node->hasChildren () )
318  return (nullptr);
319 
320  const Scalar *c = node->getCenter ();
321  int id = 0;
322 
323  if ( x >= c[0] ) id |= 4;
324  if ( y >= c[1] ) id |= 2;
325  if ( z >= c[2] ) id |= 1;
326 
327  node = node->getChild (id);
328  }
329 
330  if ( !node->hasData () )
331  return (nullptr);
332 
333  return (node);
334 }
335 
336 
337 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
339 {
340  const Scalar* c = node->getCenter ();
341  Scalar s = static_cast<Scalar> (0.5)*voxel_size_;
342  Node *neigh;
343 
344  neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
345  neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
346  neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
347  neigh = this->getFullLeaf (c[0]+s, c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
348  neigh = this->getFullLeaf (c[0]+s, c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh);
349  neigh = this->getFullLeaf (c[0]+s, c[1] , c[2]-s); 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]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
352  neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
353 
354  neigh = this->getFullLeaf (c[0] , c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
355  neigh = this->getFullLeaf (c[0] , c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
356  neigh = this->getFullLeaf (c[0] , c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
357  neigh = this->getFullLeaf (c[0] , c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
358 //neigh = this->getFullLeaf (c[0] , c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh);
359  neigh = this->getFullLeaf (c[0] , c[1] , c[2]-s); 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]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
362  neigh = this->getFullLeaf (c[0] , c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
363 
364  neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
365  neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
366  neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
367  neigh = this->getFullLeaf (c[0]-s, c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
368  neigh = this->getFullLeaf (c[0]-s, c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh);
369  neigh = this->getFullLeaf (c[0]-s, c[1] , c[2]-s); 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]-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 }
374 
375 } // namespace recognition
376 } // namespace pcl
377 
378 #endif /* SIMPLE_OCTREE_HPP_ */
379 
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
NodeDataCreator * node_data_creator_
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...
std::vector< Node * > full_leaves_
Node * getFullLeaf(int i, int j, int k)
Since the leaves are aligned in a rectilinear grid, each leaf has a unique id.