Point Cloud Library (PCL) 1.13.0
Loading...
Searching...
No Matches
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
14namespace pcl
15{
16
17namespace recognition
18{
19
20template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
22: data_ (nullptr),
23 parent_ (nullptr),
24 children_(nullptr)
25{}
26
27
28template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
30{
31 this->deleteChildren ();
32 this->deleteData ();
33}
34
35
36template<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
45template<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
57template<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}
67
68template<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);
150
151 return (true);
152}
153
155template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
157{
158 delete[] children_;
159 children_ = nullptr;
160}
161
162
163template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
165{
166 delete data_;
167 data_ = nullptr;
168}
169
170
171template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
173{
174 if ( !this->hasData () || !node->hasData () )
175 return;
177 this->full_leaf_neighbors_.insert (node);
178 node->full_leaf_neighbors_.insert (this);
179}
180
181
182template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
188
189
190template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
195
196
197template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
199{
200 delete root_;
201 root_ = nullptr;
202
203 full_leaves_.clear();
204}
205
206
207template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
208SimpleOctree<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
252template<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
291template<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
304template<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
341template<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)
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.