Point Cloud Library (PCL)  1.14.1-dev
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 
58 namespace 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 
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 () = default;
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_{0}, id_y_{0}, id_z_{0}, lin_id_{0}, num_points_{0};
161  std::set<Node*> neighbors_;
162  void *user_data_{nullptr};
163  };
164 
165  Node () = default;
166 
167  virtual~ Node ()
168  {
169  this->deleteChildren ();
170  this->deleteData ();
171  }
172 
173  inline void
174  setCenter(const float *c) { center_[0] = c[0]; center_[1] = c[1]; center_[2] = c[2];}
175 
176  inline void
177  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];}
178 
179  inline void
180  setParent(Node* parent) { parent_ = parent;}
181 
182  inline void
183  setData(Node::Data* data) { data_ = data;}
184 
185  /** \brief Computes the "radius" of the node which is half the diagonal length. */
186  inline void
188  {
189  float v[3] = {0.5f*(bounds_[1]-bounds_[0]), 0.5f*(bounds_[3]-bounds_[2]), 0.5f*(bounds_[5]-bounds_[4])};
190  radius_ = static_cast<float> (aux::length3 (v));
191  }
192 
193  inline const float*
194  getCenter() const { return center_;}
195 
196  inline const float*
197  getBounds() const { return bounds_;}
198 
199  inline void
200  getBounds(float b[6]) const
201  {
202  std::copy(bounds_, bounds_ + 6, b);
203  }
204 
205  inline Node*
206  getChild (int id) { return &children_[id];}
207 
208  inline Node*
209  getChildren () { return children_;}
210 
211  inline Node::Data*
212  getData (){ return data_;}
213 
214  inline const Node::Data*
215  getData () const { return data_;}
216 
217  inline void
218  setUserData (void* user_data){ data_->setUserData (user_data);}
219 
220  inline Node*
221  getParent (){ return parent_;}
222 
223  inline bool
224  hasData (){ return static_cast<bool> (data_);}
225 
226  inline bool
227  hasChildren (){ return static_cast<bool> (children_);}
228 
229  /** \brief Computes the "radius" of the node which is half the diagonal length. */
230  inline float
231  getRadius () const{ return radius_;}
232 
233  bool
235 
236  inline void
238  {
239  delete[] children_;
240  children_ = nullptr;
241  }
242 
243  inline void
245  {
246  delete data_;
247  data_ = nullptr;
248  }
249 
250  /** \brief Make this and 'node' neighbors by inserting each node in the others node neighbor set. Nothing happens
251  * of either of the nodes has no data. */
252  inline void
254  {
255  if ( !this->getData () || !node->getData () )
256  return;
257 
258  this->getData ()->insertNeighbor (node);
259  node->getData ()->insertNeighbor (this);
260  }
261 
262  protected:
263  Node::Data *data_{nullptr};
264  float center_[3]{}, bounds_[6]{}, radius_{0.0f};
265  Node *parent_{nullptr}, *children_{nullptr};
266  };
267 
269  virtual ~ORROctree (){ this->clear ();}
270 
271  void
272  clear ();
273 
274  /** \brief Creates an octree which encloses 'points' and with leaf size equal to 'voxel_size'.
275  * 'enlarge_bounds' makes sure that no points from the input will lie on the octree boundary
276  * by enlarging the bounds by that factor. For example, enlarge_bounds = 1 means that the
277  * bounds will be enlarged by 100%. The default value is fine. */
278  void
279  build (const PointCloudIn& points, float voxel_size, const PointCloudN* normals = nullptr, float enlarge_bounds = 0.00001f);
280 
281  /** \brief Creates an empty octree with bounds at least as large as the ones provided as input and with leaf
282  * size equal to 'voxel_size'. */
283  void
284  build (const float* bounds, float voxel_size);
285 
286  /** \brief Creates the leaf containing p = (x, y, z) and returns a pointer to it, however, only if p lies within
287  * the octree bounds! A more general version which allows p to be out of bounds is not implemented yet. The method
288  * returns NULL if p is not within the root bounds. If the leaf containing p already exists nothing happens and
289  * method just returns a pointer to the leaf. */
290  inline ORROctree::Node*
291  createLeaf (float x, float y, float z)
292  {
293  // Make sure that the input point is within the octree bounds
294  if ( x < bounds_[0] || x > bounds_[1] ||
295  y < bounds_[2] || y > bounds_[3] ||
296  z < bounds_[4] || z > bounds_[5] )
297  {
298  return (nullptr);
299  }
300 
301  ORROctree::Node* node = root_;
302  const float *c;
303  int id;
304 
305  // Go down to the right leaf
306  for ( int l = 0 ; l < tree_levels_ ; ++l )
307  {
308  node->createChildren ();
309  c = node->getCenter ();
310  id = 0;
311 
312  if ( x >= c[0] ) id |= 4;
313  if ( y >= c[1] ) id |= 2;
314  if ( z >= c[2] ) id |= 1;
315 
316  node = node->getChild (id);
317  }
318 
319  if ( !node->getData () )
320  {
321  auto* data = new Node::Data (
322  static_cast<int> ((node->getCenter ()[0] - bounds_[0])/voxel_size_),
323  static_cast<int> ((node->getCenter ()[1] - bounds_[2])/voxel_size_),
324  static_cast<int> ((node->getCenter ()[2] - bounds_[4])/voxel_size_),
325  static_cast<int> (full_leaves_.size ()));
326 
327  node->setData (data);
328  this->insertNeighbors (node);
329  full_leaves_.push_back (node);
330  }
331 
332  return (node);
333  }
334 
335  /** \brief This method returns a super set of the full leavess which are intersected by the sphere
336  * with radius 'radius' and centered at 'p'. Pointers to the intersected full leaves are saved in
337  * 'out'. The method computes a super set in the sense that in general not all leaves saved in 'out'
338  * are really intersected by the sphere. The intersection test is based on the leaf radius (since
339  * its faster than checking all leaf corners and sides), so we report more leaves than we should,
340  * but still, this is a fair approximation. */
341  void
342  getFullLeavesIntersectedBySphere (const float* p, float radius, std::list<ORROctree::Node*>& out) const;
343 
344  /** \brief Randomly chooses and returns a full leaf that is intersected by the sphere with center 'p'
345  * and 'radius'. Returns NULL if no leaf is intersected by that sphere. */
347  getRandomFullLeafOnSphere (const float* p, float radius) const;
348 
349  /** \brief Since the leaves are aligned in a rectilinear grid, each leaf has a unique id. The method returns the leaf
350  * with id [i, j, k] or NULL is no such leaf exists. */
352  getLeaf (int i, int j, int k)
353  {
354  float offset = 0.5f*voxel_size_;
355  float p[3] = {bounds_[0] + offset + static_cast<float> (i)*voxel_size_,
356  bounds_[2] + offset + static_cast<float> (j)*voxel_size_,
357  bounds_[4] + offset + static_cast<float> (k)*voxel_size_};
358 
359  return (this->getLeaf (p[0], p[1], p[2]));
360  }
361 
362  /** \brief Returns a pointer to the leaf containing p = (x, y, z) or NULL if no such leaf exists. */
363  inline ORROctree::Node*
364  getLeaf (float x, float y, float z)
365  {
366  // Make sure that the input point is within the octree bounds
367  if ( x < bounds_[0] || x > bounds_[1] ||
368  y < bounds_[2] || y > bounds_[3] ||
369  z < bounds_[4] || z > bounds_[5] )
370  {
371  return (nullptr);
372  }
373 
374  ORROctree::Node* node = root_;
375  const float *c;
376  int id;
377 
378  // Go down to the right leaf
379  for ( int l = 0 ; l < tree_levels_ ; ++l )
380  {
381  if ( !node->hasChildren () )
382  return (nullptr);
383 
384  c = node->getCenter ();
385  id = 0;
386 
387  if ( x >= c[0] ) id |= 4;
388  if ( y >= c[1] ) id |= 2;
389  if ( z >= c[2] ) id |= 1;
390 
391  node = node->getChild (id);
392  }
393 
394  return (node);
395  }
396 
397  /** \brief Deletes the branch 'node' is part of. */
398  void
400 
401  /** \brief Returns a vector with all octree leaves which contain at least one point. */
402  inline std::vector<ORROctree::Node*>&
403  getFullLeaves () { return full_leaves_;}
404 
405  inline const std::vector<ORROctree::Node*>&
406  getFullLeaves () const { return full_leaves_;}
407 
408  void
410 
411  void
413 
414  inline ORROctree::Node*
415  getRoot (){ return root_;}
416 
417  inline const float*
418  getBounds () const
419  {
420  return (bounds_);
421  }
422 
423  inline void
424  getBounds (float b[6]) const
425  {
426  std::copy(bounds_, bounds_ + 6, b);
427  }
428 
429  inline float
430  getVoxelSize () const { return voxel_size_;}
431 
432  inline void
434  {
435  const float* c = node->getCenter ();
436  float s = 0.5f*voxel_size_;
437  Node *neigh;
438 
439  neigh = this->getLeaf (c[0]+s, c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
440  neigh = this->getLeaf (c[0]+s, c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
441  neigh = this->getLeaf (c[0]+s, c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
442  neigh = this->getLeaf (c[0]+s, c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
443  neigh = this->getLeaf (c[0]+s, c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh);
444  neigh = this->getLeaf (c[0]+s, c[1] , c[2]-s); 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]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
447  neigh = this->getLeaf (c[0]+s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
448 
449  neigh = this->getLeaf (c[0] , c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
450  neigh = this->getLeaf (c[0] , c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
451  neigh = this->getLeaf (c[0] , c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
452  neigh = this->getLeaf (c[0] , c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
453  //neigh = this->getLeaf (c[0] , c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh);
454  neigh = this->getLeaf (c[0] , c[1] , c[2]-s); 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]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
457  neigh = this->getLeaf (c[0] , c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
458 
459  neigh = this->getLeaf (c[0]-s, c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
460  neigh = this->getLeaf (c[0]-s, c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
461  neigh = this->getLeaf (c[0]-s, c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
462  neigh = this->getLeaf (c[0]-s, c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
463  neigh = this->getLeaf (c[0]-s, c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh);
464  neigh = this->getLeaf (c[0]-s, c[1] , c[2]-s); 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]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh);
467  neigh = this->getLeaf (c[0]-s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
468  }
469 
470  protected:
471  float voxel_size_{-1.0}, bounds_[6];
472  int tree_levels_{-1};
473  Node* root_{nullptr};
474  std::vector<Node*> full_leaves_;
475  };
476  } // namespace recognition
477 } // namespace pcl
const std::set< Node * > & getNeighbors() const
Definition: orr_octree.h:156
void addToPoint(float x, float y, float z)
Definition: orr_octree.h:95
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:177
const float * getBounds() const
Definition: orr_octree.h:197
void setCenter(const float *c)
Definition: orr_octree.h:174
float getRadius() const
Computes the "radius" of the node which is half the diagonal length.
Definition: orr_octree.h:231
void getBounds(float b[6]) const
Definition: orr_octree.h:200
void setUserData(void *user_data)
Definition: orr_octree.h:218
void setParent(Node *parent)
Definition: orr_octree.h:180
void computeRadius()
Computes the "radius" of the node which is half the diagonal length.
Definition: orr_octree.h:187
const float * getCenter() const
Definition: orr_octree.h:194
const Node::Data * getData() const
Definition: orr_octree.h:215
void setData(Node::Data *data)
Definition: orr_octree.h:183
void makeNeighbors(Node *node)
Make this and 'node' neighbors by inserting each node in the others node neighbor set.
Definition: orr_octree.h:253
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'.
ORROctree::Node * getRoot()
Definition: orr_octree.h:415
void deleteBranch(Node *node)
Deletes the branch 'node' is part of.
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:364
void getNormalsOfFullLeaves(PointCloudN &out) const
const float * getBounds() const
Definition: orr_octree.h:418
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...
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:291
const std::vector< ORROctree::Node * > & getFullLeaves() const
Definition: orr_octree.h:406
void getFullLeavesPoints(PointCloudOut &out) const
float getVoxelSize() const
Definition: orr_octree.h:430
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 '...
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:352
std::vector< ORROctree::Node * > & getFullLeaves()
Returns a vector with all octree leaves which contain at least one point.
Definition: orr_octree.h:403
void getBounds(float b[6]) const
Definition: orr_octree.h:424
void insertNeighbors(Node *node)
Definition: orr_octree.h:433
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...
std::vector< Node * > full_leaves_
Definition: orr_octree.h:474
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