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