46 #ifndef PCL_RECOGNITION_BVH_H_
47 #define PCL_RECOGNITION_BVH_H_
49 #include <pcl/pcl_exports.h>
65 template<
class UserData>
128 Node (std::vector<BoundedObject*>& sorted_objects,
int first_id,
int last_id)
131 memcpy (bounds_, sorted_objects[first_id]->getBounds (), 6*
sizeof (
float));
134 for (
int i = first_id + 1 ; i <= last_id ; ++i )
138 if ( first_id != last_id )
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);
148 object_ = sorted_objects[first_id];
149 children_[0] = children_[1] = 0;
165 return static_cast<bool>(children_[0]);
189 return !
static_cast<bool>(children_[0]);
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] )
207 return (bounds_[1] - bounds_[0]) * (bounds_[3] - bounds_[2]) * (bounds_[5] - bounds_[4]);
237 build(std::vector<BoundedObject*>& objects)
241 if ( objects.size () == 0 )
244 sorted_objects_ = &objects;
247 std::sort (objects.begin (), objects.end (), BoundedObject::compareCentroidsXCoordinates);
250 root_ =
new Node (objects, 0, static_cast<int> (objects.size () - 1));
264 inline const std::vector<BoundedObject*>*
267 return (sorted_objects_);
273 intersect(
const float box[6], std::list<BoundedObject*>& intersected_objects)
const
278 bool got_intersection =
false;
281 std::list<Node*> working_list;
282 working_list.push_back (root_);
284 while ( !working_list.empty () )
286 Node* node = working_list.front ();
287 working_list.pop_front ();
300 intersected_objects.push_back (node->
getObject ());
301 got_intersection =
true;
306 return (got_intersection);
const float * getCentroid() const
bool intersect(const float box[6]) const
Returns true if 'box' intersects or touches (with a side or a vertex) this node.
Node(std::vector< BoundedObject * > &sorted_objects, int first_id, int last_id)
'sorted_objects' is a sorted vector of bounded objects.
std::vector< BoundedObject * > * sorted_objects_
BoundedObject * getObject()
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...
void expandBoundingBox(T dst[6], const T src[6])
Expands the destination bounding box 'dst' such that it contains 'src'.
BoundedObject(const UserData &data)
void build(std::vector< BoundedObject * > &objects)
Creates the tree.
void clear()
Frees the memory allocated by this object.
double computeBoundingBoxVolume() const
Computes and returns the volume of the bounding box of this node.
This class is an implementation of bounding volume hierarchies.
const std::vector< BoundedObject * > * getInputObjects() const
UserData data_
This is the user-defined data object.
static bool compareCentroidsXCoordinates(const BoundedObject *a, const BoundedObject *b)
This method is for std::sort.