Point Cloud Library (PCL)  1.7.0
simple_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  * simple_octree.h
41  *
42  * Created on: Mar 11, 2013
43  * Author: papazov
44  */
45 
46 #ifndef SIMPLE_OCTREE_H_
47 #define SIMPLE_OCTREE_H_
48 
49 #include <pcl/pcl_exports.h>
50 #include <set>
51 
52 namespace pcl
53 {
54  namespace recognition
55  {
56  template<typename NodeData, typename NodeDataCreator, typename Scalar = float>
57  class PCL_EXPORTS SimpleOctree
58  {
59  public:
60  class Node
61  {
62  public:
63  Node ();
64 
65  virtual~ Node ();
66 
67  inline void
68  setCenter (const Scalar *c);
69 
70  inline void
71  setBounds (const Scalar *b);
72 
73  inline const Scalar*
74  getCenter () const { return center_;}
75 
76  inline const Scalar*
77  getBounds () const { return bounds_;}
78 
79  inline void
80  getBounds (Scalar b[6]) const { memcpy (b, bounds_, 6*sizeof (Scalar));}
81 
82  inline Node*
83  getChild (int id) { return &children_[id];}
84 
85  inline Node*
86  getChildren () { return children_;}
87 
88  inline void
89  setData (const NodeData& src){ *data_ = src;}
90 
91  inline NodeData&
92  getData (){ return *data_;}
93 
94  inline const NodeData&
95  getData () const { return *data_;}
96 
97  inline Node*
98  getParent (){ return parent_;}
99 
100  inline float
101  getRadius () const { return radius_;}
102 
103  inline bool
104  hasData (){ return static_cast<bool> (data_);}
105 
106  inline bool
107  hasChildren (){ return static_cast<bool> (children_);}
108 
109  inline const std::set<Node*>&
110  getNeighbors () const { return (full_leaf_neighbors_);}
111 
112  inline void
113  deleteChildren ();
114 
115  inline void
116  deleteData ();
117 
118  friend class SimpleOctree;
119 
120  protected:
121  void
122  setData (NodeData* data){ if ( data_ ) delete data_; data_ = data;}
123 
124  inline bool
125  createChildren ();
126 
127  /** \brief Make this and 'node' neighbors by inserting each node in the others node neighbor set. Nothing happens
128  * of either of the nodes has no data. */
129  inline void
130  makeNeighbors (Node* node);
131 
132  inline void
133  setParent (Node* parent){ parent_ = parent;}
134 
135  /** \brief Computes the "radius" of the node which is half the diagonal length. */
136  inline void
137  computeRadius ();
138 
139  protected:
140  NodeData *data_;
141  Scalar center_[3], bounds_[6];
142  Node *parent_, *children_;
143  Scalar radius_;
144  std::set<Node*> full_leaf_neighbors_;
145  };
146 
147  public:
148  SimpleOctree ();
149 
150  virtual ~SimpleOctree ();
151 
152  void
153  clear ();
154 
155  /** \brief Creates an empty octree with bounds at least as large as the ones provided as input and with leaf
156  * size equal to 'voxel_size'. */
157  void
158  build (const Scalar* bounds, Scalar voxel_size, NodeDataCreator* node_data_creator);
159 
160  /** \brief Creates the leaf containing p = (x, y, z) and returns a pointer to it, however, only if p lies within
161  * the octree bounds! A more general version which allows p to be out of bounds is not implemented yet. The method
162  * returns NULL if p is not within the root bounds. If the leaf containing p already exists nothing happens and
163  * method just returns a pointer to the leaf. Note that for a new created leaf, the method also creates its data
164  * object. */
165  inline Node*
166  createLeaf (Scalar x, Scalar y, Scalar z);
167 
168  /** \brief Since the leaves are aligned in a rectilinear grid, each leaf has a unique id. The method returns the full
169  * leaf, i.e., the one having a data object, with id [i, j, k] or NULL is no such leaf exists. */
170  inline Node*
171  getFullLeaf (int i, int j, int k);
172 
173  /** \brief Returns a pointer to the full leaf, i.e., one having a data pbject, containing p = (x, y, z) or NULL if no such leaf exists. */
174  inline Node*
175  getFullLeaf (Scalar x, Scalar y, Scalar z);
176 
177  inline std::vector<Node*>&
178  getFullLeaves () { return full_leaves_;}
179 
180  inline const std::vector<Node*>&
181  getFullLeaves () const { return full_leaves_;}
182 
183  inline Node*
184  getRoot (){ return root_;}
185 
186  inline const Scalar*
187  getBounds () const { return (bounds_);}
188 
189  inline void
190  getBounds (Scalar b[6]) const { memcpy (b, bounds_, 6*sizeof (Scalar));}
191 
192  inline Scalar
193  getVoxelSize () const { return voxel_size_;}
194 
195  protected:
196  inline void
197  insertNeighbors (Node* node);
198 
199  protected:
200  Scalar voxel_size_, bounds_[6];
202  Node* root_;
203  std::vector<Node*> full_leaves_;
204  NodeDataCreator* node_data_creator_;
205  };
206  } // namespace recognition
207 } // namespace pcl
208 
209 #include <pcl/recognition/impl/ransac_based/simple_octree.hpp>
210 
211 #endif /* SIMPLE_OCTREE_H_ */
const std::vector< Node * > & getFullLeaves() const
const Scalar * getCenter() const
Definition: simple_octree.h:74
void setData(const NodeData &src)
Definition: simple_octree.h:89
const Scalar * getBounds() const
const NodeData & getData() const
Definition: simple_octree.h:95
NodeDataCreator * node_data_creator_
void getBounds(Scalar b[6]) const
Definition: simple_octree.h:80
const Scalar * getBounds() const
Definition: simple_octree.h:77
std::vector< Node * > full_leaves_
std::vector< Node * > & getFullLeaves()
const std::set< Node * > & getNeighbors() const
void getBounds(Scalar b[6]) const