Point Cloud Library (PCL) 1.12.0
orr_octree.h
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2012, Willow Garage, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of Willow Garage, Inc. nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 *
37 */
38
39/*
40 * orr_octree.h
41 *
42 * Created on: Oct 23, 2012
43 * Author: papazov
44 */
45
46#pragma once
47
48#include "auxiliary.h"
49#include <pcl/point_types.h>
50#include <pcl/point_cloud.h>
51#include <pcl/pcl_exports.h>
52#include <vector>
53#include <list>
54#include <set>
55
56//#define PCL_REC_ORR_OCTREE_VERBOSE
57
58namespace pcl
59{
60 namespace recognition
61 {
62 /** \brief That's a very specialized and simple octree class. That's the way it is intended to
63 * be, that's why no templates and stuff like this.
64 *
65 * \author Chavdar Papazov
66 * \ingroup recognition
67 */
69 {
70 public:
74
75 class Node
76 {
77 public:
78 class Data
79 {
80 public:
81 Data (int id_x, int id_y, int id_z, int lin_id, void* user_data = nullptr)
82 : id_x_ (id_x),
83 id_y_ (id_y),
84 id_z_ (id_z),
85 lin_id_ (lin_id),
86 num_points_ (0),
87 user_data_ (user_data)
88 {
89 n_[0] = n_[1] = n_[2] = p_[0] = p_[1] = p_[2] = 0.0f;
90 }
91
92 virtual~ Data (){}
93
94 inline void
95 addToPoint (float x, float y, float z)
96 {
97 p_[0] += x; p_[1] += y; p_[2] += z;
98 ++num_points_;
99 }
100
101 inline void
103 {
104 if ( num_points_ < 2 )
105 return;
106
107 aux::mult3 (p_, 1.0f/static_cast<float> (num_points_));
108 num_points_ = 1;
109 }
110
111 inline void
112 addToNormal (float x, float y, float z) { n_[0] += x; n_[1] += y; n_[2] += z;}
113
114 inline const float*
115 getPoint () const { return p_;}
116
117 inline float*
118 getPoint (){ return p_;}
119
120 inline const float*
121 getNormal () const { return n_;}
122
123 inline float*
124 getNormal (){ return n_;}
125
126 inline void
127 get3dId (int id[3]) const
128 {
129 id[0] = id_x_;
130 id[1] = id_y_;
131 id[2] = id_z_;
132 }
133
134 inline int
135 get3dIdX () const {return id_x_;}
136
137 inline int
138 get3dIdY () const {return id_y_;}
139
140 inline int
141 get3dIdZ () const {return id_z_;}
142
143 inline int
144 getLinearId () const { return lin_id_;}
145
146 inline void
147 setUserData (void* user_data){ user_data_ = user_data;}
148
149 inline void*
150 getUserData () const { return user_data_;}
151
152 inline void
153 insertNeighbor (Node* node){ neighbors_.insert (node);}
154
155 inline const std::set<Node*>&
156 getNeighbors () const { return (neighbors_);}
157
158 protected:
159 float n_[3], p_[3];
160 int id_x_, id_y_, id_z_, lin_id_, num_points_;
161 std::set<Node*> neighbors_;
163 };
164
166 : data_ (nullptr),
167 parent_ (nullptr),
168 children_(nullptr)
169 {}
170
171 virtual~ Node ()
172 {
173 this->deleteChildren ();
174 this->deleteData ();
175 }
176
177 inline void
178 setCenter(const float *c) { center_[0] = c[0]; center_[1] = c[1]; center_[2] = c[2];}
179
180 inline void
181 setBounds(const float *b) { bounds_[0] = b[0]; bounds_[1] = b[1]; bounds_[2] = b[2]; bounds_[3] = b[3]; bounds_[4] = b[4]; bounds_[5] = b[5];}
182
183 inline void
184 setParent(Node* parent) { parent_ = parent;}
185
186 inline void
187 setData(Node::Data* data) { data_ = data;}
188
189 /** \brief Computes the "radius" of the node which is half the diagonal length. */
190 inline void
192 {
193 float v[3] = {0.5f*(bounds_[1]-bounds_[0]), 0.5f*(bounds_[3]-bounds_[2]), 0.5f*(bounds_[5]-bounds_[4])};
194 radius_ = static_cast<float> (aux::length3 (v));
195 }
196
197 inline const float*
198 getCenter() const { return center_;}
199
200 inline const float*
201 getBounds() const { return bounds_;}
202
203 inline void
204 getBounds(float b[6]) const
205 {
206 memcpy (b, bounds_, 6*sizeof (float));
207 }
208
209 inline Node*
210 getChild (int id) { return &children_[id];}
211
212 inline Node*
213 getChildren () { return children_;}
214
215 inline Node::Data*
216 getData (){ return data_;}
217
218 inline const Node::Data*
219 getData () const { return data_;}
220
221 inline void
222 setUserData (void* user_data){ data_->setUserData (user_data);}
223
224 inline Node*
225 getParent (){ return parent_;}
226
227 inline bool
228 hasData (){ return static_cast<bool> (data_);}
229
230 inline bool
231 hasChildren (){ return static_cast<bool> (children_);}
232
233 /** \brief Computes the "radius" of the node which is half the diagonal length. */
234 inline float
235 getRadius () const{ return radius_;}
236
237 bool
239
240 inline void
242 {
243 delete[] children_;
244 children_ = nullptr;
245 }
246
247 inline void
249 {
250 delete data_;
251 data_ = nullptr;
252 }
253
254 /** \brief Make this and 'node' neighbors by inserting each node in the others node neighbor set. Nothing happens
255 * of either of the nodes has no data. */
256 inline void
258 {
259 if ( !this->getData () || !node->getData () )
260 return;
261
262 this->getData ()->insertNeighbor (node);
263 node->getData ()->insertNeighbor (this);
264 }
265
266 protected:
268 float center_[3], bounds_[6], radius_;
269 Node *parent_, *children_;
270 };
271
273 virtual ~ORROctree (){ this->clear ();}
274
275 void
277
278 /** \brief Creates an octree which encloses 'points' and with leaf size equal to 'voxel_size'.
279 * 'enlarge_bounds' makes sure that no points from the input will lie on the octree boundary
280 * by enlarging the bounds by that factor. For example, enlarge_bounds = 1 means that the
281 * bounds will be enlarged by 100%. The default value is fine. */
282 void
283 build (const PointCloudIn& points, float voxel_size, const PointCloudN* normals = nullptr, float enlarge_bounds = 0.00001f);
284
285 /** \brief Creates an empty octree with bounds at least as large as the ones provided as input and with leaf
286 * size equal to 'voxel_size'. */
287 void
288 build (const float* bounds, float voxel_size);
289
290 /** \brief Creates the leaf containing p = (x, y, z) and returns a pointer to it, however, only if p lies within
291 * the octree bounds! A more general version which allows p to be out of bounds is not implemented yet. The method
292 * returns NULL if p is not within the root bounds. If the leaf containing p already exists nothing happens and
293 * method just returns a pointer to the leaf. */
294 inline ORROctree::Node*
295 createLeaf (float x, float y, float z)
296 {
297 // Make sure that the input point is within the octree bounds
298 if ( x < bounds_[0] || x > bounds_[1] ||
299 y < bounds_[2] || y > bounds_[3] ||
300 z < bounds_[4] || z > bounds_[5] )
301 {
302 return (nullptr);
303 }
304
305 ORROctree::Node* node = root_;
306 const float *c;
307 int id;
308
309 // Go down to the right leaf
310 for ( int l = 0 ; l < tree_levels_ ; ++l )
311 {
312 node->createChildren ();
313 c = node->getCenter ();
314 id = 0;
315
316 if ( x >= c[0] ) id |= 4;
317 if ( y >= c[1] ) id |= 2;
318 if ( z >= c[2] ) id |= 1;
319
320 node = node->getChild (id);
321 }
322
323 if ( !node->getData () )
324 {
325 Node::Data* data = new Node::Data (
326 static_cast<int> ((node->getCenter ()[0] - bounds_[0])/voxel_size_),
327 static_cast<int> ((node->getCenter ()[1] - bounds_[2])/voxel_size_),
328 static_cast<int> ((node->getCenter ()[2] - bounds_[4])/voxel_size_),
329 static_cast<int> (full_leaves_.size ()));
330
331 node->setData (data);
332 this->insertNeighbors (node);
333 full_leaves_.push_back (node);
334 }
335
336 return (node);
337 }
338
339 /** \brief This method returns a super set of the full leavess which are intersected by the sphere
340 * with radius 'radius' and centered at 'p'. Pointers to the intersected full leaves are saved in
341 * 'out'. The method computes a super set in the sense that in general not all leaves saved in 'out'
342 * are really intersected by the sphere. The intersection test is based on the leaf radius (since
343 * its faster than checking all leaf corners and sides), so we report more leaves than we should,
344 * but still, this is a fair approximation. */
345 void
346 getFullLeavesIntersectedBySphere (const float* p, float radius, std::list<ORROctree::Node*>& out) const;
347
348 /** \brief Randomly chooses and returns a full leaf that is intersected by the sphere with center 'p'
349 * and 'radius'. Returns NULL if no leaf is intersected by that sphere. */
351 getRandomFullLeafOnSphere (const float* p, float radius) const;
352
353 /** \brief Since the leaves are aligned in a rectilinear grid, each leaf has a unique id. The method returns the leaf
354 * with id [i, j, k] or NULL is no such leaf exists. */
356 getLeaf (int i, int j, int k)
357 {
358 float offset = 0.5f*voxel_size_;
359 float p[3] = {bounds_[0] + offset + static_cast<float> (i)*voxel_size_,
360 bounds_[2] + offset + static_cast<float> (j)*voxel_size_,
361 bounds_[4] + offset + static_cast<float> (k)*voxel_size_};
362
363 return (this->getLeaf (p[0], p[1], p[2]));
364 }
365
366 /** \brief Returns a pointer to the leaf containing p = (x, y, z) or NULL if no such leaf exists. */
367 inline ORROctree::Node*
368 getLeaf (float x, float y, float z)
369 {
370 // Make sure that the input point is within the octree bounds
371 if ( x < bounds_[0] || x > bounds_[1] ||
372 y < bounds_[2] || y > bounds_[3] ||
373 z < bounds_[4] || z > bounds_[5] )
374 {
375 return (nullptr);
376 }
377
378 ORROctree::Node* node = root_;
379 const float *c;
380 int id;
381
382 // Go down to the right leaf
383 for ( int l = 0 ; l < tree_levels_ ; ++l )
384 {
385 if ( !node->hasChildren () )
386 return (nullptr);
387
388 c = node->getCenter ();
389 id = 0;
390
391 if ( x >= c[0] ) id |= 4;
392 if ( y >= c[1] ) id |= 2;
393 if ( z >= c[2] ) id |= 1;
394
395 node = node->getChild (id);
396 }
397
398 return (node);
399 }
400
401 /** \brief Deletes the branch 'node' is part of. */
402 void
404
405 /** \brief Returns a vector with all octree leaves which contain at least one point. */
406 inline std::vector<ORROctree::Node*>&
407 getFullLeaves () { return full_leaves_;}
408
409 inline const std::vector<ORROctree::Node*>&
410 getFullLeaves () const { return full_leaves_;}
411
412 void
414
415 void
417
418 inline ORROctree::Node*
419 getRoot (){ return root_;}
420
421 inline const float*
422 getBounds () const
423 {
424 return (bounds_);
425 }
426
427 inline void
428 getBounds (float b[6]) const
429 {
430 memcpy (b, bounds_, 6*sizeof (float));
431 }
432
433 inline float
434 getVoxelSize () const { return voxel_size_;}
435
436 inline void
438 {
439 const float* c = node->getCenter ();
440 float s = 0.5f*voxel_size_;
441 Node *neigh;
442
443 neigh = this->getLeaf (c[0]+s, c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
444 neigh = this->getLeaf (c[0]+s, c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
445 neigh = this->getLeaf (c[0]+s, c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
446 neigh = this->getLeaf (c[0]+s, c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
447 neigh = this->getLeaf (c[0]+s, c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh);
448 neigh = this->getLeaf (c[0]+s, c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
449 neigh = this->getLeaf (c[0]+s, c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
450 neigh = this->getLeaf (c[0]+s, c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
451 neigh = this->getLeaf (c[0]+s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
452
453 neigh = this->getLeaf (c[0] , c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
454 neigh = this->getLeaf (c[0] , c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
455 neigh = this->getLeaf (c[0] , c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
456 neigh = this->getLeaf (c[0] , c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
457 //neigh = this->getLeaf (c[0] , c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh);
458 neigh = this->getLeaf (c[0] , c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
459 neigh = this->getLeaf (c[0] , c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
460 neigh = this->getLeaf (c[0] , c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
461 neigh = this->getLeaf (c[0] , c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
462
463 neigh = this->getLeaf (c[0]-s, c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
464 neigh = this->getLeaf (c[0]-s, c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
465 neigh = this->getLeaf (c[0]-s, c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
466 neigh = this->getLeaf (c[0]-s, c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
467 neigh = this->getLeaf (c[0]-s, c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh);
468 neigh = this->getLeaf (c[0]-s, c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
469 neigh = this->getLeaf (c[0]-s, c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
470 neigh = this->getLeaf (c[0]-s, c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
471 neigh = this->getLeaf (c[0]-s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
472 }
473
474 protected:
475 float voxel_size_, bounds_[6];
478 std::vector<Node*> full_leaves_;
479 };
480 } // namespace recognition
481} // namespace pcl
void addToPoint(float x, float y, float z)
Definition: orr_octree.h:95
const std::set< Node * > & getNeighbors() const
Definition: orr_octree.h:156
Data(int id_x, int id_y, int id_z, int lin_id, void *user_data=nullptr)
Definition: orr_octree.h:81
void addToNormal(float x, float y, float z)
Definition: orr_octree.h:112
void setBounds(const float *b)
Definition: orr_octree.h:181
void setCenter(const float *c)
Definition: orr_octree.h:178
const float * getBounds() const
Definition: orr_octree.h:201
float getRadius() const
Computes the "radius" of the node which is half the diagonal length.
Definition: orr_octree.h:235
const Node::Data * getData() const
Definition: orr_octree.h:219
void getBounds(float b[6]) const
Definition: orr_octree.h:204
void setUserData(void *user_data)
Definition: orr_octree.h:222
const float * getCenter() const
Definition: orr_octree.h:198
void setParent(Node *parent)
Definition: orr_octree.h:184
void computeRadius()
Computes the "radius" of the node which is half the diagonal length.
Definition: orr_octree.h:191
void setData(Node::Data *data)
Definition: orr_octree.h:187
void makeNeighbors(Node *node)
Make this and 'node' neighbors by inserting each node in the others node neighbor set.
Definition: orr_octree.h:257
That's a very specialized and simple octree class.
Definition: orr_octree.h:69
void build(const PointCloudIn &points, float voxel_size, const PointCloudN *normals=nullptr, float enlarge_bounds=0.00001f)
Creates an octree which encloses 'points' and with leaf size equal to 'voxel_size'.
const float * getBounds() const
Definition: orr_octree.h:422
void deleteBranch(Node *node)
Deletes the branch 'node' is part of.
ORROctree::Node * getRandomFullLeafOnSphere(const float *p, float radius) const
Randomly chooses and returns a full leaf that is intersected by the sphere with center 'p' and 'radiu...
ORROctree::Node * getRoot()
Definition: orr_octree.h:419
void getNormalsOfFullLeaves(PointCloudN &out) const
ORROctree::Node * getLeaf(int i, int j, int k)
Since the leaves are aligned in a rectilinear grid, each leaf has a unique id.
Definition: orr_octree.h:356
ORROctree::Node * getLeaf(float x, float y, float z)
Returns a pointer to the leaf containing p = (x, y, z) or NULL if no such leaf exists.
Definition: orr_octree.h:368
ORROctree::Node * createLeaf(float x, float y, float z)
Creates the leaf containing p = (x, y, z) and returns a pointer to it, however, only if p lies within...
Definition: orr_octree.h:295
void build(const float *bounds, float voxel_size)
Creates an empty octree with bounds at least as large as the ones provided as input and with leaf siz...
void getFullLeavesPoints(PointCloudOut &out) const
float getVoxelSize() const
Definition: orr_octree.h:434
void getFullLeavesIntersectedBySphere(const float *p, float radius, std::list< ORROctree::Node * > &out) const
This method returns a super set of the full leavess which are intersected by the sphere with radius '...
void getBounds(float b[6]) const
Definition: orr_octree.h:428
void insertNeighbors(Node *node)
Definition: orr_octree.h:437
std::vector< ORROctree::Node * > & getFullLeaves()
Returns a vector with all octree leaves which contain at least one point.
Definition: orr_octree.h:407
std::vector< Node * > full_leaves_
Definition: orr_octree.h:478
const std::vector< ORROctree::Node * > & getFullLeaves() const
Definition: orr_octree.h:410
Defines all the PCL implemented PointT point type structures.
T length3(const T v[3])
Returns the length of v.
Definition: auxiliary.h:185
void mult3(T *v, T scalar)
v = scalar*v.
Definition: auxiliary.h:221
#define PCL_EXPORTS
Definition: pcl_macros.h:323