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 {