Point Cloud Library (PCL) 1.12.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 ()
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. */
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 delete children_[0];
155 delete children_[1];
156 }
157
158 bool
159 hasChildren () const
160 {
161 return static_cast<bool>(children_[0]);
162 }
163
164 Node*
166 {
167 return children_[0];
168 }
169
170 Node*
172 {
173 return children_[1];
174 }
175
178 {
179 return object_;
180 }
181
182 bool
183 isLeaf () const
184 {
185 return !static_cast<bool>(children_[0]);
186 }
187
188 /** \brief Returns true if 'box' intersects or touches (with a side or a vertex) this node. */
189 inline bool
190 intersect(const float box[6]) const
191 {
192 return !(box[1] < bounds_[0] || box[3] < bounds_[2] || box[5] < bounds_[4] ||
193 box[0] > bounds_[1] || box[2] > bounds_[3] || box[4] > bounds_[5]);
194 }
195
196 /** \brief Computes and returns the volume of the bounding box of this node. */
197 double
199 {
200 return (bounds_[1] - bounds_[0]) * (bounds_[3] - bounds_[2]) * (bounds_[5] - bounds_[4]);
201 }
202
203 friend class BVH;
204
205 protected:
206 float bounds_[6];
207 Node* children_[2];
209 };
210
211 public:
213 : root_ (nullptr),
214 sorted_objects_ (nullptr)
215 {
216 }
217
218 virtual ~BVH()
219 {
220 this->clear ();
221 }
222
223 /** \brief Creates the tree. No need to call clear, it's called within the method. 'objects' is a vector of
224 * pointers to bounded objects which have to have valid bounds and centroids. Use the getData method of
225 * BoundedObject to retrieve the user-defined data saved in the object. Note that vector will be sorted within
226 * the method!
227 *
228 * The tree is built such that each leaf contains exactly one object. */
229 void
230 build(std::vector<BoundedObject*>& objects)
231 {
232 this->clear();
233
234 if ( objects.empty () )
235 return;
236
237 sorted_objects_ = &objects;
238
239 // Now sort the objects according to the x-coordinates of their centroids
240 std::sort (objects.begin (), objects.end (), BoundedObject::compareCentroidsXCoordinates);
241
242 // Create the root -> it recursively creates the children nodes until each leaf contains exactly one object
243 root_ = new Node (objects, 0, static_cast<int> (objects.size () - 1));
244 }
245
246 /** \brief Frees the memory allocated by this object. After that, you have to call build to use the tree again. */
247 void
249 {
250 delete root_;
251 root_ = nullptr;
252 }
253
254 inline const std::vector<BoundedObject*>*
256 {
257 return (sorted_objects_);
258 }
259
260 /** \brief Pushes back in 'intersected_objects' the bounded objects intersected by the input 'box' and returns true.
261 * Returns false if no objects are intersected. */
262 inline bool
263 intersect(const float box[6], std::list<BoundedObject*>& intersected_objects) const
264 {
265 if ( !root_ )
266 return false;
267
268 bool got_intersection = false;
269
270 // Start the intersection process at the root
271 std::list<Node*> working_list;
272 working_list.push_back (root_);
273
274 while ( !working_list.empty () )
275 {
276 Node* node = working_list.front ();
277 working_list.pop_front ();
278
279 // Is 'node' intersected by the box?
280 if ( node->intersect (box) )
281 {
282 // We have to check the children of the intersected 'node'
283 if ( node->hasChildren () )
284 {
285 working_list.push_back (node->getLeftChild ());
286 working_list.push_back (node->getRightChild ());
287 }
288 else // 'node' is a leaf -> save it's object in the output list
289 {
290 intersected_objects.push_back (node->getObject ());
291 got_intersection = true;
292 }
293 }
294 }
295
296 return (got_intersection);
297 }
298
299 protected:
301 std::vector<BoundedObject*>* sorted_objects_;
302 };
303 } // namespace recognition
304} // namespace pcl
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
static bool compareCentroidsXCoordinates(const BoundedObject *a, const BoundedObject *b)
This method is for std::sort.
Definition bvh.h:82
UserData data_
This is the user-defined data object.
Definition bvh.h:117
BoundedObject(const UserData &data)
Definition bvh.h:71
const float * getCentroid() const
Definition bvh.h:100
BoundedObject * getObject()
Definition bvh.h:177
Node(std::vector< BoundedObject * > &sorted_objects, int first_id, int last_id)
'sorted_objects' is a sorted vector of bounded objects.
Definition bvh.h:127
bool hasChildren() const
Definition bvh.h:159
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:190
bool isLeaf() const
Definition bvh.h:183
double computeBoundingBoxVolume() const
Computes and returns the volume of the bounding box of this node.
Definition bvh.h:198
BoundedObject * object_
Definition bvh.h:208
This class is an implementation of bounding volume hierarchies.
Definition bvh.h:66
std::vector< BoundedObject * > * sorted_objects_
Definition bvh.h:301
virtual ~BVH()
Definition bvh.h:218
void clear()
Frees the memory allocated by this object.
Definition bvh.h:248
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:263
const std::vector< BoundedObject * > * getInputObjects() const
Definition bvh.h:255
void build(std::vector< BoundedObject * > &objects)
Creates the tree.
Definition bvh.h:230