Commit 9c404a1c authored by Akseppal's avatar Akseppal
Browse files

add documentation

parent c4e6d879
......@@ -24,14 +24,12 @@
namespace hpp {
namespace affordance {
/// \addtogroup
/// \{
struct TrianglePoints
struct TrianglePoints
{
fcl::Vec3f p1, p2, p3;
};
// helper class to save triangle information
struct Triangle
{
Triangle () {}
......@@ -62,51 +60,110 @@ namespace hpp {
boost::shared_ptr <fcl::Triangle> fclTri;
};
// helper function to extract mesh model of an fcl::collisionObstacle
BVHModelOBConst_Ptr_t GetModel (const fcl::CollisionObjectConstPtr_t object);
/// \addtogroup affordance
/// \{
/// Class that saves a reference collision object and indices to
/// those of its mesh triangles that form one affordance object.
/// This information will later be used to create fcl::collisionObjects
/// for each individual affordance.
class Affordance
{
public:
Affordance () {}
/// Constructor for an Affordance object
///
/// \param idxVec vector of triangle indices corresponding to
/// the mesh model within the reference fcl::collisionObject
/// \param colObj reference to pointer to fcl::collisionObject
/// containing the found affordance objects.
Affordance (const std::vector<unsigned int>& idxVec,
const fcl::CollisionObjectPtr_t& colObj):
indices_(idxVec), colObj_(colObj) {}
std::vector<unsigned int> indices_;
fcl::CollisionObjectPtr_t colObj_;
};
/// Class containing a vector of vectors of Affordance objects.
/// The length of the outer vector corresponds to the amount of different
/// affordance types found, and that of the inner vector to the amount
/// of affordance objects found for each affordance type.
class SemanticsData
{
public:
SemanticsData (): SemanticsData (0) {}
SemanticsData (const long unsigned int& affordanceCount)
/// Constructor that adjust the affordances_ vector to
/// the amount of defined affordance types but leaves the
/// vectors of affordances empty.
///
/// \param affordanceCount number of defined affordance types.
SemanticsData (const long unsigned int& affordanceCount)
{
affordances_.resize(affordanceCount);
}
std::vector<std::vector<AffordancePtr_t> > affordances_;
};
BVHModelOBConst_Ptr_t GetModel (const fcl::CollisionObjectConstPtr_t object);
/// Free functions to extract whole-body affordances from fcl
///
/// Free function that searches through a vector of mesh triangles
/// and saves the triangle indices that form a potential affordance
/// object. Given a reference triangle index that fullfils the affordance
/// requirements of a given affordance type, the function recursively goes
/// all triangles that ara linked to the the reference. Also saves the
/// through total area of found potential affordance.
///
/// \param listPotential reference to the vector of triangle indices
/// that form one potential affordance object. At every recursive
/// step maximum one triangle is added to the vector.
/// \param refOp operation that determines which affordance type the
/// triangles will be tested for.
/// \param allTris all triangles of a given fcl::collisionObject mesh.
/// This parameter is only used as to verify global (affordance type)
/// and local (neighbouring triangles) requirements, and is not
/// modified within the function.
/// \param searchableTris vector of triangle indices that should be tested
/// in the search for more triangles. Whenever a triangle is found,
/// it is deleted from the vector and will not be tested again.
/// Similarly, if a triangle is tested once and does not fullfil
/// the global requirement set by the affordance type, it is deleted
/// and will not be tested again in subsequent recursive steps.
/// \param refTriIdx index corresponding to the last found triangle that
/// fullfils bot the local and the global requirement. It is then
/// used as reference in the following recursive step.
/// \param area total area of all triangles that are part of the potential
/// affordance object. Every time a triangle fulfilling all set
/// requirements is found, its are is added to the previous total
/// before going to the next recursive step.
void searchLinkedTriangles(std::vector<unsigned int>& listPotential,
const OperationBasePtr_t& refOp,
const std::vector<Triangle>& allTris,
std::vector<unsigned int>& searchableTris,
const unsigned int& refTriIdx, double& area);
/// Free function that extracts all affordances (of all types) from a given
/// fcl::collisionObject.
///
/// \param colObj reference to a fcl::collisionObject pointer the triangles
/// of which will be searched for affordance objects.
/// \param opVec vector of operation objects that determine which requirements
/// are set for which affordance type. The length of this vector
/// corresponds to the amount of different affordance types considered.
SemanticsDataPtr_t affordanceAnalysis (const fcl::CollisionObjectPtr_t& colObj,
const OperationBases_t & opVec);
/// Free function that, given a semanticsData pointer, creates one
/// fcl::collisionObject for every Affordance object.
///
/// \param sData reference to all found Affordance objects.
std::vector<CollisionObjects_t> getAffordanceObjects
(const SemanticsDataPtr_t& sData);
/// Free helper function that creates a vector of operation objects.
OperationBases_t createOperations ();
/// \}
} // namespace affordance
} // namespace hpp
......
......@@ -40,10 +40,11 @@ namespace hpp {
const std::vector<Triangle>& allTris, std::vector<unsigned int>& searchableTris,
const unsigned int& refTriIdx, double& area)
{
// TODO: think of a better way of declaring margins?
const double marginRad = 0.3;
const double margin = 1e-15;
Triangle refTri = allTris[refTriIdx];
// find a cleaner way of removing & resizing the searchableTriangels vector
// TODO: find a cleaner way of removing & resizing the searchableTriangels vector?
std::remove (searchableTris.begin (), searchableTris.end (), refTriIdx);
searchableTris.pop_back ();
for (unsigned int searchIdx = 0; searchIdx < allTris.size (); searchIdx++) {
......@@ -105,7 +106,7 @@ namespace hpp {
triangles.push_back (Triangle (fcltri, tri));
// save vector index of triangles and their quantity.
unsetTriangles.push_back(i);
unsetTriangles.push_back(i);
}
std::vector <unsigned int> unseenTriangles;
for (unsigned int triIdx = 0; triIdx < triangles.size (); triIdx++) {
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment