Point Cloud Library (PCL) 1.12.1
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
26
27
28template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
34
35
36template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
37SimpleOctree<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
45template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
46SimpleOctree<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
57template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
58SimpleOctree<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
68template<typename NodeData, typename NodeDataCreator, typename Scalar> inline bool
69SimpleOctree<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
155template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
156SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::deleteChildren ()
157{
158 delete[] children_;
159 children_ = nullptr;
160}
161
162
163template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
164SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::deleteData ()
165{
166 delete data_;
167 data_ = nullptr;
168}
169
170
171template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
172SimpleOctree<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
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
198SimpleOctree<NodeData, NodeDataCreator, Scalar>::clear ()
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
253typename SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node*
254SimpleOctree<NodeData, NodeDataCreator, Scalar>::createLeaf (Scalar x, Scalar y, Scalar z)
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
292typename SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node*
293SimpleOctree<NodeData, NodeDataCreator, Scalar>::getFullLeaf (int i, int j, int k)
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
305typename SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node*
306SimpleOctree<NodeData, NodeDataCreator, Scalar>::getFullLeaf (Scalar x, Scalar y, Scalar z)
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
342SimpleOctree<NodeData, NodeDataCreator, Scalar>::insertNeighbors (Node* node)
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
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_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:254
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:208
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:293