Point Cloud Library (PCL) 1.15.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);
149 }
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
184
185
186template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
191
192
193template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
195{
196 delete root_;
197 root_ = nullptr;
199 full_leaves_.clear();
200}
201
202
203template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
204SimpleOctree<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);
245}
246
247
248template<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
287template<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
300template<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
337template<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)
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.