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 
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.