Point Cloud Library (PCL)  1.10.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 #include <algorithm>
12 #include <cmath>
13 
14 //===============================================================================================================================
15 
16 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
18 : data_ (nullptr),
19  parent_ (nullptr),
20  children_(nullptr)
21 {}
22 
23 //===============================================================================================================================
24 
25 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
27 {
28  this->deleteChildren ();
29  this->deleteData ();
30 }
31 
32 //===============================================================================================================================
33 
34 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
36 {
37  center_[0] = c[0];
38  center_[1] = c[1];
39  center_[2] = c[2];
40 }
41 
42 //===============================================================================================================================
43 
44 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
46 {
47  bounds_[0] = b[0];
48  bounds_[1] = b[1];
49  bounds_[2] = b[2];
50  bounds_[3] = b[3];
51  bounds_[4] = b[4];
52  bounds_[5] = b[5];
53 }
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 
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 
157 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
159 {
160  if ( children_ )
161  {
162  delete[] children_;
163  children_ = nullptr;
164  }
165 }
166 
167 //===============================================================================================================================
168 
169 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
171 {
172  if ( data_ )
173  {
174  delete data_;
175  data_ = nullptr;
176  }
177 }
178 
179 //===============================================================================================================================
180 
181 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
183 {
184  if ( !this->hasData () || !node->hasData () )
185  return;
186 
187  this->full_leaf_neighbors_.insert (node);
188  node->full_leaf_neighbors_.insert (this);
189 }
190 
191 //===============================================================================================================================
192 
193 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
195 : tree_levels_ (0),
196  root_ (nullptr)
197 {
198 }
199 
200 //===============================================================================================================================
201 
202 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
204 {
205  this->clear ();
206 }
207 
208 //===============================================================================================================================
209 
210 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
212 {
213  if ( root_ )
214  {
215  delete root_;
216  root_ = nullptr;
217  }
218 
219  full_leaves_.clear();
220 }
221 
222 //===============================================================================================================================
223 
224 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
226  NodeDataCreator* node_data_creator)
227 {
228  if ( voxel_size <= 0 )
229  return;
230 
231  this->clear();
232 
233  voxel_size_ = voxel_size;
234  node_data_creator_ = node_data_creator;
235 
236  Scalar extent = std::max (std::max (bounds[1]-bounds[0], bounds[3]-bounds[2]), bounds[5]-bounds[4]);
237  Scalar center[3] = {static_cast<Scalar> (0.5)*(bounds[0]+bounds[1]),
238  static_cast<Scalar> (0.5)*(bounds[2]+bounds[3]),
239  static_cast<Scalar> (0.5)*(bounds[4]+bounds[5])};
240 
241  Scalar arg = extent/voxel_size;
242 
243  // Compute the number of tree levels
244  if ( arg > 1 )
245  tree_levels_ = static_cast<int> (std::ceil (std::log (arg)/std::log (2.0)) + 0.5);
246  else
247  tree_levels_ = 0;
248 
249  // Compute the number of octree levels and the bounds of the root
250  Scalar half_root_side = static_cast<Scalar> (0.5f*pow (2.0, tree_levels_)*voxel_size);
251 
252  // Determine the bounding box of the octree
253  bounds_[0] = center[0] - half_root_side;
254  bounds_[1] = center[0] + half_root_side;
255  bounds_[2] = center[1] - half_root_side;
256  bounds_[3] = center[1] + half_root_side;
257  bounds_[4] = center[2] - half_root_side;
258  bounds_[5] = center[2] + half_root_side;
259 
260  // Create and initialize the root
261  root_ = new Node ();
262  root_->setCenter (center);
263  root_->setBounds (bounds_);
264  root_->setParent (nullptr);
265  root_->computeRadius ();
266 }
267 
268 //===============================================================================================================================
269 
270 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
273 {
274  // Make sure that the input point is within the octree bounds
275  if ( x < bounds_[0] || x > bounds_[1] ||
276  y < bounds_[2] || y > bounds_[3] ||
277  z < bounds_[4] || z > bounds_[5] )
278  {
279  return (nullptr);
280  }
281 
282  Node* node = root_;
283 
284  // Go down to the right leaf
285  for ( int l = 0 ; l < tree_levels_ ; ++l )
286  {
287  node->createChildren ();
288  const Scalar *c = node->getCenter ();
289  int id = 0;
290 
291  if ( x >= c[0] ) id |= 4;
292  if ( y >= c[1] ) id |= 2;
293  if ( z >= c[2] ) id |= 1;
294 
295  node = node->getChild (id);
296  }
297 
298  if ( !node->hasData () )
299  {
300  node->setData (node_data_creator_->create (node));
301  this->insertNeighbors (node);
302  full_leaves_.push_back (node);
303  }
304 
305  return (node);
306 }
307 
308 //===============================================================================================================================
309 
310 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
313 {
314  Scalar offset = 0.5f*voxel_size_;
315  Scalar p[3] = {bounds_[0] + offset + static_cast<Scalar> (i)*voxel_size_,
316  bounds_[2] + offset + static_cast<Scalar> (j)*voxel_size_,
317  bounds_[4] + offset + static_cast<Scalar> (k)*voxel_size_};
318 
319  return (this->getFullLeaf (p[0], p[1], p[2]));
320 }
321 
322 //===============================================================================================================================
323 
324 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
327 {
328  // Make sure that the input point is within the octree bounds
329  if ( x < bounds_[0] || x > bounds_[1] ||
330  y < bounds_[2] || y > bounds_[3] ||
331  z < bounds_[4] || z > bounds_[5] )
332  {
333  return (nullptr);
334  }
335 
336  Node* node = root_;
337 
338  // Go down to the right leaf
339  for ( int l = 0 ; l < tree_levels_ ; ++l )
340  {
341  if ( !node->hasChildren () )
342  return (nullptr);
343 
344  const Scalar *c = node->getCenter ();
345  int id = 0;
346 
347  if ( x >= c[0] ) id |= 4;
348  if ( y >= c[1] ) id |= 2;
349  if ( z >= c[2] ) id |= 1;
350 
351  node = node->getChild (id);
352  }
353 
354  if ( !node->hasData () )
355  return (nullptr);
356 
357  return (node);
358 }
359 
360 //===============================================================================================================================
361 
362 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
364 {
365  const Scalar* c = node->getCenter ();
366  Scalar s = static_cast<Scalar> (0.5)*voxel_size_;
367  Node *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  neigh = this->getFullLeaf (c[0] , c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
380  neigh = this->getFullLeaf (c[0] , c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
381  neigh = this->getFullLeaf (c[0] , c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
382  neigh = this->getFullLeaf (c[0] , c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
383 //neigh = this->getFullLeaf (c[0] , c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh);
384  neigh = this->getFullLeaf (c[0] , c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
385  neigh = this->getFullLeaf (c[0] , c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
386  neigh = this->getFullLeaf (c[0] , c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
387  neigh = this->getFullLeaf (c[0] , c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
388 
389  neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
390  neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
391  neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
392  neigh = this->getFullLeaf (c[0]-s, c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
393  neigh = this->getFullLeaf (c[0]-s, c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh);
394  neigh = this->getFullLeaf (c[0]-s, c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
395  neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
396  neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
397  neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
398 }
399 
400 //===============================================================================================================================
401 
402 #endif /* SIMPLE_OCTREE_HPP_ */
pcl::recognition::SimpleOctree::Node::Node
Node()
Definition: simple_octree.hpp:17
pcl::recognition::SimpleOctree::Node::createChildren
bool createChildren()
Definition: simple_octree.hpp:70
pcl::recognition::SimpleOctree::bounds_
Scalar bounds_[6]
Definition: simple_octree.h:201
pcl::recognition::SimpleOctree::Node::deleteChildren
void deleteChildren()
Definition: simple_octree.hpp:158
pcl::recognition::SimpleOctree::getFullLeaf
Node * getFullLeaf(int i, int j, int k)
Since the leaves are aligned in a rectilinear grid, each leaf has a unique id.
Definition: simple_octree.hpp:312
pcl::recognition::SimpleOctree
Definition: simple_octree.h:58
pcl::recognition::SimpleOctree::Node::hasData
bool hasData()
Definition: simple_octree.h:105
pcl::recognition::SimpleOctree::insertNeighbors
void insertNeighbors(Node *node)
Definition: simple_octree.hpp:363
pcl::recognition::SimpleOctree::clear
void clear()
Definition: simple_octree.hpp:211
pcl::recognition::SimpleOctree::SimpleOctree
SimpleOctree()
Definition: simple_octree.hpp:194
pcl::recognition::SimpleOctree::Node::getCenter
const Scalar * getCenter() const
Definition: simple_octree.h:75
pcl::recognition::SimpleOctree::Node
Definition: simple_octree.h:61
pcl::recognition::SimpleOctree::tree_levels_
int tree_levels_
Definition: simple_octree.h:202
pcl::recognition::SimpleOctree::Node::full_leaf_neighbors_
std::set< Node * > full_leaf_neighbors_
Definition: simple_octree.h:145
pcl::recognition::SimpleOctree::Node::setBounds
void setBounds(const Scalar *b)
Definition: simple_octree.hpp:45
pcl::recognition::SimpleOctree::createLeaf
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...
Definition: simple_octree.hpp:272
pcl::recognition::SimpleOctree::Node::makeNeighbors
void makeNeighbors(Node *node)
Make this and 'node' neighbors by inserting each node in the others node neighbor set.
Definition: simple_octree.hpp:182
pcl::recognition::SimpleOctree::Node::computeRadius
void computeRadius()
Computes the "radius" of the node which is half the diagonal length.
Definition: simple_octree.hpp:58
pcl::recognition::SimpleOctree::root_
Node * root_
Definition: simple_octree.h:203
pcl::recognition::SimpleOctree::build
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...
Definition: simple_octree.hpp:225
pcl::recognition::SimpleOctree::~SimpleOctree
virtual ~SimpleOctree()
Definition: simple_octree.hpp:203
pcl::recognition::SimpleOctree::Node::deleteData
void deleteData()
Definition: simple_octree.hpp:170
pcl::recognition::SimpleOctree::Node::setCenter
void setCenter(const Scalar *c)
Definition: simple_octree.hpp:35