Skip to content
Snippets Groups Projects
Commit b8ef8727 authored by Raphael Grimm's avatar Raphael Grimm
Browse files

Fix qhull 

* removed global lock
* fixed warnings
parent 088a570d
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
CMakeModules/FindQHULL.cmake
+ 2
− 2
View file @ b8ef8727
......@@ -7,7 +7,7 @@
SET (QHULL_FOUND FALSE)
FIND_PATH (QHULL_INCLUDE_DIR qhull/qhull.h
FIND_PATH (QHULL_INCLUDE_DIR libqhull_r/qhull_ra.h
/usr/include
/usr/local/include
$ENV{HOME}/local/include
......@@ -15,7 +15,7 @@ FIND_PATH (QHULL_INCLUDE_DIR qhull/qhull.h
$ENV{QHULL_PATH}/include
$ENV{QHULL_INCLUDE_PATH}
)
FIND_LIBRARY (QHULL_LIBRARY NAMES qhull PATHS
FIND_LIBRARY (QHULL_LIBRARY NAMES qhull_r PATHS
/usr/lib
/usr/local/lib
$ENV{HOME}/local/lib
......
GraspPlanning/ConvexHullGenerator.cpp
+ 51
− 124
View file @ b8ef8727
......@@ -13,7 +13,7 @@ using namespace VirtualRobot::MathTools;
extern "C"
{
#include "qhull/qhull_a.h"
#include "libqhull_r/qhull_ra.h"
}
//extern "C"
......@@ -23,37 +23,19 @@ extern "C"
namespace GraspStudio
{
boost::mutex ConvexHullGenerator::qhull_mutex;
bool ConvexHullGenerator::ConvertPoints(std::vector<Eigen::Vector3f>& points, double* storePointsQHull, bool lockMutex)
bool ConvexHullGenerator::ConvertPoints(std::vector<Eigen::Vector3f>& points, double* storePointsQHull)
{
if (lockMutex)
{
qhull_mutex.lock();
}
for (int i = 0; i < (int)points.size(); i++)
{
storePointsQHull[i * 3 + 0] = points[i][0];
storePointsQHull[i * 3 + 1] = points[i][1];
storePointsQHull[i * 3 + 2] = points[i][2];
}
if (lockMutex)
{
qhull_mutex.unlock();
}
return true;
}
bool ConvexHullGenerator::ConvertPoints(std::vector<ContactPoint>& points, double* storePointsQHull, bool lockMutex)
bool ConvexHullGenerator::ConvertPoints(std::vector<ContactPoint>& points, double* storePointsQHull)
{
if (lockMutex)
{
qhull_mutex.lock();
}
for (int i = 0; i < (int)points.size(); i++)
{
storePointsQHull[i * 6 + 0] = points[i].p[0];
......@@ -63,42 +45,19 @@ namespace GraspStudio
storePointsQHull[i * 6 + 4] = points[i].n[1];
storePointsQHull[i * 6 + 5] = points[i].n[2];
}
if (lockMutex)
{
qhull_mutex.unlock();
}
return true;
}
VirtualRobot::MathTools::ConvexHull3DPtr ConvexHullGenerator::CreateConvexHull(VirtualRobot::TriMeshModelPtr pointsInput, bool lockMutex /*= true*/)
VirtualRobot::MathTools::ConvexHull3DPtr ConvexHullGenerator::CreateConvexHull(VirtualRobot::TriMeshModelPtr pointsInput)
{
if (lockMutex)
{
qhull_mutex.lock();
}
VirtualRobot::MathTools::ConvexHull3DPtr r = CreateConvexHull(pointsInput->vertices, false);
if (lockMutex)
{
qhull_mutex.unlock();
}
return r;
return CreateConvexHull(pointsInput->vertices);
}
VirtualRobot::MathTools::ConvexHull3DPtr ConvexHullGenerator::CreateConvexHull(std::vector<Eigen::Vector3f>& pointsInput, bool lockMutex /*= true*/)
{
if (lockMutex)
{
qhull_mutex.lock();
}
VirtualRobot::MathTools::ConvexHull3DPtr ConvexHullGenerator::CreateConvexHull(std::vector<Eigen::Vector3f>& pointsInput)
{
//clock_t startT = clock();
ConvexHull3DPtr result(new ConvexHull3D());
......@@ -109,12 +68,6 @@ namespace GraspStudio
if (nPoints < 4)
{
cout << __FUNCTION__ << "Error: Need at least 4 points (nr of points registered: " << nPoints << ")" << endl;
if (lockMutex)
{
qhull_mutex.unlock();
}
return result;
}
......@@ -149,36 +102,40 @@ namespace GraspStudio
#endif
//cout << "QHULL input: nVertices: " << pointsInput.size() << endl;
ConvertPoints(pointsInput, points, false);
ConvertPoints(pointsInput, points);
/*for (i=numpoints; i--; )
rows[i]= points+dim*i;
qh_printmatrix (outfile, "input", rows, numpoints, dim);*/
exitcode = qh_new_qhull(dim, numpoints, points, ismalloc, flags, outfile, errfile);
qhT qh_qh; /* Qhull's data structure. First argument of most calls */
qhT *qh= &qh_qh;
qh_zero(qh, errfile);
QHULL_LIB_CHECK
exitcode = qh_new_qhull(qh, dim, numpoints, points, ismalloc, flags, outfile, errfile);
if (!exitcode) /* if no error */
{
facetT* facet_list = qh facet_list;
facetT* facet_list = qh->facet_list;
//int convexNumFaces = qh num_facets;
/*int convexNumVert =*/ qh_setsize(qh_facetvertices(facet_list, nullptr, false));
/*int convexNumVert =*/ qh_setsize(qh, qh_facetvertices(qh, facet_list, nullptr, false));
qh_triangulate(); // need this for triangulated output!
qh_triangulate(qh); // need this for triangulated output!
//int convexNumFaces2 = qh num_facets;
/*int convexNumVert2 =*/ qh_setsize(qh_facetvertices(facet_list, nullptr, false));
/*int convexNumVert2 =*/ qh_setsize(qh, qh_facetvertices(qh, facet_list, nullptr, false));
/*
cout << "Numfacets1:" << convexNumFaces << endl;
cout << "Numvertices1:" << convexNumVert << endl;
cout << "Numfacets2:" << convexNumFaces2 << endl;
cout << "Numvertices2:" << convexNumVert2 << endl;*/
/* 'qh facet_list' contains the convex hull */
/* 'qht->facet_list' contains the convex hull */
Eigen::Vector3f v[3];
int nIds[3];
TriangleFace f;
int nFacets = 0;
//cout << "Volume: " << qh totvol << endl;
qh_getarea(qh facet_list);
//cout << "Volume: " << qh totvol << endl;
result->volume = qh totvol;
//cout << "Volume: " << qht-> totvol << endl;
qh_getarea(qh, qh->facet_list);
//cout << "Volume: " << qht-> totvol << endl;
result->volume = qh-> totvol;
double pCenter[3];
......@@ -189,6 +146,7 @@ namespace GraspStudio
}
int nVcertexCount = 0;
FORALLvertices
{
for (int u = 0; u < 3; u++)
......@@ -266,8 +224,8 @@ namespace GraspStudio
}
qh_freeqhull(!qh_ALL);
qh_memfreeshort(&curlong, &totlong);
qh_freeqhull(qh, !qh_ALL);
qh_memfreeshort(qh, &curlong, &totlong);
if (curlong || totlong)
fprintf(errfile, "qhull internal warning (main): did not free %d bytes of long memory (%d pieces)\n",
......@@ -277,35 +235,21 @@ namespace GraspStudio
//long timeMS = (long)(((float)(endT - startT) / (float)CLOCKS_PER_SEC) * 1000.0);
//cout << __FUNCTION__ << ": Created convex hull in " << timeMS << " ms" << endl;
if (lockMutex)
{
qhull_mutex.unlock();
}
return result;
}
VirtualRobot::MathTools::ConvexHull6DPtr ConvexHullGenerator::CreateConvexHull(std::vector<ContactPoint>& pointsInput, bool lockMutex)
VirtualRobot::MathTools::ConvexHull6DPtr ConvexHullGenerator::CreateConvexHull(std::vector<ContactPoint>& pointsInput)
{
if (lockMutex)
{
qhull_mutex.lock();
}
//clock_t startT = clock();
ConvexHull6DPtr result(new ConvexHull6D());
int nPoints = (int)pointsInput.size();
int nPoints = static_cast<int>(pointsInput.size());
if (nPoints < 4)
{
cout << __FUNCTION__ << "Error: Need at least 4 points (nr of points registered: " << nPoints << ")" << endl;
if (lockMutex)
{
qhull_mutex.unlock();
}
return result;
}
......@@ -345,19 +289,23 @@ namespace GraspStudio
//cout << "QHULL input: nVertices: " << pointsInput.size() << endl;
//printVertices(pointsInput);
ConvertPoints(pointsInput, points, false);
exitcode = qh_new_qhull(dim, numpoints, points, ismalloc,
ConvertPoints(pointsInput, points);
qhT qh_qh; /* Qhull's data structure. First argument of most calls */
qhT *qh= &qh_qh;
qh_zero(qh, errfile);
QHULL_LIB_CHECK
exitcode = qh_new_qhull(qh, dim, numpoints, points, ismalloc,
flags, outfile, errfile);
if (!exitcode) /* if no error */
{
facetT* facet_list = qh facet_list;
facetT* facet_list = qh->facet_list;
//int convexNumFaces = qh num_facets;
/*int convexNumVert =*/ qh_setsize(qh_facetvertices(facet_list, nullptr, false));
/*int convexNumVert =*/ qh_setsize(qh, qh_facetvertices(qh, facet_list, nullptr, false));
qh_triangulate(); // need this for triangulated output!
qh_triangulate(qh); // need this for triangulated output!
//int convexNumFaces2 = qh num_facets;
/*int convexNumVert2 =*/ qh_setsize(qh_facetvertices(facet_list, nullptr, false));
/*int convexNumVert2 =*/ qh_setsize(qh, qh_facetvertices(qh, facet_list, nullptr, false));
double pCenter[6];
for (double& u : pCenter)
......@@ -396,14 +344,14 @@ namespace GraspStudio
result->center.n[1] = pCenter[4];
result->center.n[2] = pCenter[5];
/* 'qh facet_list' contains the convex hull */
/* 'qht->facet_list' contains the convex hull */
ContactPoint v[6];
int nIds[6];
MathTools::TriangleFace6D f;
int nFacets = 0;
qh_getarea(qh facet_list);
result->volume = qh totvol;
qh_getarea(qh, qh->facet_list);
result->volume = qh-> totvol;
double p[6];
p[0] = p[1] = p[2] = p[3] = p[4] = p[5] = 0;
FORALLfacets
......@@ -468,11 +416,11 @@ namespace GraspStudio
f.offset = facet->offset;
double dist = qh_distnorm(6, pCenter, facet->normal, &(facet->offset));
f.distNormCenter = dist;
qh_distplane(pCenter, facet, &dist);
qh_distplane(qh, pCenter, facet, &dist);
f.distPlaneCenter = dist;
dist = qh_distnorm(6, pZero, facet->normal, &(facet->offset));
f.distNormZero = dist;
qh_distplane(pZero, facet, &dist);
qh_distplane(qh, pZero, facet, &dist);
f.distPlaneZero = dist;
#ifdef CONVEXHULL_DEBUG_OUTPUT
......@@ -498,8 +446,8 @@ namespace GraspStudio
cout << "QHULL result: nFactes: " << convexNumFaces2 << endl;*/
}
qh_freeqhull(!qh_ALL);
qh_memfreeshort(&curlong, &totlong);
qh_freeqhull(qh, !qh_ALL);
qh_memfreeshort(qh, &curlong, &totlong);
if (curlong || totlong)
fprintf(errfile, "qhull internal warning (main): did not free %d bytes of long memory (%d pieces)\n",
......@@ -509,52 +457,39 @@ namespace GraspStudio
//long timeMS = (long)(((float)(endT - startT) / (float)CLOCKS_PER_SEC) * 1000.0);
//cout << __FUNCTION__ << ": Created 6D convex hull in " << timeMS << " ms" << endl;
if (lockMutex)
{
qhull_mutex.unlock();
}
return result;
}
/*
bool createPoints( SoSeparator *pInputIVModel, std::vector<Vec3D> &vStorePoints, bool lockMutex )
bool createPoints( SoSeparator *pInputIVModel, std::vector<Vec3D> &vStorePoints)
{
if (!pInputIVModel)
return false;
if (lockMutex)
qhull_mutex.lock();
vStorePoints.clear();
SoCallbackAction ca;
ca.addTriangleCallback(SoShape::getClassTypeId(), &CConvexHullGenerator_triangleCB, &vStorePoints);
ca.apply(pInputIVModel);
if (lockMutex)
qhull_mutex.unlock();
return true;
}*/
/*
bool createConvexHull(SoSeparator *pInputIVModel, ConvexHull3D &storeResult)
{
qhull_mutex.lock();
vector<Vec3D> points;
if (!CreatePoints(pInputIVModel,points,false))
return false;
bool bRes = CreateConvexHull(points,storeResult,false);
qhull_mutex.unlock();
return bRes;
}*/
/*
bool createIVModel( ConvexHull3D &convexHull, SoSeparator *pStoreResult, bool lockMutex )
bool createIVModel( ConvexHull3D &convexHull, SoSeparator *pStoreResult)
{
if (!pStoreResult || convexHull.vertices.size()<=0 || convexHull.faces.size()<=0)
return false;
if (lockMutex)
qhull_mutex.lock();
SoCoordinate3* pCoords = new SoCoordinate3();
SoFaceSet* pFaceSet = new SoFaceSet();
......@@ -602,9 +537,6 @@ namespace GraspStudio
delete []pVertexArray;
delete []nNumVertices;
if (lockMutex)
qhull_mutex.unlock();
return true;
}*/
......@@ -636,7 +568,6 @@ namespace GraspStudio
if (!pStoreResult || convHull.vertices.size()<=0 || convHull.faces.size()<=0)
return false;
qhull_mutex.lock();
Face6D f;
Vec3D v1,v2,v3,v4,v5,v6;
......@@ -701,11 +632,9 @@ namespace GraspStudio
if (!CreateConvexHull(vProjectedPoints, projectedHull, false))
{
cout << __FUNCTION__ << " Could not create hull of projected points, aborting..." << endl;
qhull_mutex.unlock();
return false;
}
bool bRes = CreateIVModel(projectedHull, pStoreResult, false);
qhull_mutex.unlock();
return bRes;
/ *
// creates 3d-projection of all 6d facets
......@@ -813,7 +742,6 @@ namespace GraspStudio
delete []pVertexArray;
delete []nNumVertices;
qhull_mutex.unlock();
return true;
* /
......@@ -821,7 +749,7 @@ namespace GraspStudio
void ConvexHullGenerator::PrintVertices(std::vector<ContactPoint>& pointsInput)
{
for (int i = 0; i < (int)pointsInput.size(); i++)
for (std::size_t i = 0; i < pointsInput.size(); i++)
{
cout << "v" << i << ": " << pointsInput[i].p[0] << "," << pointsInput[i].p[1] << "," << pointsInput[i].p[2] << ","
<< pointsInput[i].n[0] << "," << pointsInput[i].n[1] << "," << pointsInput[i].n[2] << endl;
......@@ -837,15 +765,14 @@ namespace GraspStudio
float minValue[6];
float maxValue[6];
int i;
for (i = 0; i <= 5; i++)
for (std::size_t i = 0; i <= 5; i++)
{
minValue[i] = FLT_MAX;
maxValue[i] = -FLT_MAX;
}
for (i = 0; i < (int)convHull->vertices.size(); i++)
for (std::size_t i = 0; i < convHull->vertices.size(); i++)
{
if (convHull->vertices[i].p[0] < minValue[0])
{
......
GraspPlanning/ConvexHullGenerator.h
+ 5
− 9
View file @ b8ef8727
......@@ -42,25 +42,21 @@ namespace GraspStudio
/*!
Creates a convex hull of the points stored in pointsInput.
*/
static VirtualRobot::MathTools::ConvexHull3DPtr CreateConvexHull(std::vector<Eigen::Vector3f>& pointsInput, bool lockMutex = true);
static VirtualRobot::MathTools::ConvexHull3DPtr CreateConvexHull(VirtualRobot::TriMeshModelPtr pointsInput, bool lockMutex = true);
static VirtualRobot::MathTools::ConvexHull6DPtr CreateConvexHull(std::vector<VirtualRobot::MathTools::ContactPoint>& pointsInput, bool lockMutex = true);
static VirtualRobot::MathTools::ConvexHull3DPtr CreateConvexHull(std::vector<Eigen::Vector3f>& pointsInput);
static VirtualRobot::MathTools::ConvexHull3DPtr CreateConvexHull(VirtualRobot::TriMeshModelPtr pointsInput);
static VirtualRobot::MathTools::ConvexHull6DPtr CreateConvexHull(std::vector<VirtualRobot::MathTools::ContactPoint>& pointsInput);
static void PrintStatistics(VirtualRobot::MathTools::ConvexHull6DPtr convHull);
/*!
Convert points to qhull format
*/
static bool ConvertPoints(std::vector<Eigen::Vector3f>& points, double* storePointsQHull, bool lockMutex = true);
static bool ConvertPoints(std::vector<VirtualRobot::MathTools::ContactPoint>& points, double* storePointsQHull, bool lockMutex = true);
static bool ConvertPoints(std::vector<Eigen::Vector3f>& points, double* storePointsQHull);
static bool ConvertPoints(std::vector<VirtualRobot::MathTools::ContactPoint>& points, double* storePointsQHull);
static void PrintVertices(std::vector<VirtualRobot::MathTools::ContactPoint>& pointsInput);
static bool checkVerticeOrientation(const Eigen::Vector3f& v1, const Eigen::Vector3f& v2, const Eigen::Vector3f& v3, const Eigen::Vector3f& n);
protected:
//! QHull is not thread safe, so protect qHull calls with a mutex
static boost::mutex qhull_mutex;
};
}
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