Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2012, Willow Garage, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of Willow Garage, Inc. nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * 00037 */ 00038 00039 /* 00040 * simple_octree.h 00041 * 00042 * Created on: Mar 11, 2013 00043 * Author: papazov 00044 */ 00045 00046 #ifndef SIMPLE_OCTREE_H_ 00047 #define SIMPLE_OCTREE_H_ 00048 00049 #include <pcl/pcl_exports.h> 00050 #include <set> 00051 00052 namespace pcl 00053 { 00054 namespace recognition 00055 { 00056 template<typename NodeData, typename NodeDataCreator, typename Scalar = float> 00057 class PCL_EXPORTS SimpleOctree 00058 { 00059 public: 00060 class Node 00061 { 00062 public: 00063 Node (); 00064 00065 virtual~ Node (); 00066 00067 inline void 00068 setCenter (const Scalar *c); 00069 00070 inline void 00071 setBounds (const Scalar *b); 00072 00073 inline const Scalar* 00074 getCenter () const { return center_;} 00075 00076 inline const Scalar* 00077 getBounds () const { return bounds_;} 00078 00079 inline void 00080 getBounds (Scalar b[6]) const { memcpy (b, bounds_, 6*sizeof (Scalar));} 00081 00082 inline Node* 00083 getChild (int id) { return &children_[id];} 00084 00085 inline Node* 00086 getChildren () { return children_;} 00087 00088 inline void 00089 setData (const NodeData& src){ *data_ = src;} 00090 00091 inline NodeData& 00092 getData (){ return *data_;} 00093 00094 inline const NodeData& 00095 getData () const { return *data_;} 00096 00097 inline Node* 00098 getParent (){ return parent_;} 00099 00100 inline float 00101 getRadius () const { return radius_;} 00102 00103 inline bool 00104 hasData (){ return static_cast<bool> (data_);} 00105 00106 inline bool 00107 hasChildren (){ return static_cast<bool> (children_);} 00108 00109 inline const std::set<Node*>& 00110 getNeighbors () const { return (full_leaf_neighbors_);} 00111 00112 inline void 00113 deleteChildren (); 00114 00115 inline void 00116 deleteData (); 00117 00118 friend class SimpleOctree; 00119 00120 protected: 00121 void 00122 setData (NodeData* data){ if ( data_ ) delete data_; data_ = data;} 00123 00124 inline bool 00125 createChildren (); 00126 00127 /** \brief Make this and 'node' neighbors by inserting each node in the others node neighbor set. Nothing happens 00128 * of either of the nodes has no data. */ 00129 inline void 00130 makeNeighbors (Node* node); 00131 00132 inline void 00133 setParent (Node* parent){ parent_ = parent;} 00134 00135 /** \brief Computes the "radius" of the node which is half the diagonal length. */ 00136 inline void 00137 computeRadius (); 00138 00139 protected: 00140 NodeData *data_; 00141 Scalar center_[3], bounds_[6]; 00142 Node *parent_, *children_; 00143 Scalar radius_; 00144 std::set<Node*> full_leaf_neighbors_; 00145 }; 00146 00147 public: 00148 SimpleOctree (); 00149 00150 virtual ~SimpleOctree (); 00151 00152 void 00153 clear (); 00154 00155 /** \brief Creates an empty octree with bounds at least as large as the ones provided as input and with leaf 00156 * size equal to 'voxel_size'. */ 00157 void 00158 build (const Scalar* bounds, Scalar voxel_size, NodeDataCreator* node_data_creator); 00159 00160 /** \brief Creates the leaf containing p = (x, y, z) and returns a pointer to it, however, only if p lies within 00161 * the octree bounds! A more general version which allows p to be out of bounds is not implemented yet. The method 00162 * returns NULL if p is not within the root bounds. If the leaf containing p already exists nothing happens and 00163 * method just returns a pointer to the leaf. Note that for a new created leaf, the method also creates its data 00164 * object. */ 00165 inline Node* 00166 createLeaf (Scalar x, Scalar y, Scalar z); 00167 00168 /** \brief Since the leaves are aligned in a rectilinear grid, each leaf has a unique id. The method returns the full 00169 * leaf, i.e., the one having a data object, with id [i, j, k] or NULL is no such leaf exists. */ 00170 inline Node* 00171 getFullLeaf (int i, int j, int k); 00172 00173 /** \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. */ 00174 inline Node* 00175 getFullLeaf (Scalar x, Scalar y, Scalar z); 00176 00177 inline std::vector<Node*>& 00178 getFullLeaves () { return full_leaves_;} 00179 00180 inline const std::vector<Node*>& 00181 getFullLeaves () const { return full_leaves_;} 00182 00183 inline Node* 00184 getRoot (){ return root_;} 00185 00186 inline const Scalar* 00187 getBounds () const { return (bounds_);} 00188 00189 inline void 00190 getBounds (Scalar b[6]) const { memcpy (b, bounds_, 6*sizeof (Scalar));} 00191 00192 inline Scalar 00193 getVoxelSize () const { return voxel_size_;} 00194 00195 protected: 00196 inline void 00197 insertNeighbors (Node* node); 00198 00199 protected: 00200 Scalar voxel_size_, bounds_[6]; 00201 int tree_levels_; 00202 Node* root_; 00203 std::vector<Node*> full_leaves_; 00204 NodeDataCreator* node_data_creator_; 00205 }; 00206 } // namespace recognition 00207 } // namespace pcl 00208 00209 #include <pcl/recognition/impl/ransac_based/simple_octree.hpp> 00210 00211 #endif /* SIMPLE_OCTREE_H_ */