Commit e37d062e authored by Steve Tonneau's avatar Steve Tonneau
Browse files

comments

parent 7e49de09
/build/* /build/*
*.swp *.swp
*.user
...@@ -34,6 +34,7 @@ namespace hpp { ...@@ -34,6 +34,7 @@ namespace hpp {
{ {
Triangle () {} Triangle () {}
//STEVE: are you sure you want to do a copy of the fcl triangle
Triangle (const fcl::Triangle& inFclTri, const TrianglePoints& inPoints): Triangle (const fcl::Triangle& inFclTri, const TrianglePoints& inPoints):
points (inPoints), fclTri (new fcl::Triangle (inFclTri)) points (inPoints), fclTri (new fcl::Triangle (inFclTri))
{ {
...@@ -61,7 +62,7 @@ namespace hpp { ...@@ -61,7 +62,7 @@ namespace hpp {
}; };
// helper function to extract mesh model of an fcl::collisionObstacle // helper function to extract mesh model of an fcl::collisionObstacle
BVHModelOBConst_Ptr_t GetModel (const fcl::CollisionObjectConstPtr_t object); BVHModelOBConst_Ptr_t GetModel (const fcl::CollisionObjectConstPtr_t& object);
/// \addtogroup affordance /// \addtogroup affordance
/// \{ /// \{
...@@ -93,18 +94,25 @@ namespace hpp { ...@@ -93,18 +94,25 @@ namespace hpp {
class SemanticsData class SemanticsData
{ {
public: public:
//STEVE Delegate constructors are c++ 11 only. For the time being we must remain in c ++ 10
SemanticsData (): SemanticsData (0) {} SemanticsData (): SemanticsData (0) {}
/// Constructor that adjust the affordances_ vector to /// Constructor that adjust the affordances_ vector to
/// the amount of defined affordance types but leaves the /// the amount of defined affordance types but leaves the
/// vectors of affordances empty. /// vectors of affordances empty.
/// ///
/// \param affordanceCount number of defined affordance types. /// \param affordanceCount number of defined affordance types.
SemanticsData (const long unsigned int& affordanceCount) // STEVE: probably useless to call resize here, so I'd stick with the default constructor.
SemanticsData (const long unsigned int affordanceCount)
// STEVE Taking refs of numbers is not desirable because it does not allow dynamic initlization
// SemanticsData(2) is not possible, you need to do int tmp = 2; SemanticsData(tmp)
{ {
affordances_.resize(affordanceCount); affordances_.resize(affordanceCount);
} }
// STEVE typedef ?
std::vector<std::vector<AffordancePtr_t> > affordances_; std::vector<std::vector<AffordancePtr_t> > affordances_;
private:
SemanticsData(const SemanticsData&); // Prevent copy-construction
SemanticsData& operator=(const SemanticsData&);
}; };
...@@ -131,7 +139,7 @@ namespace hpp { ...@@ -131,7 +139,7 @@ namespace hpp {
/// the global requirement set by the affordance type, it is deleted /// the global requirement set by the affordance type, it is deleted
/// and will not be tested again in subsequent recursive steps. /// and will not be tested again in subsequent recursive steps.
/// \param refTriIdx index corresponding to the last found triangle that /// \param refTriIdx index corresponding to the last found triangle that
/// fullfils bot the local and the global requirement. It is then /// fullfils both the local and the global requirement. It is then
/// used as reference in the following recursive step. /// used as reference in the following recursive step.
/// \param area total area of all triangles that are part of the potential /// \param area total area of all triangles that are part of the potential
/// affordance object. Every time a triangle fulfilling all set /// affordance object. Every time a triangle fulfilling all set
......
...@@ -29,10 +29,10 @@ namespace hpp { ...@@ -29,10 +29,10 @@ namespace hpp {
public: public:
OperationBase (): zWorld_(0,0,1), margin_(0.3), minArea_(0.05), OperationBase (): zWorld_(0,0,1), margin_(0.3), minArea_(0.05),
affordance_("noAffordance") {} affordance_("noAffordance") {}
explicit OperationBase (const double & margin = 0.3, const double minArea = 0.05, explicit OperationBase (const double margin = 0.3, const double minArea = 0.05,
const char* affordanceName = "noAffordance"): zWorld_(0,0,1), const char* affordanceName = "noAffordance"): zWorld_(0,0,1),
margin_(margin), minArea_(minArea), affordance_(affordanceName) {} margin_(margin), minArea_(minArea), affordance_(affordanceName) {}
virtual bool requirement (fcl::Vec3f normal) =0; virtual bool requirement (const fcl::Vec3f& normal) =0;
const fcl::Vec3f zWorld_; const fcl::Vec3f zWorld_;
const double margin_; const double margin_;
...@@ -43,31 +43,25 @@ namespace hpp { ...@@ -43,31 +43,25 @@ namespace hpp {
class SupportOperation : public OperationBase class SupportOperation : public OperationBase
{ {
public: public:
explicit SupportOperation (const double & margin = 0.3, const double minArea = 0.05, explicit SupportOperation (const double margin = 0.3, const double minArea = 0.05,
const char* affordanceName = "Support"): const char* affordanceName = "Support"):
OperationBase(margin, minArea, affordanceName) {} OperationBase(margin, minArea, affordanceName) {}
bool requirement (fcl::Vec3f normal) bool requirement (const fcl::Vec3f& normal)
{ {
if ((zWorld_ - normal).sqrLength() < margin_) { return ((zWorld_ - normal).sqrLength() < margin_);
return true;
}
return false;
} }
}; // class SupportOperation }; // class SupportOperation
class LeanOperation : public OperationBase class LeanOperation : public OperationBase
{ {
public: public:
explicit LeanOperation (const double & margin = 0.3, const double minArea = 0.05, explicit LeanOperation (const double margin = 0.3, const double minArea = 0.05,
const char* affordanceName = "Lean"): const char* affordanceName = "Lean"):
OperationBase(margin, minArea, affordanceName) {} OperationBase(margin, minArea, affordanceName) {}
bool requirement (fcl::Vec3f normal) bool requirement (const fcl::Vec3f& normal)
{ {
if (fabs (normal.dot(zWorld_)) < margin_) { return (fabs (normal.dot(zWorld_)) < margin_);
return true;
}
return false;
} }
}; // class LeanOperation }; // class LeanOperation
......
...@@ -27,7 +27,7 @@ ...@@ -27,7 +27,7 @@
namespace hpp { namespace hpp {
namespace affordance { namespace affordance {
BVHModelOBConst_Ptr_t GetModel (const fcl::CollisionObjectConstPtr_t object) BVHModelOBConst_Ptr_t GetModel (const fcl::CollisionObjectConstPtr_t& object)
{ {
assert (object->collisionGeometry ()->getNodeType () == fcl::BV_OBBRSS); assert (object->collisionGeometry ()->getNodeType () == fcl::BV_OBBRSS);
const BVHModelOBConst_Ptr_t model = boost::static_pointer_cast<const BVHModelOB> const BVHModelOBConst_Ptr_t model = boost::static_pointer_cast<const BVHModelOB>
...@@ -41,12 +41,11 @@ namespace hpp { ...@@ -41,12 +41,11 @@ namespace hpp {
const unsigned int& refTriIdx, double& area) const unsigned int& refTriIdx, double& area)
{ {
// TODO: think of a better way of declaring margins? // TODO: think of a better way of declaring margins?
// STEVE: create a struct Configuration with all those params, and use it as a parameter
const double marginRad = 0.3; const double marginRad = 0.3;
const double margin = 1e-15; const double margin = 1e-15;
Triangle refTri = allTris[refTriIdx]; const Triangle& refTri = allTris[refTriIdx]; // STEVE no need to copy object
// TODO: find a cleaner way of removing & resizing the searchableTriangels vector? searchableTris.erase(std::remove(searchableTris.begin(), searchableTris.end(), refTriIdx), searchableTris.end());
std::remove (searchableTris.begin (), searchableTris.end (), refTriIdx);
searchableTris.pop_back ();
for (unsigned int searchIdx = 0; searchIdx < allTris.size (); searchIdx++) { for (unsigned int searchIdx = 0; searchIdx < allTris.size (); searchIdx++) {
std::vector<unsigned int>::iterator it = std::find (searchableTris.begin (), std::vector<unsigned int>::iterator it = std::find (searchableTris.begin (),
searchableTris.end (), searchIdx); searchableTris.end (), searchIdx);
...@@ -58,7 +57,7 @@ namespace hpp { ...@@ -58,7 +57,7 @@ namespace hpp {
refPoints.push_back(refTri.points.p2); refPoints.push_back(refTri.points.p2);
refPoints.push_back(refTri.points.p3); refPoints.push_back(refTri.points.p3);
for (unsigned int vertIdx = 0; vertIdx < 3; vertIdx++) { for (unsigned int vertIdx = 0; vertIdx < 3; vertIdx++) {
Triangle searchTri = allTris [searchIdx]; const Triangle& searchTri = allTris [searchIdx];
if ((refPoints[vertIdx] - searchTri.points.p1).sqrLength () < margin if ((refPoints[vertIdx] - searchTri.points.p1).sqrLength () < margin
|| (refPoints[vertIdx] - searchTri.points.p2).sqrLength () < margin || (refPoints[vertIdx] - searchTri.points.p2).sqrLength () < margin
|| (refPoints[vertIdx] - searchTri.points.p3).sqrLength () < margin) { || (refPoints[vertIdx] - searchTri.points.p3).sqrLength () < margin) {
...@@ -70,9 +69,8 @@ namespace hpp { ...@@ -70,9 +69,8 @@ namespace hpp {
searchableTris, searchIdx, area); searchableTris, searchIdx, area);
} }
} else { } else {
// if linked face does not fulfil global requirement, discard // if linked face does not fulfill global requirement, discard
std::remove (searchableTris.begin (), searchableTris.end (), searchIdx); searchableTris.erase(std::remove(searchableTris.begin(), searchableTris.end(), searchIdx), searchableTris.end());
searchableTris.pop_back ();
} }
break; // jump out of vertex loop if triangle already tested for affordance break; // jump out of vertex loop if triangle already tested for affordance
} }
...@@ -91,7 +89,7 @@ namespace hpp { ...@@ -91,7 +89,7 @@ namespace hpp {
std::vector<std::vector<unsigned int> > potentialAffordances (opVec.size ()); std::vector<std::vector<unsigned int> > potentialAffordances (opVec.size ());
SemanticsDataPtr_t foundAffordances(new SemanticsData(opVec.size ())); SemanticsDataPtr_t foundAffordances(new SemanticsData(opVec.size ()));
for(unsigned int i = 0; i < model->num_tris; ++i) for(int i = 0; i < model->num_tris; ++i)
{ {
// TODO: make sure triagle points are correct in world frame after every // TODO: make sure triagle points are correct in world frame after every
// change!! // change!!
......
...@@ -22,11 +22,23 @@ ...@@ -22,11 +22,23 @@
#define BOOST_TEST_MODULE test-operations #define BOOST_TEST_MODULE test-operations
#include <boost/test/included/unit_test.hpp> #include <boost/test/included/unit_test.hpp>
const double epsilon = 10e-6;
bool compDouble(const double a, const double b)
{
return a-b < epsilon;
}
bool compVec(const fcl::Vec3f& a, const fcl::Vec3f& b)
{
return (a-b).norm() < epsilon;
}
BOOST_AUTO_TEST_SUITE (test_affordance) BOOST_AUTO_TEST_SUITE (test_affordance)
BOOST_AUTO_TEST_CASE (operations) BOOST_AUTO_TEST_CASE (operations)
{ {
hpp::affordance::SupportOperationPtr_t support (new hpp::affordance::SupportOperation(0.3)); hpp::affordance::SupportOperationPtr_t support (new hpp::affordance::SupportOperation(0.3));
hpp::affordance::LeanOperationPtr_t lean (new hpp::affordance::LeanOperation(0.1)); hpp::affordance::LeanOperationPtr_t lean (new hpp::affordance::LeanOperation(0.1));
std::vector <hpp::affordance::OperationBasePtr_t> operations; std::vector <hpp::affordance::OperationBasePtr_t> operations;
...@@ -35,10 +47,10 @@ BOOST_AUTO_TEST_CASE (operations) ...@@ -35,10 +47,10 @@ BOOST_AUTO_TEST_CASE (operations)
const fcl::Vec3f normal1(0, 0, 1); const fcl::Vec3f normal1(0, 0, 1);
BOOST_CHECK_MESSAGE (support->zWorld_ == normal1, BOOST_CHECK_MESSAGE (compVec(support->zWorld_,normal1),
"default value for zWorld should be " << normal1 << " but is " << support->zWorld_); "default value for zWorld should be " << normal1 << " but is " << support->zWorld_);
BOOST_CHECK_MESSAGE (lean->margin_ == 0.1, BOOST_CHECK_MESSAGE (compDouble(lean->margin_,0.1),
"margin should match the one given when creating operation"); "margin should match the one given when creating operation");
BOOST_CHECK_MESSAGE (operations.size () == 2, BOOST_CHECK_MESSAGE (operations.size () == 2,
......
...@@ -54,7 +54,7 @@ BOOST_AUTO_TEST_CASE (oriented_triangles) ...@@ -54,7 +54,7 @@ BOOST_AUTO_TEST_CASE (oriented_triangles)
fcl::Vec3f axis (1,0,0); fcl::Vec3f axis (1,0,0);
fcl::Matrix3f R; fcl::Matrix3f R;
for (unsigned int i = 0; i < size_; ++i) { for (int i = 0; i < size_; ++i) {
boost::shared_ptr<Model> model (new Model ()); boost::shared_ptr<Model> model (new Model ());
fcl::Triangle tri (0,1,2); fcl::Triangle tri (0,1,2);
triangles.push_back (tri); triangles.push_back (tri);
......
Markdown is supported
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