Point Cloud Library (PCL)  1.7.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 #ifndef PCL_RECOGNITION_ORR_OCTREE_H_
47 #define PCL_RECOGNITION_ORR_OCTREE_H_
48 
49 #include "auxiliary.h"
50 #include <pcl/point_types.h>
51 #include <pcl/point_cloud.h>
52 #include <pcl/pcl_exports.h>
53 #include <cstdlib>
54 #include <ctime>
55 #include <vector>
56 #include <list>
57 #include <set>
58 
59 //#define PCL_REC_ORR_OCTREE_VERBOSE
60 
61 namespace pcl
62 {
63  namespace recognition
64  {
65  /** \brief That's a very specialized and simple octree class. That's the way it is intended to
66  * be, that's why no templates and stuff like this.
67  *
68  * \author Chavdar Papazov
69  * \ingroup recognition
70  */
71  class PCL_EXPORTS ORROctree
72  {
73  public:
77 
78  class Node
79  {
80  public:
81  class Data
82  {
83  public:
84  Data (int id_x, int id_y, int id_z, int lin_id, void* user_data = NULL)
85  : id_x_ (id_x),
86  id_y_ (id_y),
87  id_z_ (id_z),
88  lin_id_ (lin_id),
89  num_points_ (0),
90  user_data_ (user_data)
91  {
92  n_[0] = n_[1] = n_[2] = p_[0] = p_[1] = p_[2] = 0.0f;
93  }
94 
95  virtual~ Data (){}
96 
97  inline void
98  addToPoint (float x, float y, float z)
99  {
100  p_[0] += x; p_[1] += y; p_[2] += z;
101  ++num_points_;
102  }
103 
104  inline void
106  {
107  if ( num_points_ < 2 )
108  return;
109 
110  aux::mult3 (p_, 1.0f/static_cast<float> (num_points_));
111  num_points_ = 1;
112  }
113 
114  inline void
115  addToNormal (float x, float y, float z) { n_[0] += x; n_[1] += y; n_[2] += z;}
116 
117  inline const float*
118  getPoint () const { return p_;}
119 
120  inline float*
121  getPoint (){ return p_;}
122 
123  inline const float*
124  getNormal () const { return n_;}
125 
126  inline float*
127  getNormal (){ return n_;}
128 
129  inline void
130  get3dId (int id[3]) const
131  {
132  id[0] = id_x_;
133  id[1] = id_y_;
134  id[2] = id_z_;
135  }
136 
137  inline int
138  get3dIdX () const {return id_x_;}
139 
140  inline int
141  get3dIdY () const {return id_y_;}
142 
143  inline int
144  get3dIdZ () const {return id_z_;}
145 
146  inline int
147  getLinearId () const { return lin_id_;}
148 
149  inline void
150  setUserData (void* user_data){ user_data_ = user_data;}
151 
152  inline void*
153  getUserData () const { return user_data_;}
154 
155  inline void
156  insertNeighbor (Node* node){ neighbors_.insert (node);}
157 
158  inline const std::set<Node*>&
159  getNeighbors () const { return (neighbors_);}
160 
161  protected:
162  float n_[3], p_[3];
163  int id_x_, id_y_, id_z_, lin_id_, num_points_;
164  std::set<Node*> neighbors_;
165  void *user_data_;
166  };
167 
168  Node ()
169  : data_ (NULL),
170  parent_ (NULL),
171  children_(NULL)
172  {}
173 
174  virtual~ Node ()
175  {
176  this->deleteChildren ();
177  this->deleteData ();
178  }
179 
180  inline void
181  setCenter(const float *c) { center_[0] = c[0]; center_[1] = c[1]; center_[2] = c[2];}
182 
183  inline void
184  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];}
185 
186  inline void
187  setParent(Node* parent) { parent_ = parent;}
188 
189  inline void
190  setData(Node::Data* data) { data_ = data;}
191 
192  /** \brief Computes the "radius" of the node which is half the diagonal length. */
193  inline void
195  {
196  float v[3] = {0.5f*(bounds_[1]-bounds_[0]), 0.5f*(bounds_[3]-bounds_[2]), 0.5f*(bounds_[5]-bounds_[4])};
197  radius_ = static_cast<float> (aux::length3 (v));
198  }
199 
200  inline const float*
201  getCenter() const { return center_;}
202 
203  inline const float*
204  getBounds() const { return bounds_;}
205 
206  inline void
207  getBounds(float b[6]) const
208  {
209  memcpy (b, bounds_, 6*sizeof (float));
210  }
211 
212  inline Node*
213  getChild (int id) { return &children_[id];}
214 
215  inline Node*
216  getChildren () { return children_;}
217 
218  inline Node::Data*
219  getData (){ return data_;}
220 
221  inline const Node::Data*
222  getData () const { return data_;}
223 
224  inline void
225  setUserData (void* user_data){ data_->setUserData (user_data);}
226 
227  inline Node*
228  getParent (){ return parent_;}
229 
230  inline bool
231  hasData (){ return static_cast<bool> (data_);}
232 
233  inline bool
234  hasChildren (){ return static_cast<bool> (children_);}
235 
236  /** \brief Computes the "radius" of the node which is half the diagonal length. */
237  inline float
238  getRadius (){ return radius_;}
239 
240  bool
241  createChildren ();
242 
243  inline void
245  {
246  if ( children_ )
247  {
248  delete[] children_;
249  children_ = NULL;
250  }
251  }
252 
253  inline void
255  {
256  if ( data_ )
257  {
258  delete data_;
259  data_ = NULL;
260  }
261  }
262 
263  /** \brief Make this and 'node' neighbors by inserting each node in the others node neighbor set. Nothing happens
264  * of either of the nodes has no data. */
265  inline void
267  {
268  if ( !this->getData () || !node->getData () )
269  return;
270 
271  this->getData ()->insertNeighbor (node);
272  node->getData ()->insertNeighbor (this);
273  }
274 
275  protected:
277  float center_[3], bounds_[6], radius_;
278  Node *parent_, *children_;
279  };
280 
281  ORROctree ();
282  virtual ~ORROctree (){ this->clear ();}
283 
284  void
285  clear ();
286 
287  /** \brief Creates an octree which encloses 'points' and with leaf size equal to 'voxel_size'.
288  * 'enlarge_bounds' makes sure that no points from the input will lie on the octree boundary
289  * by enlarging the bounds by that factor. For example, enlarge_bounds = 1 means that the
290  * bounds will be enlarged by 100%. The default value is fine. */
291  void
292  build (const PointCloudIn& points, float voxel_size, const PointCloudN* normals = NULL, float enlarge_bounds = 0.00001f);
293 
294  /** \brief Creates an empty octree with bounds at least as large as the ones provided as input and with leaf
295  * size equal to 'voxel_size'. */
296  void
297  build (const float* bounds, float voxel_size);
298 
299  /** \brief Creates the leaf containing p = (x, y, z) and returns a pointer to it, however, only if p lies within
300  * the octree bounds! A more general version which allows p to be out of bounds is not implemented yet. The method
301  * returns NULL if p is not within the root bounds. If the leaf containing p already exists nothing happens and
302  * method just returns a pointer to the leaf. */
303  inline ORROctree::Node*
304  createLeaf (float x, float y, float z)
305  {
306  // Make sure that the input point is within the octree bounds
307  if ( x < bounds_[0] || x > bounds_[1] ||
308  y < bounds_[2] || y > bounds_[3] ||
309  z < bounds_[4] || z > bounds_[5] )
310  {
311  return (NULL);
312  }
313 
314  ORROctree::Node* node = root_;
315  const float *c;
316  int id;
317 
318  // Go down to the right leaf
319  for ( int l = 0 ; l < tree_levels_ ; ++l )
320  {
321  node->createChildren ();
322  c = node->getCenter ();
323  id = 0;
324 
325  if ( x >= c[0] ) id |= 4;
326  if ( y >= c[1] ) id |= 2;
327  if ( z >= c[2] ) id |= 1;
328 
329  node = node->getChild (id);
330  }
331 
332  if ( !node->getData () )
333  {
334  Node::Data* data = new Node::Data (
335  static_cast<int> ((node->getCenter ()[0] - bounds_[0])/voxel_size_),
336  static_cast<int> ((node->getCenter ()[1] - bounds_[2])/voxel_size_),
337  static_cast<int> ((node->getCenter ()[2] - bounds_[4])/voxel_size_),
338  static_cast<int> (full_leaves_.size ()));
339 
340  node->setData (data);
341  this->insertNeighbors (node);
342  full_leaves_.push_back (node);
343  }
344 
345  return (node);
346  }
347 
348  /** \brief This method returns a super set of the full leavess which are intersected by the sphere
349  * with radius 'radius' and centered at 'p'. Pointers to the intersected full leaves are saved in
350  * 'out'. The method computes a super set in the sense that in general not all leaves saved in 'out'
351  * are really intersected by the sphere. The intersection test is based on the leaf radius (since
352  * its faster than checking all leaf corners and sides), so we report more leaves than we should,
353  * but still, this is a fair approximation. */
354  void
355  getFullLeavesIntersectedBySphere (const float* p, float radius, std::list<ORROctree::Node*>& out) const;
356 
357  /** \brief Randomly chooses and returns a full leaf that is intersected by the sphere with center 'p'
358  * and 'radius'. Returns NULL if no leaf is intersected by that sphere. */
360  getRandomFullLeafOnSphere (const float* p, float radius) const;
361 
362  /** \brief Since the leaves are aligned in a rectilinear grid, each leaf has a unique id. The method returns the leaf
363  * with id [i, j, k] or NULL is no such leaf exists. */
365  getLeaf (int i, int j, int k)
366  {
367  float offset = 0.5f*voxel_size_;
368  float p[3] = {bounds_[0] + offset + static_cast<float> (i)*voxel_size_,
369  bounds_[2] + offset + static_cast<float> (j)*voxel_size_,
370  bounds_[4] + offset + static_cast<float> (k)*voxel_size_};
371 
372  return (this->getLeaf (p[0], p[1], p[2]));
373  }
374 
375  /** \brief Returns a pointer to the leaf containing p = (x, y, z) or NULL if no such leaf exists. */
376  inline ORROctree::Node*
377  getLeaf (float x, float y, float z)
378  {
379  // Make sure that the input point is within the octree bounds
380  if ( x < bounds_[0] || x > bounds_[1] ||
381  y < bounds_[2] || y > bounds_[3] ||
382  z < bounds_[4] || z > bounds_[5] )
383  {
384  return (NULL);
385  }
386 
387  ORROctree::Node* node = root_;
388  const float *c;
389  int id;
390 
391  // Go down to the right leaf
392  for ( int l = 0 ; l < tree_levels_ ; ++l )
393  {
394  if ( !node->hasChildren () )
395  return (NULL);
396 
397  c = node->getCenter ();
398  id = 0;
399 
400  if ( x >= c[0] ) id |= 4;
401  if ( y >= c[1] ) id |= 2;
402  if ( z >= c[2] ) id |= 1;
403 
404  node = node->getChild (id);
405  }
406 
407  return (node);
408  }
409 
410  /** \brief Deletes the branch 'node' is part of. */
411  void
412  deleteBranch (Node* node);
413 
414  /** \brief Returns a vector with all octree leaves which contain at least one point. */
415  inline std::vector<ORROctree::Node*>&
416  getFullLeaves () { return full_leaves_;}
417 
418  inline const std::vector<ORROctree::Node*>&
419  getFullLeaves () const { return full_leaves_;}
420 
421  void
422  getFullLeavesPoints (PointCloudOut& out) const;
423 
424  void
425  getNormalsOfFullLeaves (PointCloudN& out) const;
426 
427  inline ORROctree::Node*
428  getRoot (){ return root_;}
429 
430  inline const float*
431  getBounds () const
432  {
433  return (bounds_);
434  }
435 
436  inline void
437  getBounds (float b[6]) const
438  {
439  memcpy (b, bounds_, 6*sizeof (float));
440  }
441 
442  inline float
443  getVoxelSize () const { return voxel_size_;}
444 
445  inline void
447  {
448  const float* c = node->getCenter ();
449  float s = 0.5f*voxel_size_;
450  Node *neigh;
451 
452  neigh = this->getLeaf (c[0]+s, c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
453  neigh = this->getLeaf (c[0]+s, c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
454  neigh = this->getLeaf (c[0]+s, c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
455  neigh = this->getLeaf (c[0]+s, c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
456  neigh = this->getLeaf (c[0]+s, c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh);
457  neigh = this->getLeaf (c[0]+s, c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
458  neigh = this->getLeaf (c[0]+s, c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
459  neigh = this->getLeaf (c[0]+s, c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
460  neigh = this->getLeaf (c[0]+s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
461 
462  neigh = this->getLeaf (c[0] , c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
463  neigh = this->getLeaf (c[0] , c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
464  neigh = this->getLeaf (c[0] , c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
465  neigh = this->getLeaf (c[0] , c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
466  //neigh = this->getLeaf (c[0] , c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh);
467  neigh = this->getLeaf (c[0] , c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
468  neigh = this->getLeaf (c[0] , c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
469  neigh = this->getLeaf (c[0] , c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
470  neigh = this->getLeaf (c[0] , c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
471 
472  neigh = this->getLeaf (c[0]-s, c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
473  neigh = this->getLeaf (c[0]-s, c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
474  neigh = this->getLeaf (c[0]-s, c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
475  neigh = this->getLeaf (c[0]-s, c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
476  neigh = this->getLeaf (c[0]-s, c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh);
477  neigh = this->getLeaf (c[0]-s, c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
478  neigh = this->getLeaf (c[0]-s, c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
479  neigh = this->getLeaf (c[0]-s, c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
480  neigh = this->getLeaf (c[0]-s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
481  }
482 
483  protected:
484  float voxel_size_, bounds_[6];
487  std::vector<Node*> full_leaves_;
488  };
489  } // namespace recognition
490 } // namespace pcl
491 
492 #endif /* PCL_RECOGNITION_ORR_OCTREE_H_ */
const float * getNormal() const
Definition: orr_octree.h:124
float getRadius()
Computes the &quot;radius&quot; of the node which is half the diagonal length.
Definition: orr_octree.h:238
const Node::Data * getData() const
Definition: orr_octree.h:222
void setUserData(void *user_data)
Definition: orr_octree.h:225
void mult3(T *v, T scalar)
v = scalar*v.
Definition: auxiliary.h:216
void computeRadius()
Computes the &quot;radius&quot; of the node which is half the diagonal length.
Definition: orr_octree.h:194
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:304
void getBounds(float b[6]) const
Definition: orr_octree.h:207
const std::set< Node * > & getNeighbors() const
Definition: orr_octree.h:159
const float * getCenter() const
Definition: orr_octree.h:201
float getVoxelSize() const
Definition: orr_octree.h:443
std::vector< ORROctree::Node * > & getFullLeaves()
Returns a vector with all octree leaves which contain at least one point.
Definition: orr_octree.h:416
void setCenter(const float *c)
Definition: orr_octree.h:181
const float * getBounds() const
Definition: orr_octree.h:431
pcl::PointCloud< pcl::PointXYZ > PointCloudOut
Definition: orr_octree.h:75
Data(int id_x, int id_y, int id_z, int lin_id, void *user_data=NULL)
Definition: orr_octree.h:84
const float * getBounds() const
Definition: orr_octree.h:204
ORROctree::Node * getRoot()
Definition: orr_octree.h:428
pcl::PointCloud< pcl::PointXYZ > PointCloudIn
Definition: orr_octree.h:74
void setData(Node::Data *data)
Definition: orr_octree.h:190
void insertNeighbors(Node *node)
Definition: orr_octree.h:446
std::vector< Node * > full_leaves_
Definition: orr_octree.h:487
void setBounds(const float *b)
Definition: orr_octree.h:184
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:365
T length3(const T v[3])
Returns the length of v.
Definition: auxiliary.h:180
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:377
void getBounds(float b[6]) const
Definition: orr_octree.h:437
pcl::PointCloud< pcl::Normal > PointCloudN
Definition: orr_octree.h:76
void makeNeighbors(Node *node)
Make this and &#39;node&#39; neighbors by inserting each node in the others node neighbor set...
Definition: orr_octree.h:266
const std::vector< ORROctree::Node * > & getFullLeaves() const
Definition: orr_octree.h:419
void addToNormal(float x, float y, float z)
Definition: orr_octree.h:115
void setParent(Node *parent)
Definition: orr_octree.h:187
That&#39;s a very specialized and simple octree class.
Definition: orr_octree.h:71
void addToPoint(float x, float y, float z)
Definition: orr_octree.h:98