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 * bvh.h 00041 * 00042 * Created on: Mar 7, 2013 00043 * Author: papazov 00044 */ 00045 00046 #ifndef PCL_RECOGNITION_BVH_H_ 00047 #define PCL_RECOGNITION_BVH_H_ 00048 00049 #include <pcl/pcl_exports.h> 00050 #include <cstring> 00051 #include <algorithm> 00052 #include <vector> 00053 #include <list> 00054 00055 namespace pcl 00056 { 00057 namespace recognition 00058 { 00059 /** \brief This class is an implementation of bounding volume hierarchies. Use the build method to construct 00060 * the data structure. To use the class, construct an std::vector of pointers to BVH::BoundedObject objects 00061 * and pass it to the build method. BVH::BoundedObject is a template class, so you can save user-defined data 00062 * in it. 00063 * 00064 * The tree is built such that each leaf contains exactly one object. */ 00065 template<class UserData> 00066 class PCL_EXPORTS BVH 00067 { 00068 public: 00069 class BoundedObject 00070 { 00071 public: 00072 BoundedObject (const UserData& data) 00073 : data_ (data) 00074 { 00075 } 00076 00077 virtual ~BoundedObject () 00078 { 00079 } 00080 00081 /** \brief This method is for std::sort. */ 00082 inline static bool 00083 compareCentroidsXCoordinates (const BoundedObject* a, const BoundedObject* b) 00084 { 00085 return static_cast<bool> (a->getCentroid ()[0] < b->getCentroid ()[0]); 00086 } 00087 00088 float* 00089 getBounds () 00090 { 00091 return (bounds_); 00092 } 00093 00094 float* 00095 getCentroid () 00096 { 00097 return (centroid_); 00098 } 00099 00100 const float* 00101 getCentroid () const 00102 { 00103 return (centroid_); 00104 } 00105 00106 UserData& 00107 getData () 00108 { 00109 return (data_); 00110 } 00111 00112 protected: 00113 /** These are the bounds of the object.*/ 00114 float bounds_[6]; 00115 /** This is the centroid. */ 00116 float centroid_[3]; 00117 /** This is the user-defined data object. */ 00118 UserData data_; 00119 }; 00120 00121 protected: 00122 class Node 00123 { 00124 public: 00125 /** \brief 'sorted_objects' is a sorted vector of bounded objects. It has to be sorted in ascending order according 00126 * to the objects' x-coordinates. The constructor recursively calls itself with the right 'first_id' and 'last_id' 00127 * and with the same vector 'sorted_objects'. */ 00128 Node (std::vector<BoundedObject*>& sorted_objects, int first_id, int last_id) 00129 { 00130 // Initialize the bounds of the node 00131 memcpy (bounds_, sorted_objects[first_id]->getBounds (), 6*sizeof (float)); 00132 00133 // Expand the bounds of the node 00134 for ( int i = first_id + 1 ; i <= last_id ; ++i ) 00135 aux::expandBoundingBox(bounds_, sorted_objects[i]->getBounds()); 00136 00137 // Shall we create children? 00138 if ( first_id != last_id ) 00139 { 00140 // Division by 2 00141 int mid_id = (first_id + last_id) >> 1; 00142 children_[0] = new Node(sorted_objects, first_id, mid_id); 00143 children_[1] = new Node(sorted_objects, mid_id + 1, last_id); 00144 } 00145 else 00146 { 00147 // We reached a leaf 00148 object_ = sorted_objects[first_id]; 00149 children_[0] = children_[1] = 0; 00150 } 00151 } 00152 00153 virtual ~Node () 00154 { 00155 if ( children_[0] ) 00156 { 00157 delete children_[0]; 00158 delete children_[1]; 00159 } 00160 } 00161 00162 bool 00163 hasChildren () const 00164 { 00165 return static_cast<bool>(children_[0]); 00166 } 00167 00168 Node* 00169 getLeftChild () 00170 { 00171 return children_[0]; 00172 } 00173 00174 Node* 00175 getRightChild () 00176 { 00177 return children_[1]; 00178 } 00179 00180 BoundedObject* 00181 getObject () 00182 { 00183 return object_; 00184 } 00185 00186 bool 00187 isLeaf () const 00188 { 00189 return !static_cast<bool>(children_[0]); 00190 } 00191 00192 /** \brief Returns true if 'box' intersects or touches (with a side or a vertex) this node. */ 00193 inline bool 00194 intersect(const float box[6]) const 00195 { 00196 if ( box[1] < bounds_[0] || box[3] < bounds_[2] || box[5] < bounds_[4] || 00197 box[0] > bounds_[1] || box[2] > bounds_[3] || box[4] > bounds_[5] ) 00198 return false; 00199 00200 return true; 00201 } 00202 00203 /** \brief Computes and returns the volume of the bounding box of this node. */ 00204 double 00205 computeBoundingBoxVolume() const 00206 { 00207 return (bounds_[1] - bounds_[0]) * (bounds_[3] - bounds_[2]) * (bounds_[5] - bounds_[4]); 00208 } 00209 00210 friend class BVH; 00211 00212 protected: 00213 float bounds_[6]; 00214 Node* children_[2]; 00215 BoundedObject* object_; 00216 }; 00217 00218 public: 00219 BVH() 00220 : root_ (0), 00221 sorted_objects_ (0) 00222 { 00223 } 00224 00225 virtual ~BVH() 00226 { 00227 this->clear (); 00228 } 00229 00230 /** \brief Creates the tree. No need to call clear, it's called within the method. 'objects' is a vector of 00231 * pointers to bounded objects which have to have valid bounds and centroids. Use the getData method of 00232 * BoundedObject to retrieve the user-defined data saved in the object. Note that vector will be sorted within 00233 * the method! 00234 * 00235 * The tree is built such that each leaf contains exactly one object. */ 00236 void 00237 build(std::vector<BoundedObject*>& objects) 00238 { 00239 this->clear(); 00240 00241 if ( objects.size () == 0 ) 00242 return; 00243 00244 sorted_objects_ = &objects; 00245 00246 // Now sort the objects according to the x-coordinates of their centroids 00247 std::sort (objects.begin (), objects.end (), BoundedObject::compareCentroidsXCoordinates); 00248 00249 // Create the root -> it recursively creates the children nodes until each leaf contains exactly one object 00250 root_ = new Node (objects, 0, static_cast<int> (objects.size () - 1)); 00251 } 00252 00253 /** \brief Frees the memory allocated by this object. After that, you have to call build to use the tree again. */ 00254 void 00255 clear() 00256 { 00257 if ( root_ ) 00258 { 00259 delete root_; 00260 root_ = 0; 00261 } 00262 } 00263 00264 inline const std::vector<BoundedObject*>* 00265 getInputObjects () const 00266 { 00267 return (sorted_objects_); 00268 } 00269 00270 /** \brief Pushes back in 'intersected_objects' the bounded objects intersected by the input 'box' and returns true. 00271 * Returns false if no objects are intersected. */ 00272 inline bool 00273 intersect(const float box[6], std::list<BoundedObject*>& intersected_objects) const 00274 { 00275 if ( !root_ ) 00276 return false; 00277 00278 bool got_intersection = false; 00279 00280 // Start the intersection process at the root 00281 std::list<Node*> working_list; 00282 working_list.push_back (root_); 00283 00284 while ( !working_list.empty () ) 00285 { 00286 Node* node = working_list.front (); 00287 working_list.pop_front (); 00288 00289 // Is 'node' intersected by the box? 00290 if ( node->intersect (box) ) 00291 { 00292 // We have to check the children of the intersected 'node' 00293 if ( node->hasChildren () ) 00294 { 00295 working_list.push_back (node->getLeftChild ()); 00296 working_list.push_back (node->getRightChild ()); 00297 } 00298 else // 'node' is a leaf -> save it's object in the output list 00299 { 00300 intersected_objects.push_back (node->getObject ()); 00301 got_intersection = true; 00302 } 00303 } 00304 } 00305 00306 return (got_intersection); 00307 } 00308 00309 protected: 00310 Node* root_; 00311 std::vector<BoundedObject*>* sorted_objects_; 00312 }; 00313 } // namespace recognition 00314 } // namespace pcl 00315 00316 #endif /* PCL_RECOGNITION_BVH_H_ */