void
convex_hull_compute(double* points,
std::size_t npoints,
point_oiter_t chull,
tri_id_oiter_t tri_ids,
std::size_t tri_id_offset)
{
qh_init_A(0, 0, stderr, 0, 0);
qh_init_B(points, int(npoints), int(N), false);
qh_initflags((char*)"qhull Pp QJ");
qh_qhull();
qh_check_output();
qh_triangulate();
// set by FORALLfacets:
facetT *facet;
// set by FORALLvertices, FOREACHvertex_:
vertexT *vertex;
// set by FOREACHvertex_:
vertexT **vertexp;
coordT *point, *pointtemp;
FORALLpoints {
opoint_t p;
std::copy(point, point + N, &p[0]);
*chull = p;
}
FORALLfacets {
setT const* const tri = qh_facet3vertex(facet);
FOREACHvertex_(tri) {
*tri_ids = qh_pointid(vertex->point) + (int)tri_id_offset;
}
}
qh_freeqhull(!qh_ALL);
int curlong, totlong;
qh_memfreeshort(&curlong, &totlong);
if(curlong || totlong) throw std::runtime_error("qhull memory was not freed");
}
template <typename PointInT> void
pcl::ConvexHull<PointInT>::performReconstruction3D (
PointCloud &hull, std::vector<pcl::Vertices> &polygons, bool fill_polygon_data)
{
int dimension = 3;
// True if qhull should free points in qh_freeqhull() or reallocation
boolT ismalloc = True;
// output from qh_produce_output(), use NULL to skip qh_produce_output()
FILE *outfile = NULL;
if (compute_area_)
outfile = stderr;
// option flags for qhull, see qh_opt.htm
const char *flags = qhull_flags.c_str ();
// error messages from qhull code
FILE *errfile = stderr;
// Array of coordinates for each point
coordT *points = reinterpret_cast<coordT*> (calloc (indices_->size () * dimension, sizeof (coordT)));
int j = 0;
for (size_t i = 0; i < indices_->size (); ++i, j+=dimension)
{
points[j + 0] = static_cast<coordT> (input_->points[(*indices_)[i]].x);
points[j + 1] = static_cast<coordT> (input_->points[(*indices_)[i]].y);
points[j + 2] = static_cast<coordT> (input_->points[(*indices_)[i]].z);
}
// Compute convex hull
int exitcode = qh_new_qhull (dimension, static_cast<int> (indices_->size ()), points, ismalloc, const_cast<char*> (flags), outfile, errfile);
// 0 if no error from qhull
if (exitcode != 0)
{
PCL_ERROR ("[pcl::%s::performReconstrution3D] ERROR: qhull was unable to compute a convex hull for the given point cloud (%zu)!\n", getClassName ().c_str (), input_->points.size ());
hull.points.resize (0);
hull.width = hull.height = 0;
polygons.resize (0);
qh_freeqhull (!qh_ALL);
int curlong, totlong;
qh_memfreeshort (&curlong, &totlong);
return;
}
qh_triangulate ();
int num_facets = qh num_facets;
int num_vertices = qh num_vertices;
hull.points.resize (num_vertices);
vertexT * vertex;
int i = 0;
// Max vertex id
int max_vertex_id = 0;
FORALLvertices
{
if (vertex->id + 1 > max_vertex_id)
max_vertex_id = vertex->id + 1;
}
++max_vertex_id;
std::vector<int> qhid_to_pcidx (max_vertex_id);
FORALLvertices
{
// Add vertices to hull point_cloud
hull.points[i] = input_->points[(*indices_)[qh_pointid (vertex->point)]];
qhid_to_pcidx[vertex->id] = i; // map the vertex id of qhull to the point cloud index
++i;
}
if (compute_area_)
{
total_area_ = qh totarea;
total_volume_ = qh totvol;
}
if (fill_polygon_data)
{
polygons.resize (num_facets);
int dd = 0;
facetT * facet;
FORALLfacets
{
polygons[dd].vertices.resize (3);
// Needed by FOREACHvertex_i_
int vertex_n, vertex_i;
FOREACHvertex_i_ ((*facet).vertices)
//facet_vertices.vertices.push_back (qhid_to_pcidx[vertex->id]);
polygons[dd].vertices[vertex_i] = qhid_to_pcidx[vertex->id];
++dd;
}
}
// Deallocates memory (also the points)
qh_freeqhull (!qh_ALL);
int curlong, totlong;
qh_memfreeshort (&curlong, &totlong);
hull.width = static_cast<uint32_t> (hull.points.size ());
hull.height = 1;
hull.is_dense = false;
}
RTC::ReturnCode_t setupCollisionModel(hrp::BodyPtr m_robot, const char *url, OpenHRP::BodyInfo_var binfo) {
// do qhull
OpenHRP::ShapeInfoSequence_var sis = binfo->shapes();
OpenHRP::LinkInfoSequence_var lis = binfo->links();
for(int i = 0; i < m_robot->numLinks(); i++ ) {
const OpenHRP::LinkInfo& i_li = lis[i];
const OpenHRP::TransformedShapeIndexSequence& tsis = i_li.shapeIndices;
// setup
int numTriangles = 0;
for (unsigned int l=0; l<tsis.length(); l++) {
OpenHRP::ShapeInfo& si = sis[tsis[l].shapeIndex];
const OpenHRP::LongSequence& triangles = si.triangles;
numTriangles += triangles.length();
}
double points[numTriangles*3];
int points_i = 0;
hrp::Matrix44 Rs44; // inv
hrp::Matrix33 Rs33 = m_robot->link(i)->Rs;
Rs44 << Rs33(0,0),Rs33(0,1), Rs33(0,2), 0,
Rs33(1,0),Rs33(1,1), Rs33(1,2), 0,
Rs33(2,0),Rs33(2,1), Rs33(2,2), 0,
0.0, 0.0, 0.0, 1.0;
for (unsigned int l=0; l<tsis.length(); l++) {
const OpenHRP::DblArray12& M = tsis[l].transformMatrix;
hrp::Matrix44 T0;
T0 << M[0], M[1], M[2], M[3],
M[4], M[5], M[6], M[7],
M[8], M[9], M[10], M[11],
0.0, 0.0, 0.0, 1.0;
hrp::Matrix44 T(Rs44 * T0);
const OpenHRP::ShapeInfo& si = sis[tsis[l].shapeIndex];
const OpenHRP::LongSequence& triangles = si.triangles;
const float *vertices = si.vertices.get_buffer();
for(unsigned int j=0; j < triangles.length() / 3; ++j){
for(int k=0; k < 3; ++k){
long orgVertexIndex = si.triangles[j * 3 + k];
int p = orgVertexIndex * 3;
hrp::Vector4 v(T * hrp::Vector4(vertices[p+0], vertices[p+1], vertices[p+2], 1.0));
points[points_i++] = v[0];
points[points_i++] = v[1];
points[points_i++] = v[2];
}
}
}
hrp::ColdetModelPtr coldetModel(new hrp::ColdetModel());
coldetModel->setName(std::string(i_li.name));
// qhull
int vertexIndex = 0;
int triangleIndex = 0;
int num = 0;
char flags[250];
boolT ismalloc = False;
sprintf(flags,"qhull Qt Tc");
if (! qh_new_qhull (3,numTriangles,points,ismalloc,flags,NULL,stderr) ) {
qh_triangulate();
qh_vertexneighbors();
vertexT *vertex,**vertexp;
coldetModel->setNumVertices(qh num_vertices);
coldetModel->setNumTriangles(qh num_facets);
int index[qh num_vertices];
FORALLvertices {
int p = qh_pointid(vertex->point);
index[p] = vertexIndex;
coldetModel->setVertex(vertexIndex++, points[p*3+0], points[p*3+1], points[p*3+2]);
}
facetT *facet;
num = qh num_facets;;
{
FORALLfacets {
int j = 0, p[3];
setT *vertices = qh_facet3vertex (facet);
FOREACHvertexreverse12_ (vertices) {
if (j<3) {
p[j] = index[qh_pointid(vertex->point)];
} else {
fprintf(stderr, "extra vertex %d\n",j);
}
j++;
}
coldetModel->setTriangle(triangleIndex++, p[0], p[1], p[2]);
}
}
} // qh_new_qhull
coldetModel->build();
m_robot->link(i)->coldetModel = coldetModel;
qh_freeqhull(!qh_ALL);
int curlong, totlong;
qh_memfreeshort (&curlong, &totlong);
if (curlong || totlong) {
fprintf(stderr, "convhulln: did not free %d bytes of long memory (%d pieces)\n", totlong, curlong);
}
//
std::cerr << std::setw(16) << i_li.name << " reduce triangles from " << std::setw(5) << numTriangles << " to " << std::setw(4) << num << std::endl;
}
/* Delaunay implementation methyod. If hide_qhull_errors is 1 then qhull error
* messages are discarded; if it is 0 then they are written to stderr. */
static PyObject*
delaunay_impl(int npoints, const double* x, const double* y,
int hide_qhull_errors)
{
coordT* points = NULL;
facetT* facet;
int i, ntri, max_facet_id;
FILE* error_file = NULL; /* qhull expects a FILE* to write errors to. */
int exitcode; /* Value returned from qh_new_qhull(). */
int* tri_indices = NULL; /* Maps qhull facet id to triangle index. */
int indices[3];
int curlong, totlong; /* Memory remaining after qh_memfreeshort. */
PyObject* tuple; /* Return tuple (triangles, neighbors). */
const int ndim = 2;
npy_intp dims[2];
PyArrayObject* triangles = NULL;
PyArrayObject* neighbors = NULL;
int* triangles_ptr;
int* neighbors_ptr;
/* Allocate points. */
points = (coordT*)malloc(npoints*ndim*sizeof(coordT));
if (points == NULL) {
PyErr_SetString(PyExc_MemoryError,
"Could not allocate points array in qhull.delaunay");
goto error_before_qhull;
}
/* Prepare points array to pass to qhull. */
for (i = 0; i < npoints; ++i) {
points[2*i ] = x[i];
points[2*i+1] = y[i];
}
/* qhull expects a FILE* to write errors to. */
if (hide_qhull_errors) {
/* qhull errors are ignored by writing to OS-equivalent of /dev/null.
* Rather than have OS-specific code here, instead it is determined by
* setupext.py and passed in via the macro MPL_DEVNULL. */
error_file = fopen(STRINGIFY(MPL_DEVNULL), "w");
if (error_file == NULL) {
PyErr_SetString(PyExc_RuntimeError,
"Could not open devnull in qhull.delaunay");
goto error_before_qhull;
}
}
else {
/* qhull errors written to stderr. */
error_file = stderr;
}
/* Perform Delaunay triangulation. */
exitcode = qh_new_qhull(ndim, npoints, points, False,
"qhull d Qt Qbb Qc Qz", NULL, error_file);
if (exitcode != qh_ERRnone) {
PyErr_Format(PyExc_RuntimeError,
"Error in qhull Delaunay triangulation calculation: %s (exitcode=%d)%s",
qhull_error_msg[exitcode], exitcode,
hide_qhull_errors ? "; use python verbose option (-v) to see original qhull error." : "");
goto error;
}
/* Split facets so that they only have 3 points each. */
qh_triangulate();
/* Determine ntri and max_facet_id.
Note that libqhull uses macros to iterate through collections. */
ntri = 0;
FORALLfacets {
if (!facet->upperdelaunay)
++ntri;
}
max_facet_id = qh facet_id - 1;
/* Create array to map facet id to triangle index. */
tri_indices = (int*)malloc((max_facet_id+1)*sizeof(int));
if (tri_indices == NULL) {
PyErr_SetString(PyExc_MemoryError,
"Could not allocate triangle map in qhull.delaunay");
goto error;
}
/* Allocate python arrays to return. */
dims[0] = ntri;
dims[1] = 3;
triangles = (PyArrayObject*)PyArray_SimpleNew(ndim, dims, NPY_INT);
if (triangles == NULL) {
PyErr_SetString(PyExc_MemoryError,
"Could not allocate triangles array in qhull.delaunay");
goto error;
}
neighbors = (PyArrayObject*)PyArray_SimpleNew(ndim, dims, NPY_INT);
if (neighbors == NULL) {
PyErr_SetString(PyExc_MemoryError,
"Could not allocate neighbors array in qhull.delaunay");
goto error;
}
triangles_ptr = (int*)PyArray_DATA(triangles);
neighbors_ptr = (int*)PyArray_DATA(neighbors);
/* Determine triangles array and set tri_indices array. */
i = 0;
FORALLfacets {
if (!facet->upperdelaunay) {
tri_indices[facet->id] = i++;
get_facet_vertices(facet, indices);
*triangles_ptr++ = (facet->toporient ? indices[0] : indices[2]);
*triangles_ptr++ = indices[1];
*triangles_ptr++ = (facet->toporient ? indices[2] : indices[0]);
}
else
tri_indices[facet->id] = -1;
}
/* Determine neighbors array. */
FORALLfacets {
if (!facet->upperdelaunay) {
get_facet_neighbours(facet, tri_indices, indices);
*neighbors_ptr++ = (facet->toporient ? indices[2] : indices[0]);
*neighbors_ptr++ = (facet->toporient ? indices[0] : indices[2]);
*neighbors_ptr++ = indices[1];
}
}
/* Clean up. */
qh_freeqhull(!qh_ALL);
qh_memfreeshort(&curlong, &totlong);
if (curlong || totlong)
PyErr_WarnEx(PyExc_RuntimeWarning,
"Qhull could not free all allocated memory", 1);
if (hide_qhull_errors)
fclose(error_file);
free(tri_indices);
free(points);
tuple = PyTuple_New(2);
PyTuple_SetItem(tuple, 0, (PyObject*)triangles);
PyTuple_SetItem(tuple, 1, (PyObject*)neighbors);
return tuple;
error:
/* Clean up. */
Py_XDECREF(triangles);
Py_XDECREF(neighbors);
qh_freeqhull(!qh_ALL);
qh_memfreeshort(&curlong, &totlong);
/* Don't bother checking curlong and totlong as raising error anyway. */
if (hide_qhull_errors)
fclose(error_file);
free(tri_indices);
error_before_qhull:
free(points);
return NULL;
}
void gr_delaunay(int npoints, const double *x, const double *y,
int *ntri, int **triangles)
{
coordT *points = NULL;
facetT *facet;
vertexT *vertex, **vertexp;
int i, max_facet_id;
int *tri_indices = NULL;
int indices[3], *indicesp;
int curlong, totlong;
const int ndim = 2;
int *tri = NULL;
*ntri = 0;
*triangles = NULL;
points = (coordT *) malloc(npoints * ndim * sizeof(coordT));
if (points != NULL) {
for (i = 0; i < npoints; ++i) {
points[2*i ] = x[i];
points[2*i+1] = y[i];
}
/* Perform Delaunay triangulation */
if (qh_new_qhull(ndim, npoints, points, False,
"qhull d Qt QbB Qz", NULL, stderr) == qh_ERRnone) {
/* Split facets so that they only have 3 points each */
qh_triangulate();
/* Determine ntri and max_facet_id */
FORALLfacets {
if (!facet->upperdelaunay)
(*ntri)++;
}
max_facet_id = qh facet_id - 1;
/* Create array to map facet id to triangle index */
tri_indices = (int *) malloc((max_facet_id+1) * sizeof(int));
if (tri_indices != NULL) {
tri = (int *) malloc(*ntri * 3 * sizeof(int));
if (tri != NULL) {
*triangles = tri;
/* Determine triangles array and set tri_indices array */
i = 0;
FORALLfacets {
if (!facet->upperdelaunay) {
tri_indices[facet->id] = i++;
indicesp = indices;
FOREACHvertex_(facet->vertices)
*indicesp++ = qh_pointid(vertex->point);
*tri++ = (facet->toporient ? indices[0] : indices[2]);
*tri++ = indices[1];
*tri++ = (facet->toporient ? indices[2] : indices[0]);
}
else
tri_indices[facet->id] = -1;
}
xp = x;
yp = y;
qsort(*triangles, *ntri, 3 * sizeof(int), compar);
}
else
template <typename PointInT> void
pcl::ConvexHull<PointInT>::performReconstruction (PointCloud &hull, std::vector<pcl::Vertices> &polygons,
bool fill_polygon_data)
{
// FInd the principal directions
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
Eigen::Vector4f xyz_centroid;
compute3DCentroid (*input_, *indices_, xyz_centroid);
computeCovarianceMatrix (*input_, *indices_, xyz_centroid, covariance_matrix);
EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);
Eigen::Affine3f transform1;
transform1.setIdentity ();
int dim = 3;
if (eigen_values[0] / eigen_values[2] < 1.0e-5)
{
// We have points laying on a plane, using 2d convex hull
// Compute transformation bring eigen_vectors.col(i) to z-axis
eigen_vectors.col (2) = eigen_vectors.col (0).cross (eigen_vectors.col (1));
eigen_vectors.col (1) = eigen_vectors.col (2).cross (eigen_vectors.col (0));
transform1 (0, 2) = eigen_vectors (0, 0);
transform1 (1, 2) = eigen_vectors (1, 0);
transform1 (2, 2) = eigen_vectors (2, 0);
transform1 (0, 1) = eigen_vectors (0, 1);
transform1 (1, 1) = eigen_vectors (1, 1);
transform1 (2, 1) = eigen_vectors (2, 1);
transform1 (0, 0) = eigen_vectors (0, 2);
transform1 (1, 0) = eigen_vectors (1, 2);
transform1 (2, 0) = eigen_vectors (2, 2);
transform1 = transform1.inverse ();
dim = 2;
}
else
transform1.setIdentity ();
PointCloud cloud_transformed;
pcl::demeanPointCloud (*input_, *indices_, xyz_centroid, cloud_transformed);
pcl::transformPointCloud (cloud_transformed, cloud_transformed, transform1);
// True if qhull should free points in qh_freeqhull() or reallocation
boolT ismalloc = True;
// option flags for qhull, see qh_opt.htm
char flags[] = "qhull Tc";
// output from qh_produce_output(), use NULL to skip qh_produce_output()
FILE *outfile = NULL;
// error messages from qhull code
FILE *errfile = stderr;
// 0 if no error from qhull
int exitcode;
// Array of coordinates for each point
coordT *points = (coordT *)calloc (cloud_transformed.points.size () * dim, sizeof(coordT));
for (size_t i = 0; i < cloud_transformed.points.size (); ++i)
{
points[i * dim + 0] = (coordT)cloud_transformed.points[i].x;
points[i * dim + 1] = (coordT)cloud_transformed.points[i].y;
if (dim > 2)
points[i * dim + 2] = (coordT)cloud_transformed.points[i].z;
}
// Compute convex hull
exitcode = qh_new_qhull (dim, cloud_transformed.points.size (), points, ismalloc, flags, outfile, errfile);
if (exitcode != 0)
{
PCL_ERROR ("[pcl::%s::performReconstrution] ERROR: qhull was unable to compute a convex hull for the given point cloud (%lu)!\n",
getClassName ().c_str (), (unsigned long) input_->points.size ());
//check if it fails because of NaN values...
if (!cloud_transformed.is_dense)
{
bool NaNvalues = false;
for (size_t i = 0; i < cloud_transformed.size (); ++i)
{
if (!pcl_isfinite (cloud_transformed.points[i].x) ||
!pcl_isfinite (cloud_transformed.points[i].y) ||
!pcl_isfinite (cloud_transformed.points[i].z))
{
NaNvalues = true;
break;
}
}
if (NaNvalues)
PCL_ERROR ("[pcl::%s::performReconstruction] ERROR: point cloud contains NaN values, consider running pcl::PassThrough filter first to remove NaNs!\n", getClassName ().c_str ());
}
hull.points.resize (0);
hull.width = hull.height = 0;
polygons.resize (0);
qh_freeqhull (!qh_ALL);
int curlong, totlong;
qh_memfreeshort (&curlong, &totlong);
return;
}
qh_triangulate ();
int num_facets = qh num_facets;
int num_vertices = qh num_vertices;
hull.points.resize (num_vertices);
vertexT * vertex;
int i = 0;
// Max vertex id
int max_vertex_id = -1;
FORALLvertices
{
if ((int)vertex->id > max_vertex_id)
max_vertex_id = vertex->id;
}
++max_vertex_id;
std::vector<int> qhid_to_pcidx (max_vertex_id);
FORALLvertices
{
// Add vertices to hull point_cloud
hull.points[i].x = vertex->point[0];
hull.points[i].y = vertex->point[1];
if (dim>2)
hull.points[i].z = vertex->point[2];
else
hull.points[i].z = 0;
qhid_to_pcidx[vertex->id] = i; // map the vertex id of qhull to the point cloud index
++i;
}
if (fill_polygon_data)
{
if (dim == 3)
{
polygons.resize (num_facets);
int dd = 0;
facetT * facet;
FORALLfacets
{
polygons[dd].vertices.resize (3);
// Needed by FOREACHvertex_i_
int vertex_n, vertex_i;
FOREACHvertex_i_((*facet).vertices)
//facet_vertices.vertices.push_back (qhid_to_pcidx[vertex->id]);
polygons[dd].vertices[vertex_i] = qhid_to_pcidx[vertex->id];
++dd;
}
}
else
{
