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