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