305 lines
9.5 KiB
C++
305 lines
9.5 KiB
C++
/*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* Point Cloud Library (PCL) - www.pointclouds.org
|
|
* Copyright (c) 2010-2012, Willow Garage, Inc.
|
|
*
|
|
* All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions
|
|
* are met:
|
|
*
|
|
* * Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* * Redistributions in binary form must reproduce the above
|
|
* copyright notice, this list of conditions and the following
|
|
* disclaimer in the documentation and/or other materials provided
|
|
* with the distribution.
|
|
* * Neither the name of Willow Garage, Inc. nor the names of its
|
|
* contributors may be used to endorse or promote products derived
|
|
* from this software without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*
|
|
*
|
|
*/
|
|
|
|
/*
|
|
* bvh.h
|
|
*
|
|
* Created on: Mar 7, 2013
|
|
* Author: papazov
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <pcl/pcl_exports.h>
|
|
#include <cstring>
|
|
#include <algorithm>
|
|
#include <vector>
|
|
#include <list>
|
|
|
|
namespace pcl
|
|
{
|
|
namespace recognition
|
|
{
|
|
/** \brief This class is an implementation of bounding volume hierarchies. Use the build method to construct
|
|
* the data structure. To use the class, construct an std::vector of pointers to BVH::BoundedObject objects
|
|
* and pass it to the build method. BVH::BoundedObject is a template class, so you can save user-defined data
|
|
* in it.
|
|
*
|
|
* The tree is built such that each leaf contains exactly one object. */
|
|
template<class UserData>
|
|
class PCL_EXPORTS BVH
|
|
{
|
|
public:
|
|
class BoundedObject
|
|
{
|
|
public:
|
|
BoundedObject (const UserData& data)
|
|
: data_ (data)
|
|
{
|
|
}
|
|
|
|
virtual ~BoundedObject ()
|
|
{
|
|
}
|
|
|
|
/** \brief This method is for std::sort. */
|
|
inline static bool
|
|
compareCentroidsXCoordinates (const BoundedObject* a, const BoundedObject* b)
|
|
{
|
|
return a->getCentroid ()[0] < b->getCentroid ()[0];
|
|
}
|
|
|
|
float*
|
|
getBounds ()
|
|
{
|
|
return (bounds_);
|
|
}
|
|
|
|
float*
|
|
getCentroid ()
|
|
{
|
|
return (centroid_);
|
|
}
|
|
|
|
const float*
|
|
getCentroid () const
|
|
{
|
|
return (centroid_);
|
|
}
|
|
|
|
UserData&
|
|
getData ()
|
|
{
|
|
return (data_);
|
|
}
|
|
|
|
protected:
|
|
/** These are the bounds of the object.*/
|
|
float bounds_[6];
|
|
/** This is the centroid. */
|
|
float centroid_[3];
|
|
/** This is the user-defined data object. */
|
|
UserData data_;
|
|
};
|
|
|
|
protected:
|
|
class Node
|
|
{
|
|
public:
|
|
/** \brief 'sorted_objects' is a sorted vector of bounded objects. It has to be sorted in ascending order according
|
|
* to the objects' x-coordinates. The constructor recursively calls itself with the right 'first_id' and 'last_id'
|
|
* and with the same vector 'sorted_objects'. */
|
|
Node (std::vector<BoundedObject*>& sorted_objects, int first_id, int last_id)
|
|
{
|
|
// Initialize the bounds of the node
|
|
memcpy (bounds_, sorted_objects[first_id]->getBounds (), 6*sizeof (float));
|
|
|
|
// Expand the bounds of the node
|
|
for ( int i = first_id + 1 ; i <= last_id ; ++i )
|
|
aux::expandBoundingBox(bounds_, sorted_objects[i]->getBounds());
|
|
|
|
// Shall we create children?
|
|
if ( first_id != last_id )
|
|
{
|
|
// Division by 2
|
|
int mid_id = (first_id + last_id) >> 1;
|
|
children_[0] = new Node(sorted_objects, first_id, mid_id);
|
|
children_[1] = new Node(sorted_objects, mid_id + 1, last_id);
|
|
}
|
|
else
|
|
{
|
|
// We reached a leaf
|
|
object_ = sorted_objects[first_id];
|
|
children_[0] = children_[1] = nullptr;
|
|
}
|
|
}
|
|
|
|
virtual ~Node ()
|
|
{
|
|
delete children_[0];
|
|
delete children_[1];
|
|
}
|
|
|
|
bool
|
|
hasChildren () const
|
|
{
|
|
return static_cast<bool>(children_[0]);
|
|
}
|
|
|
|
Node*
|
|
getLeftChild ()
|
|
{
|
|
return children_[0];
|
|
}
|
|
|
|
Node*
|
|
getRightChild ()
|
|
{
|
|
return children_[1];
|
|
}
|
|
|
|
BoundedObject*
|
|
getObject ()
|
|
{
|
|
return object_;
|
|
}
|
|
|
|
bool
|
|
isLeaf () const
|
|
{
|
|
return !static_cast<bool>(children_[0]);
|
|
}
|
|
|
|
/** \brief Returns true if 'box' intersects or touches (with a side or a vertex) this node. */
|
|
inline bool
|
|
intersect(const float box[6]) const
|
|
{
|
|
return !(box[1] < bounds_[0] || box[3] < bounds_[2] || box[5] < bounds_[4] ||
|
|
box[0] > bounds_[1] || box[2] > bounds_[3] || box[4] > bounds_[5]);
|
|
}
|
|
|
|
/** \brief Computes and returns the volume of the bounding box of this node. */
|
|
double
|
|
computeBoundingBoxVolume() const
|
|
{
|
|
return (bounds_[1] - bounds_[0]) * (bounds_[3] - bounds_[2]) * (bounds_[5] - bounds_[4]);
|
|
}
|
|
|
|
friend class BVH;
|
|
|
|
protected:
|
|
float bounds_[6];
|
|
Node* children_[2];
|
|
BoundedObject* object_;
|
|
};
|
|
|
|
public:
|
|
BVH()
|
|
: root_ (nullptr),
|
|
sorted_objects_ (nullptr)
|
|
{
|
|
}
|
|
|
|
virtual ~BVH()
|
|
{
|
|
this->clear ();
|
|
}
|
|
|
|
/** \brief Creates the tree. No need to call clear, it's called within the method. 'objects' is a vector of
|
|
* pointers to bounded objects which have to have valid bounds and centroids. Use the getData method of
|
|
* BoundedObject to retrieve the user-defined data saved in the object. Note that vector will be sorted within
|
|
* the method!
|
|
*
|
|
* The tree is built such that each leaf contains exactly one object. */
|
|
void
|
|
build(std::vector<BoundedObject*>& objects)
|
|
{
|
|
this->clear();
|
|
|
|
if ( objects.empty () )
|
|
return;
|
|
|
|
sorted_objects_ = &objects;
|
|
|
|
// Now sort the objects according to the x-coordinates of their centroids
|
|
std::sort (objects.begin (), objects.end (), BoundedObject::compareCentroidsXCoordinates);
|
|
|
|
// Create the root -> it recursively creates the children nodes until each leaf contains exactly one object
|
|
root_ = new Node (objects, 0, static_cast<int> (objects.size () - 1));
|
|
}
|
|
|
|
/** \brief Frees the memory allocated by this object. After that, you have to call build to use the tree again. */
|
|
void
|
|
clear()
|
|
{
|
|
delete root_;
|
|
root_ = nullptr;
|
|
}
|
|
|
|
inline const std::vector<BoundedObject*>*
|
|
getInputObjects () const
|
|
{
|
|
return (sorted_objects_);
|
|
}
|
|
|
|
/** \brief Pushes back in 'intersected_objects' the bounded objects intersected by the input 'box' and returns true.
|
|
* Returns false if no objects are intersected. */
|
|
inline bool
|
|
intersect(const float box[6], std::list<BoundedObject*>& intersected_objects) const
|
|
{
|
|
if ( !root_ )
|
|
return false;
|
|
|
|
bool got_intersection = false;
|
|
|
|
// Start the intersection process at the root
|
|
std::list<Node*> working_list;
|
|
working_list.push_back (root_);
|
|
|
|
while ( !working_list.empty () )
|
|
{
|
|
Node* node = working_list.front ();
|
|
working_list.pop_front ();
|
|
|
|
// Is 'node' intersected by the box?
|
|
if ( node->intersect (box) )
|
|
{
|
|
// We have to check the children of the intersected 'node'
|
|
if ( node->hasChildren () )
|
|
{
|
|
working_list.push_back (node->getLeftChild ());
|
|
working_list.push_back (node->getRightChild ());
|
|
}
|
|
else // 'node' is a leaf -> save it's object in the output list
|
|
{
|
|
intersected_objects.push_back (node->getObject ());
|
|
got_intersection = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
return (got_intersection);
|
|
}
|
|
|
|
protected:
|
|
Node* root_;
|
|
std::vector<BoundedObject*>* sorted_objects_;
|
|
};
|
|
} // namespace recognition
|
|
} // namespace pcl
|