Skip to content

GitLab

Unverified Commit cd85b7de authored by Fernbach Pierre's avatar Fernbach Pierre Committed by GitHub
Browse files

Merge pull request #7 from stonneau/rbprm_v_1 

reimplemented reducesize algorithm
parents 1998c652 c0fb2cae
Hide whitespace changes
Inline Side-by-side
src/affordance-extraction.cc
View file @ cd85b7de
......@@ -143,245 +143,180 @@ namespace hpp {
return foundAffordances;
}
std::vector<CollisionObjects_t> getAffordanceObjects
(const SemanticsDataPtr_t& sData)
Eigen::Matrix3d GetRotationMatrix(const Eigen::Vector3d &from, const Eigen::Vector3d &to)
{
std::vector<CollisionObjects_t> affObjs;
affObjs.clear();
for (unsigned int affIdx = 0; affIdx < sData->affordances_.size (); affIdx ++) {
std::vector<fcl::CollisionObjectPtr_t> objVec;
objVec.clear();
affObjs.push_back (objVec);
// get number of affordances of specific type (lean OR support etc)
// this corresponds to number of objects to be created for specific aff type
long unsigned int len = sData->affordances_[affIdx].size ();
for (unsigned int idx = 0; idx < len; idx++) {
std::vector<fcl::Vec3f> vertices;
std::vector<fcl::Triangle> triangles;
std::vector<std::size_t> triIndices;
triIndices.clear ();
hpp::affordance::AffordancePtr_t affPtr = sData->affordances_[affIdx][idx];
BVHModelOBConst_Ptr_t model = GetModel (affPtr->colObj_);
for (unsigned int triIdx = 0; triIdx < affPtr->indices_.size (); triIdx++) {
// give triangles of new object new vertex indices (start from 0
// and go up to 3*nbTris - 1 [all tris have 3 unique indices])
std::vector<std::size_t> triPoints;
const fcl::Triangle& refTri = model->tri_indices[affPtr->indices_[triIdx]];
// triangles.push_back (refTri);
for (unsigned int vertIdx = 0; vertIdx < 3; vertIdx++) {
std::vector<std::size_t>::iterator it =
std::find (triIndices.begin (), triIndices.end (), refTri[vertIdx]);
if (it == triIndices.end ()) {
triIndices.push_back (refTri[vertIdx]);
vertices.push_back (model->vertices [refTri[vertIdx]]);
triPoints.push_back (std::size_t (vertices.size () -1));
} else {
triPoints.push_back (it - triIndices.begin ());
}
}
if (triPoints.size () != 3) {
std::ostringstream oss
("wrong number of vertex indices in triangle!!");
throw std::runtime_error (oss.str ());
}
triangles.push_back (fcl::Triangle (triPoints[0], triPoints[1], triPoints[2]));
}
BVHModelOB_Ptr_t model1 (new BVHModelOB ());
// add the mesh data into the BVHModel structure
model1->beginModel ();
model1->addSubModel (vertices, triangles);
model1->endModel ();
// create affordance collision object from created affModel and
// tranformation of corresponding reference collision object.
fcl::CollisionObjectPtr_t obj (new fcl::CollisionObject(model1,
affPtr->colObj_->getTransform ()));
affObjs[affIdx].push_back (obj);
}
}
return affObjs;
Eigen::Quaterniond quat; quat.setFromTwoVectors(from, to);
return quat.toRotationMatrix();
}
/// isLeft(): tests if a point is Left|On|Right of an infinite line.
/// \param lA 1st point of the line
/// \param lB 2nd point of the line
/// \param p2 point to test
/// \return: >0 for p2 left of the line through p0 and p1
/// =0 for p2 on the line
/// <0 for p2 right of the line
/// See: the January 2001 Algorithm "Area of 2D and 3D Triangles and Polygons"
double isLeft(fcl::Vec3f lA, fcl::Vec3f lB, fcl::Vec3f p2)
Eigen::Matrix3d GetRotationMatrix(const std::vector<fcl::Vec3f>& points)
{
return (lB[0] - lA[0]) * (p2[1] - lA[1]) - (p2[0] - lA[0]) * (lB[1] - lA[1]);
assert(points.size() > 2);
fcl::Vec3f z(0.,0.,1.);
fcl::Vec3f normal = (points[1] - points[0]).cross(points[2] - points[0]);
return GetRotationMatrix(normal, z);
}
inline bool isTop(fcl::Vec3f A,fcl::Vec3f B){
return A[1]>B[1];
}
typedef fcl::Vec3f Point;
typedef std::vector<Point> T_Point;
typedef T_Point::const_iterator CIT_Point;
std::vector<fcl::Vec3f> projectPoints(const Eigen::Matrix3d& project, const std::vector<fcl::Vec3f>& points)
{
std::vector<fcl::Vec3f> res;
for(CIT_Point cit = points.begin(); cit != points.end(); ++cit)
res.push_back(project * (*cit));
return res;
}
// return the index of the left most points of points
size_t leftTopMost(std::vector<fcl::Vec3f> points)
fcl::Vec3f computeCentroid(const std::vector<fcl::Vec3f>& points)
{
size_t res=0;
for(size_t id=1;id<points.size();++id)
{
if(points[id][0] < points[res][0]){
res=id;
}
else if (points[id][0] == points[res][0]){ // take the top most
if(points[id][1] > points[res][1]){
res=id;
}
}
}
return res;
fcl::Vec3f acc(0.,0.,0.);
for(CIT_Point cit = points.begin(); cit != points.end(); ++cit)
acc += (*cit);
return acc / points.size();
}
std::vector<fcl::Vec3f> translatePoints(const std::vector<fcl::Vec3f>& points, const double reduceSize)
{
fcl::Vec3f centroid = computeCentroid(points);
fcl::Vec3f dir;
std::vector<fcl::Vec3f> res;
for(CIT_Point cit = points.begin(); cit != points.end(); ++cit)
{
dir = centroid - (*cit); dir.normalize();
res.push_back((*cit) + dir * reduceSize);
}
return res;
}
// return a vector containing the order of the index of points, such that the vertices are in clockwise order
std::vector<size_t> convexHull(const std::vector<fcl::Vec3f> points)
void createAffordanceModel(hpp::affordance::AffordancePtr_t affPtr,
const std::vector<fcl::Vec3f>& vertices,
const std::vector<fcl::Triangle>& triangles,
const unsigned int affIdx,
std::vector<CollisionObjects_t>& affObjs)
{
BVHModelOB_Ptr_t model1 (new BVHModelOB ());
// add the mesh data into the BVHModel structure
model1->beginModel ();
model1->addSubModel (vertices, triangles);
model1->endModel ();
// create affordance collision object from created affModel and
// tranformation of corresponding reference collision object.
fcl::CollisionObjectPtr_t obj (new fcl::CollisionObject(model1,
affPtr->colObj_->getTransform ()));
affObjs[affIdx].push_back (obj);
}
std::vector<size_t> res;
size_t id_pointOnHull = leftTopMost(points);
size_t id_lastPoint = 0;
// std::cout<<"leftTopMost : "<<points[id_pointOnHull]<<" id = "<<id_pointOnHull<<std::endl;
do {
id_lastPoint = 0;
//std::cout<<"On Hull : "<<points[id_pointOnHull]<<" id = "<<id_pointOnHull<<std::endl;
// std::cout<<"last point : "<<points[id_lastPoint]<<" id = "<<id_lastPoint<<std::endl;
for(size_t id_current = 1 ; id_current < points.size();++id_current)
void addTriangles(hpp::affordance::AffordancePtr_t affPtr, BVHModelOBConst_Ptr_t model,
std::vector<std::size_t>& triIndices, const unsigned int triIdx,
std::vector<fcl::Vec3f>& vertices, std::vector<fcl::Triangle>& triangles)
{
// give triangles of new object new vertex indices (start from 0
// and go up to 3*nbTris - 1 [all tris have 3 unique indices])
std::vector<std::size_t> triPoints;
const fcl::Triangle& refTri = model->tri_indices[affPtr->indices_[triIdx]];
// triangles.push_back (refTri);
for (unsigned int vertIdx = 0; vertIdx < 3; vertIdx++)
{
std::vector<std::size_t>::iterator it =
std::find (triIndices.begin (), triIndices.end (), refTri[vertIdx]);
if (it == triIndices.end ())
{
if((id_lastPoint == id_pointOnHull) || (isLeft(points[id_pointOnHull], points[id_lastPoint],points[id_current]) > 0)
|| ((id_current!=id_pointOnHull) && (id_current!= id_lastPoint) &&
(isLeft(points[id_pointOnHull], points[id_lastPoint],points[id_current]) == 0 )
&& ((points[id_pointOnHull]-points[id_current]).squaredNorm() < (points[id_pointOnHull]-points[id_lastPoint]).squaredNorm()))){ // if on the same line, take the closest one from ptsOnHull
if( ( std::find(res.begin(),res.end(),id_current) == res.end() ) || ((res.size()>0) && (id_current == res[0])))// only selected it if not on the list (or the first)
id_lastPoint = id_current;
}
triIndices.push_back (refTri[vertIdx]);
vertices.push_back (model->vertices [refTri[vertIdx]]);
triPoints.push_back (std::size_t (vertices.size () -1));
} else
{
triPoints.push_back (it - triIndices.begin ());
}
//std::cout<<"new last point : "<<points[id_lastPoint]<<" id = "<<id_lastPoint<<std::endl;
res.push_back(id_pointOnHull);
id_pointOnHull = id_lastPoint;
} while(id_lastPoint != res[0]);
//res.insert(res.end(), lastPoint);
// std::cout<<"points size = "<<points.size()<<" orderedlist size = "<<res.size()<<std::endl;
if(points.size() != res.size()){
std::ostringstream oss("Error while sorting vertices of affordances");
}
if (triPoints.size () != 3)
{
std::ostringstream oss("wrong number of vertex indices in triangle!!");
throw std::runtime_error (oss.str ());
}
return res;
triangles.push_back (fcl::Triangle (triPoints[0], triPoints[1], triPoints[2]));
}
// same as getAffordanceObject but apply a reduction of a given size on the mesh
std::vector<CollisionObjects_t> getReducedAffordanceObjects
(const SemanticsDataPtr_t& sData,std::vector<double> reduceSizes)
std::vector<CollisionObjects_t> getAffordanceObjects(const SemanticsDataPtr_t& sData)
{
if(reduceSizes.size() != sData->affordances_.size ()){
std::ostringstream oss("Error : the vector 'reduceSizes' must have one element per affordance types");
throw std::runtime_error (oss.str ());
}
std::vector<CollisionObjects_t> affObjs;
affObjs.clear();
for (unsigned int affIdx = 0; affIdx < sData->affordances_.size (); affIdx ++) { // for all affordance type
for (unsigned int affIdx = 0; affIdx < sData->affordances_.size (); affIdx ++)
{
std::vector<fcl::CollisionObjectPtr_t> objVec;
objVec.clear();
affObjs.push_back (objVec);
// get number of affordances of specific type (lean OR support etc)
// this corresponds to number of objects to be created for specific aff type
long unsigned int len = sData->affordances_[affIdx].size ();
for (unsigned int idx = 0; idx < len; idx++) { // for all affordances of a given type
for (unsigned int idx = 0; idx < len; idx++)
{
std::vector<fcl::Vec3f> vertices;
std::vector<fcl::Triangle> triangles;
std::vector<std::size_t> triIndices;
triIndices.clear ();
hpp::affordance::AffordancePtr_t affPtr = sData->affordances_[affIdx][idx]; // current affordance object to copy
hpp::affordance::AffordancePtr_t affPtr = sData->affordances_[affIdx][idx];
BVHModelOBConst_Ptr_t model = GetModel (affPtr->colObj_);
for (unsigned int triIdx = 0; triIdx < affPtr->indices_.size (); triIdx++) {
// give triangles of new object new vertex indices (start from 0
// and go up to 3*nbTris - 1 [all tris have 3 unique indices])
std::vector<std::size_t> triPoints;
const fcl::Triangle& refTri = model->tri_indices[affPtr->indices_[triIdx]];
for (unsigned int vertIdx = 0; vertIdx < 3; vertIdx++) {
std::vector<std::size_t>::iterator it = std::find (triIndices.begin (), triIndices.end (), refTri[vertIdx]);
if (it == triIndices.end ()) {
triIndices.push_back (refTri[vertIdx]);
vertices.push_back (model->vertices [refTri[vertIdx]]);
triPoints.push_back (std::size_t (vertices.size () -1));
} else {
triPoints.push_back (it - triIndices.begin ());
}
}
if (triPoints.size () != 3) {
std::ostringstream oss("wrong number of vertex indices in triangle!!");
throw std::runtime_error (oss.str ());
}
triangles.push_back (fcl::Triangle (triPoints[0], triPoints[1], triPoints[2]));
}
if(reduceSizes[affIdx]>std::numeric_limits<double>::epsilon()){
//std::cout<<"For points of affordance : "<<std::endl;
for(unsigned int id=0;id<vertices.size();++id){
std::cout<<"( "<<vertices[id][0]<<" , "<<vertices[id][1]<<" , "<<vertices[id][2]<<" ) "<<std::endl;
}
//first we need to sort the vertices in clockwise order.
// but as we cannot change the order in the 'vertices' vector we use another vector for correspondance of index
std::vector<size_t> orderedIndex = convexHull(vertices); // this vector contain the index of 'vertices' vertice in a counter-clockwise order.
// shift each vertice of 'reduceSize' along the vector defined by : unit(next-current)+unit(prev-current)
fcl::Vec3f prev,next,A,B,dir;
std::vector<fcl::Vec3f> shiftedVertices(vertices.size());
for(unsigned int id=0;id<orderedIndex.size();++id){
if(id==0)
prev=vertices[orderedIndex[orderedIndex.size()-1]];
else
prev=vertices[orderedIndex[id-1]];
if(id==(orderedIndex.size()-1))
next=vertices[orderedIndex[0]];
else
next=vertices[orderedIndex[id+1]];
A=(next-vertices[orderedIndex[id]]).normalized();
B=(prev-vertices[orderedIndex[id]]).normalized();
dir=(A+B).normalized();
/* if(dir.length() < std::numeric_limits<double>::epsilon()){ // this mean that prev, current and next are aligned, we make a 90° angle
dir=
}*/
fcl::Vec3f shiftedVertice = vertices[orderedIndex[id]] + ((dir)*reduceSizes[affIdx]);
shiftedVertices[orderedIndex[id]] = shiftedVertice;
}
vertices=shiftedVertices;
}
BVHModelOB_Ptr_t model1 (new BVHModelOB ());
// add the mesh data into the BVHModel structure
model1->beginModel ();
model1->addSubModel (vertices, triangles);
model1->endModel ();
// create affordance collision object from created affModel and
// tranformation of corresponding reference collision object.
fcl::CollisionObjectPtr_t obj (new fcl::CollisionObject(model1,affPtr->colObj_->getTransform ()));
affObjs[affIdx].push_back (obj);
for (unsigned int triIdx = 0; triIdx < affPtr->indices_.size (); triIdx++)
addTriangles(affPtr,model,triIndices,triIdx,vertices,triangles);
createAffordanceModel(affPtr, vertices, triangles, affIdx,affObjs);
}
}
return affObjs;
}
} // namespace affordance
//same as getAffordanceObject but apply a reduction of a given size on the mesh
std::vector<CollisionObjects_t> getReducedAffordanceObjects
(const SemanticsDataPtr_t& sData,std::vector<double> reduceSizes)
{
if(reduceSizes.size() != sData->affordances_.size ()){
std::ostringstream oss("Error : the vector 'reduceSizes' must have one element per affordance types");
throw std::runtime_error (oss.str ());
}
std::vector<CollisionObjects_t> affObjs;
affObjs.clear();
for (unsigned int affIdx = 0; affIdx < sData->affordances_.size (); affIdx ++)
{
std::vector<fcl::CollisionObjectPtr_t> objVec;
affObjs.push_back (objVec);
// get number of affordances of specific type (lean OR support etc)
// this corresponds to number of objects to be created for specific aff type
long unsigned int len = sData->affordances_[affIdx].size ();
for (unsigned int idx = 0; idx < len; idx++)
{
std::vector<fcl::Vec3f> vertices;
std::vector<fcl::Triangle> triangles;
std::vector<std::size_t> triIndices;
hpp::affordance::AffordancePtr_t affPtr = sData->affordances_[affIdx][idx];
BVHModelOBConst_Ptr_t model = GetModel (affPtr->colObj_);
for (unsigned int triIdx = 0; triIdx < affPtr->indices_.size (); triIdx++)
addTriangles(affPtr,model,triIndices,triIdx,vertices,triangles);
if(reduceSizes[affIdx]>std::numeric_limits<double>::epsilon())
{
//std::cout<<"For points of affordance : "<<std::endl;
for(unsigned int id=0;id<vertices.size();++id)
std::cout<<"( "<<vertices[id][0]<<" , "<<vertices[id][1]<<" , "<<vertices[id][2]<<" ) "<<std::endl;
//first we need to sort the vertices in clockwise order.
// but as we cannot change the order in the 'vertices' vector we use another vector for correspondance of index
// first getTransformation matrix to project triangles in a 2d plane
Eigen::Matrix3d project = GetRotationMatrix(vertices);
std::vector<fcl::Vec3f> projectedVertices = projectPoints(project, vertices);
// now translate each point by the reducesize factor in the direction
// towards the centroid
std::vector<fcl::Vec3f> translatedVertices = translatePoints(projectedVertices, reduceSizes[affIdx]);
vertices = projectPoints(project.transpose(), translatedVertices);
}
createAffordanceModel(affPtr, vertices, triangles, affIdx,affObjs);
}
}
return affObjs;
}
} // namespace affordance
} // namespace hpp
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