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
std::vector< BoundedObject * > * sorted_objects_
void expandBoundingBox(T dst[6], const T src[6])
Expands the destination bounding box 'dst' such that it contains 'src'.
double computeBoundingBoxVolume() const
Computes and returns the volume of the bounding box of this node.
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...
BoundedObject(const UserData &data)
void build(std::vector< BoundedObject * > &objects)
Creates the tree.
bool intersect(const float box[6]) const
Returns true if 'box' intersects or touches (with a side or a vertex) this node.
void clear()
Frees the memory allocated by this object.
Node(std::vector< BoundedObject * > &sorted_objects, int first_id, int last_id)
'sorted_objects' is a sorted vector of bounded objects.
BoundedObject * getObject()
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.