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