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
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
161
162
163template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
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
183SimpleOctree<NodeData, NodeDataCreator, Scalar>::SimpleOctree () = default;
184
185
186template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
191
192
193template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
194SimpleOctree<NodeData, NodeDataCreator, Scalar>::clear ()
195{
196 delete root_;
197 root_ = nullptr;
198
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);
242 root_->setBounds (bounds_);
243 root_->setParent (nullptr);
244 root_->computeRadius ();
245}
246
247
248template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
249typename SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node*
250SimpleOctree<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
287template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
288typename SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node*
289SimpleOctree<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
300template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
301typename SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node*
302SimpleOctree<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
337template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
338SimpleOctree<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_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