Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
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
54namespace 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 () = 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:
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
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
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
const std::vector< BoundedObject * > * getInputObjects() const
Definition bvh.h:256
void build(std::vector< BoundedObject * > &objects)
Creates the tree.
Definition bvh.h:231