Point Cloud Library (PCL)  1.7.0
bvh.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  * bvh.h
41  *
42  * Created on: Mar 7, 2013
43  * Author: papazov
44  */
45 
46 #ifndef PCL_RECOGNITION_BVH_H_
47 #define PCL_RECOGNITION_BVH_H_
48 
49 #include <pcl/pcl_exports.h>
50 #include <cstring>
51 #include <algorithm>
52 #include <vector>
53 #include <list>
54 
55 namespace pcl
56 {
57  namespace recognition
58  {
59  /** \brief This class is an implementation of bounding volume hierarchies. Use the build method to construct
60  * the data structure. To use the class, construct an std::vector of pointers to BVH::BoundedObject objects
61  * and pass it to the build method. BVH::BoundedObject is a template class, so you can save user-defined data
62  * in it.
63  *
64  * The tree is built such that each leaf contains exactly one object. */
65  template<class UserData>
66  class PCL_EXPORTS BVH
67  {
68  public:
70  {
71  public:
72  BoundedObject (const UserData& data)
73  : data_ (data)
74  {
75  }
76 
77  virtual ~BoundedObject ()
78  {
79  }
80 
81  /** \brief This method is for std::sort. */
82  inline static bool
83  compareCentroidsXCoordinates (const BoundedObject* a, const BoundedObject* b)
84  {
85  return static_cast<bool> (a->getCentroid ()[0] < b->getCentroid ()[0]);
86  }
87 
88  float*
89  getBounds ()
90  {
91  return (bounds_);
92  }
93 
94  float*
95  getCentroid ()
96  {
97  return (centroid_);
98  }
99 
100  const float*
101  getCentroid () const
102  {
103  return (centroid_);
104  }
105 
106  UserData&
107  getData ()
108  {
109  return (data_);
110  }
111 
112  protected:
113  /** These are the bounds of the object.*/
114  float bounds_[6];
115  /** This is the centroid. */
116  float centroid_[3];
117  /** This is the user-defined data object. */
118  UserData data_;
119  };
120 
121  protected:
122  class Node
123  {
124  public:
125  /** \brief 'sorted_objects' is a sorted vector of bounded objects. It has to be sorted in ascending order according
126  * to the objects' x-coordinates. The constructor recursively calls itself with the right 'first_id' and 'last_id'
127  * and with the same vector 'sorted_objects'. */
128  Node (std::vector<BoundedObject*>& sorted_objects, int first_id, int last_id)
129  {
130  // Initialize the bounds of the node
131  memcpy (bounds_, sorted_objects[first_id]->getBounds (), 6*sizeof (float));
132 
133  // Expand the bounds of the node
134  for ( int i = first_id + 1 ; i <= last_id ; ++i )
135  aux::expandBoundingBox(bounds_, sorted_objects[i]->getBounds());
136 
137  // Shall we create children?
138  if ( first_id != last_id )
139  {
140  // Division by 2
141  int mid_id = (first_id + last_id) >> 1;
142  children_[0] = new Node(sorted_objects, first_id, mid_id);
143  children_[1] = new Node(sorted_objects, mid_id + 1, last_id);
144  }
145  else
146  {
147  // We reached a leaf
148  object_ = sorted_objects[first_id];
149  children_[0] = children_[1] = 0;
150  }
151  }
152 
153  virtual ~Node ()
154  {
155  if ( children_[0] )
156  {
157  delete children_[0];
158  delete children_[1];
159  }
160  }
161 
162  bool
163  hasChildren () const
164  {
165  return static_cast<bool>(children_[0]);
166  }
167 
168  Node*
169  getLeftChild ()
170  {
171  return children_[0];
172  }
173 
174  Node*
175  getRightChild ()
176  {
177  return children_[1];
178  }
179 
181  getObject ()
182  {
183  return object_;
184  }
185 
186  bool
187  isLeaf () const
188  {
189  return !static_cast<bool>(children_[0]);
190  }
191 
192  /** \brief Returns true if 'box' intersects or touches (with a side or a vertex) this node. */
193  inline bool
194  intersect(const float box[6]) const
195  {
196  if ( box[1] < bounds_[0] || box[3] < bounds_[2] || box[5] < bounds_[4] ||
197  box[0] > bounds_[1] || box[2] > bounds_[3] || box[4] > bounds_[5] )
198  return false;
199 
200  return true;
201  }
202 
203  /** \brief Computes and returns the volume of the bounding box of this node. */
204  double
205  computeBoundingBoxVolume() const
206  {
207  return (bounds_[1] - bounds_[0]) * (bounds_[3] - bounds_[2]) * (bounds_[5] - bounds_[4]);
208  }
209 
210  friend class BVH;
211 
212  protected:
213  float bounds_[6];
214  Node* children_[2];
216  };
217 
218  public:
219  BVH()
220  : root_ (0),
221  sorted_objects_ (0)
222  {
223  }
224 
225  virtual ~BVH()
226  {
227  this->clear ();
228  }
229 
230  /** \brief Creates the tree. No need to call clear, it's called within the method. 'objects' is a vector of
231  * pointers to bounded objects which have to have valid bounds and centroids. Use the getData method of
232  * BoundedObject to retrieve the user-defined data saved in the object. Note that vector will be sorted within
233  * the method!
234  *
235  * The tree is built such that each leaf contains exactly one object. */
236  void
237  build(std::vector<BoundedObject*>& objects)
238  {
239  this->clear();
240 
241  if ( objects.size () == 0 )
242  return;
243 
244  sorted_objects_ = &objects;
245 
246  // Now sort the objects according to the x-coordinates of their centroids
247  std::sort (objects.begin (), objects.end (), BoundedObject::compareCentroidsXCoordinates);
248 
249  // Create the root -> it recursively creates the children nodes until each leaf contains exactly one object
250  root_ = new Node (objects, 0, static_cast<int> (objects.size () - 1));
251  }
252 
253  /** \brief Frees the memory allocated by this object. After that, you have to call build to use the tree again. */
254  void
255  clear()
256  {
257  if ( root_ )
258  {
259  delete root_;
260  root_ = 0;
261  }
262  }
263 
264  inline const std::vector<BoundedObject*>*
265  getInputObjects () const
266  {
267  return (sorted_objects_);
268  }
269 
270  /** \brief Pushes back in 'intersected_objects' the bounded objects intersected by the input 'box' and returns true.
271  * Returns false if no objects are intersected. */
272  inline bool
273  intersect(const float box[6], std::list<BoundedObject*>& intersected_objects) const
274  {
275  if ( !root_ )
276  return false;
277 
278  bool got_intersection = false;
279 
280  // Start the intersection process at the root
281  std::list<Node*> working_list;
282  working_list.push_back (root_);
283 
284  while ( !working_list.empty () )
285  {
286  Node* node = working_list.front ();
287  working_list.pop_front ();
288 
289  // Is 'node' intersected by the box?
290  if ( node->intersect (box) )
291  {
292  // We have to check the children of the intersected 'node'
293  if ( node->hasChildren () )
294  {
295  working_list.push_back (node->getLeftChild ());
296  working_list.push_back (node->getRightChild ());
297  }
298  else // 'node' is a leaf -> save it's object in the output list
299  {
300  intersected_objects.push_back (node->getObject ());
301  got_intersection = true;
302  }
303  }
304  }
305 
306  return (got_intersection);
307  }
308 
309  protected:
311  std::vector<BoundedObject*>* sorted_objects_;
312  };
313  } // namespace recognition
314 } // namespace pcl
315 
316 #endif /* PCL_RECOGNITION_BVH_H_ */