Point Cloud Library (PCL)  1.14.0-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
21 SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::Node ()
22 : data_ (nullptr),
23  parent_ (nullptr),
24  children_(nullptr)
25 {}
26 
27 
28 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
29 SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::~Node ()
30 {
31  this->deleteChildren ();
32  this->deleteData ();
33 }
34 
35 
36 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
37 SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::setCenter (const Scalar *c)
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
46 SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::setBounds (const Scalar *b)
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
58 SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::computeRadius ()
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
69 SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::createChildren ()
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
156 SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::deleteChildren ()
157 {
158  delete[] children_;
159  children_ = nullptr;
160 }
161 
162 
163 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
164 SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::deleteData ()
165 {
166  delete data_;
167  data_ = nullptr;
168 }
169 
170 
171 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
172 SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::makeNeighbors (Node* node)
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
183 SimpleOctree<NodeData, NodeDataCreator, Scalar>::SimpleOctree () = default;
184 
185 
186 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
187 SimpleOctree<NodeData, NodeDataCreator, Scalar>::~SimpleOctree ()
188 {
189  this->clear ();
190 }
191 
192 
193 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
194 SimpleOctree<NodeData, NodeDataCreator, Scalar>::clear ()
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);
242  root_->setBounds (bounds_);
243  root_->setParent (nullptr);
244  root_->computeRadius ();
245 }
246 
247 
248 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
249 typename SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node*
250 SimpleOctree<NodeData, NodeDataCreator, Scalar>::createLeaf (Scalar x, Scalar y, Scalar z)
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
288 typename SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node*
289 SimpleOctree<NodeData, NodeDataCreator, Scalar>::getFullLeaf (int i, int j, int k)
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
301 typename SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node*
302 SimpleOctree<NodeData, NodeDataCreator, Scalar>::getFullLeaf (Scalar x, Scalar y, Scalar z)
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
338 SimpleOctree<NodeData, NodeDataCreator, Scalar>::insertNeighbors (Node* node)
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 
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:172
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::Node::setData
void setData(const NodeData &src)
Definition: simple_octree.h:90
pcl::recognition::SimpleOctree::Node::getCenter
const Scalar * getCenter() const
Definition: simple_octree.h:75
pcl::recognition::SimpleOctree::node_data_creator_
NodeDataCreator * node_data_creator_
Definition: simple_octree.h:205
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:250
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:204
pcl::recognition::SimpleOctree::full_leaves_
std::vector< Node * > full_leaves_
Definition: simple_octree.h:204
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:289