Point Cloud Library (PCL)  1.3.1
concave_hull.hpp
Go to the documentation of this file.
00001 
00038 #include <pcl/pcl_config.h>
00039 #ifdef HAVE_QHULL
00040 
00041 #ifndef PCL_SURFACE_IMPL_CONCAVE_HULL_H_
00042 #define PCL_SURFACE_IMPL_CONCAVE_HULL_H_
00043 
00044 #include <map>
00045 #include <pcl/surface/concave_hull.h>
00046 #include <pcl/common/common.h>
00047 #include <pcl/common/eigen.h>
00048 #include <pcl/common/centroid.h>
00049 #include <pcl/common/transforms.h>
00050 #include <pcl/kdtree/kdtree_flann.h>
00051 #include <pcl/common/io.h>
00052 #include <stdio.h>
00053 #include <stdlib.h>
00054 
00055 extern "C"
00056 {
00057 #ifdef HAVE_QHULL_2011
00058 #  include "libqhull/libqhull.h"
00059 #  include "libqhull/mem.h"
00060 #  include "libqhull/qset.h"
00061 #  include "libqhull/geom.h"
00062 #  include "libqhull/merge.h"
00063 #  include "libqhull/poly.h"
00064 #  include "libqhull/io.h"
00065 #  include "libqhull/stat.h"
00066 #else
00067 #  include "qhull/qhull.h"
00068 #  include "qhull/mem.h"
00069 #  include "qhull/qset.h"
00070 #  include "qhull/geom.h"
00071 #  include "qhull/merge.h"
00072 #  include "qhull/poly.h"
00073 #  include "qhull/io.h"
00074 #  include "qhull/stat.h"
00075 #endif
00076 }
00077 
00079 template <typename PointInT> void
00080 pcl::ConcaveHull<PointInT>::reconstruct (PointCloud &output)
00081 {
00082   output.header = input_->header;
00083   if (alpha_ <= 0)
00084   {
00085     PCL_ERROR ("[pcl::%s::reconstruct] Alpha parameter must be set to a positive number!\n", getClassName ().c_str ());
00086     output.points.clear ();
00087     return;
00088   }
00089 
00090   if (!initCompute ())
00091   {
00092     output.points.clear ();
00093     return;
00094   }
00095 
00096   // Perform the actual surface reconstruction
00097   std::vector<pcl::Vertices> polygons;
00098   performReconstruction (output, polygons);
00099 
00100   output.width = output.points.size ();
00101   output.height = 1;
00102   output.is_dense = true;
00103 
00104   deinitCompute ();
00105 }
00106 
00108 template <typename PointInT> void
00109 pcl::ConcaveHull<PointInT>::reconstruct (PointCloud &output, std::vector<pcl::Vertices> &polygons)
00110 {
00111   output.header = input_->header;
00112   if (alpha_ <= 0)
00113   {
00114     PCL_ERROR ("[pcl::%s::reconstruct] Alpha parameter must be set to a positive number!\n", getClassName ().c_str ());
00115     output.points.clear ();
00116     return;
00117   }
00118 
00119   if (!initCompute ())
00120   {
00121     output.points.clear ();
00122     return;
00123   }
00124 
00125   // Perform the actual surface reconstruction
00126   performReconstruction (output, polygons);
00127 
00128   output.width = output.points.size ();
00129   output.height = 1;
00130   output.is_dense = true;
00131 
00132   deinitCompute ();
00133 }
00134 
00136 template <typename PointInT> void
00137 pcl::ConcaveHull<PointInT>::performReconstruction (PointCloud &alpha_shape, std::vector<pcl::Vertices> &polygons)
00138 {
00139   EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
00140   Eigen::Vector4f xyz_centroid;
00141   compute3DCentroid (*input_, *indices_, xyz_centroid);
00142   computeCovarianceMatrix (*input_, *indices_, xyz_centroid, covariance_matrix);
00143   EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
00144   EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
00145   pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);
00146 
00147   Eigen::Affine3f transform1;
00148   transform1.setIdentity ();
00149   int dim = 3;
00150   if (eigen_values[0] / eigen_values[2] < 1.0e-5)
00151   {
00152     // we have points laying on a plane, using 2d convex hull
00153     // compute transformation bring eigen_vectors.col(i) to z-axis
00154 
00155     eigen_vectors.col (2) = eigen_vectors.col (0).cross (eigen_vectors.col (1));
00156     eigen_vectors.col (1) = eigen_vectors.col (2).cross (eigen_vectors.col (0));
00157 
00158     transform1 (0, 2) = eigen_vectors (0, 0);
00159     transform1 (1, 2) = eigen_vectors (1, 0);
00160     transform1 (2, 2) = eigen_vectors (2, 0);
00161 
00162     transform1 (0, 1) = eigen_vectors (0, 1);
00163     transform1 (1, 1) = eigen_vectors (1, 1);
00164     transform1 (2, 1) = eigen_vectors (2, 1);
00165     transform1 (0, 0) = eigen_vectors (0, 2);
00166     transform1 (1, 0) = eigen_vectors (1, 2);
00167     transform1 (2, 0) = eigen_vectors (2, 2);
00168 
00169     transform1 = transform1.inverse ();
00170     dim = 2;
00171   }
00172   else
00173   {
00174     transform1.setIdentity ();
00175   }
00176 
00177   PointCloud cloud_transformed;
00178   pcl::demeanPointCloud (*input_, *indices_, xyz_centroid, cloud_transformed);
00179   pcl::transformPointCloud (cloud_transformed, cloud_transformed, transform1);
00180 
00181   // True if qhull should free points in qh_freeqhull() or reallocation
00182   boolT ismalloc = True;
00183   // option flags for qhull, see qh_opt.htm
00184   char flags[] = "qhull d QJ";
00185   // output from qh_produce_output(), use NULL to skip qh_produce_output()
00186   FILE *outfile = NULL;
00187   // error messages from qhull code
00188   FILE *errfile = stderr;
00189   // 0 if no error from qhull
00190   int exitcode;
00191 
00192   // Array of coordinates for each point
00193   coordT *points = (coordT*)calloc (cloud_transformed.points.size () * dim, sizeof(coordT));
00194 
00195   for (size_t i = 0; i < cloud_transformed.points.size (); ++i)
00196   {
00197     points[i * dim + 0] = (coordT)cloud_transformed.points[i].x;
00198     points[i * dim + 1] = (coordT)cloud_transformed.points[i].y;
00199 
00200     if (dim > 2)
00201       points[i * dim + 2] = (coordT)cloud_transformed.points[i].z;
00202   }
00203 
00204   // Compute concave hull
00205   exitcode = qh_new_qhull (dim, cloud_transformed.points.size (), points, ismalloc, flags, outfile, errfile);
00206 
00207   if (exitcode != 0)
00208   {
00209     PCL_ERROR ("[pcl::%s::performReconstrution] ERROR: qhull was unable to compute a concave hull for the given point cloud (%lu)!\n", getClassName ().c_str (), (unsigned long) cloud_transformed.points.size ());
00210 
00211     //check if it fails because of NaN values...
00212     if (!cloud_transformed.is_dense)
00213     {
00214       bool NaNvalues = false;
00215       for (size_t i = 0; i < cloud_transformed.size (); ++i)
00216       {
00217         if (!pcl_isfinite (cloud_transformed.points[i].x) || 
00218             !pcl_isfinite (cloud_transformed.points[i].y) ||
00219             !pcl_isfinite (cloud_transformed.points[i].z))
00220         {
00221           NaNvalues = true;
00222           break;
00223         }
00224       }
00225 
00226       if (NaNvalues)
00227         PCL_ERROR ("[pcl::%s::performReconstruction] ERROR: point cloud contains NaN values, consider running pcl::PassThrough filter first to remove NaNs!\n", getClassName ().c_str ());
00228     }
00229 
00230     alpha_shape.points.resize (0);
00231     alpha_shape.width = alpha_shape.height = 0;
00232     polygons.resize (0);
00233 
00234     qh_freeqhull (!qh_ALL);
00235     int curlong, totlong;
00236     qh_memfreeshort (&curlong, &totlong);
00237 
00238     return;
00239   }
00240 
00241   qh_setvoronoi_all ();
00242 
00243   int num_vertices = qh num_vertices;
00244   alpha_shape.points.resize (num_vertices);
00245 
00246   vertexT *vertex;
00247   // Max vertex id
00248   int max_vertex_id = - 1;
00249   FORALLvertices
00250   {
00251     if ((int)vertex->id > max_vertex_id)
00252       max_vertex_id = vertex->id;
00253   }
00254 
00255   facetT *facet;    // set by FORALLfacets
00256 
00257   ++max_vertex_id;
00258   std::vector<int> qhid_to_pcidx (max_vertex_id);
00259 
00260   int num_facets = qh num_facets;
00261   int dd = 0;
00262 
00263   if (dim == 3)
00264   {
00265     setT *triangles_set = qh_settemp (4 * num_facets);
00266     if (voronoi_centers_)
00267       voronoi_centers_->points.resize (num_facets);
00268 
00269     int non_upper = 0;
00270     FORALLfacets
00271     {
00272       // Facets are tetrahedrons (3d)
00273       if (!facet->upperdelaunay)
00274       {
00275         vertexT *anyVertex = (vertexT*)(facet->vertices->e[0].p);
00276         double *center = facet->center;
00277         double r = qh_pointdist (anyVertex->point,center,dim);
00278         facetT *neighb;
00279 
00280         if (voronoi_centers_)
00281         {
00282           voronoi_centers_->points[non_upper].x = facet->center[0];
00283           voronoi_centers_->points[non_upper].y = facet->center[1];
00284           voronoi_centers_->points[non_upper].z = facet->center[2];
00285         }
00286 
00287         non_upper++;
00288 
00289         if (r <= alpha_)
00290         {
00291           // all triangles in tetrahedron are good, add them all to the alpha shape (triangles_set)
00292           qh_makeridges (facet);
00293           facet->good = true;
00294           facet->visitid = qh visit_id;
00295           ridgeT *ridge, **ridgep;
00296           FOREACHridge_ (facet->ridges)
00297           {
00298             neighb = otherfacet_ (ridge, facet);
00299             if ((neighb->visitid != qh visit_id))
00300               qh_setappend (&triangles_set, ridge);
00301           }
00302         }
00303         else
00304         {
00305           // consider individual triangles from the tetrahedron...
00306           facet->good = false;
00307           facet->visitid = qh visit_id;
00308           qh_makeridges (facet);
00309           ridgeT *ridge, **ridgep;
00310           FOREACHridge_ (facet->ridges)
00311           {
00312             facetT *neighb;
00313             neighb = otherfacet_ (ridge, facet);
00314             if ((neighb->visitid != qh visit_id))
00315             {
00316               // check if individual triangle is good and add it to triangles_set
00317 
00318               PointInT a, b, c;
00319               a.x = ((vertexT*)(ridge->vertices->e[0].p))->point[0];
00320               a.y = ((vertexT*)(ridge->vertices->e[0].p))->point[1];
00321               a.z = ((vertexT*)(ridge->vertices->e[0].p))->point[2];
00322               b.x = ((vertexT*)(ridge->vertices->e[1].p))->point[0];
00323               b.y = ((vertexT*)(ridge->vertices->e[1].p))->point[1];
00324               b.z = ((vertexT*)(ridge->vertices->e[1].p))->point[2];
00325               c.x = ((vertexT*)(ridge->vertices->e[2].p))->point[0];
00326               c.y = ((vertexT*)(ridge->vertices->e[2].p))->point[1];
00327               c.z = ((vertexT*)(ridge->vertices->e[2].p))->point[2];
00328 
00329               double r = pcl::getCircumcircleRadius (a, b, c);
00330               if (r <= alpha_)
00331                 qh_setappend (&triangles_set, ridge);
00332             }
00333           }
00334         }
00335       }
00336     }
00337 
00338     if (voronoi_centers_)
00339       voronoi_centers_->points.resize (non_upper);
00340 
00341     // filter, add points to alpha_shape and create polygon structure
00342 
00343     int num_good_triangles = 0;
00344     ridgeT *ridge, **ridgep;
00345     FOREACHridge_ (triangles_set)
00346     {
00347       if (ridge->bottom->upperdelaunay || ridge->top->upperdelaunay || !ridge->top->good || !ridge->bottom->good)
00348         num_good_triangles++;
00349     }
00350 
00351     polygons.resize (num_good_triangles);
00352 
00353     int vertices = 0;
00354     std::vector<bool> added_vertices (max_vertex_id, false);
00355 
00356     int triangles = 0;
00357     FOREACHridge_ (triangles_set)
00358     {
00359       if (ridge->bottom->upperdelaunay || ridge->top->upperdelaunay || !ridge->top->good || !ridge->bottom->good)
00360       {
00361         polygons[triangles].vertices.resize (3);
00362         int vertex_n, vertex_i;
00363         FOREACHvertex_i_ ((*ridge).vertices)  //3 vertices per ridge!
00364         {
00365           if (!added_vertices[vertex->id])
00366           {
00367             alpha_shape.points[vertices].x = vertex->point[0];
00368             alpha_shape.points[vertices].y = vertex->point[1];
00369             alpha_shape.points[vertices].z = vertex->point[2];
00370 
00371             qhid_to_pcidx[vertex->id] = vertices;   //map the vertex id of qhull to the point cloud index
00372             added_vertices[vertex->id] = true;
00373             vertices++;
00374           }
00375 
00376           polygons[triangles].vertices[vertex_i] = qhid_to_pcidx[vertex->id];
00377 
00378         }
00379 
00380         triangles++;
00381       }
00382     }
00383 
00384     alpha_shape.points.resize (vertices);
00385     alpha_shape.width = alpha_shape.points.size ();
00386     alpha_shape.height = 1;
00387   }
00388   else
00389   {
00390     // Compute the alpha complex for the set of points
00391     // Filters the delaunay triangles
00392     setT *edges_set = qh_settemp (3 * num_facets);
00393     if (voronoi_centers_)
00394       voronoi_centers_->points.resize (num_facets);
00395 
00396     FORALLfacets
00397     {
00398       // Facets are the delaunay triangles (2d)
00399       if (!facet->upperdelaunay)
00400       {
00401         // Check if the distance from any vertex to the facet->center
00402         // (center of the voronoi cell) is smaller than alpha
00403         vertexT *anyVertex = (vertexT*)(facet->vertices->e[0].p);
00404         double r = (sqrt ((anyVertex->point[0] - facet->center[0]) *
00405                           (anyVertex->point[0] - facet->center[0]) +
00406                           (anyVertex->point[1] - facet->center[1]) *
00407                           (anyVertex->point[1] - facet->center[1])));
00408         if (r <= alpha_)
00409         {
00410           pcl::Vertices facet_vertices;   //TODO: is not used!!
00411           qh_makeridges (facet);
00412           facet->good = true;
00413 
00414           ridgeT *ridge, **ridgep;
00415           FOREACHridge_ (facet->ridges)
00416           qh_setappend (&edges_set, ridge);
00417 
00418           if (voronoi_centers_)
00419           {
00420             voronoi_centers_->points[dd].x = facet->center[0];
00421             voronoi_centers_->points[dd].y = facet->center[1];
00422             voronoi_centers_->points[dd].z = 0;
00423           }
00424 
00425           ++dd;
00426         }
00427         else
00428           facet->good = false;
00429       }
00430     }
00431 
00432     int vertices = 0;
00433     std::vector<bool> added_vertices (max_vertex_id, false);
00434     std::map<int, std::vector<int> > edges;
00435 
00436     ridgeT *ridge, **ridgep;
00437     FOREACHridge_ (edges_set)
00438     {
00439       if (ridge->bottom->upperdelaunay || ridge->top->upperdelaunay || !ridge->top->good || !ridge->bottom->good)
00440       {
00441         int vertex_n, vertex_i;
00442         int vertices_in_ridge=0;
00443         std::vector<int> pcd_indices;
00444         pcd_indices.resize (2);
00445 
00446         FOREACHvertex_i_ ((*ridge).vertices)  //in 2-dim, 2 vertices per ridge!
00447         {
00448           if (!added_vertices[vertex->id])
00449           {
00450             alpha_shape.points[vertices].x = vertex->point[0];
00451             alpha_shape.points[vertices].y = vertex->point[1];
00452 
00453             if (dim > 2)
00454               alpha_shape.points[vertices].z = vertex->point[2];
00455             else
00456               alpha_shape.points[vertices].z = 0;
00457 
00458             qhid_to_pcidx[vertex->id] = vertices;   //map the vertex id of qhull to the point cloud index
00459             added_vertices[vertex->id] = true;
00460             pcd_indices[vertices_in_ridge] = vertices;
00461             vertices++;
00462           }
00463           else
00464           {
00465             pcd_indices[vertices_in_ridge] = qhid_to_pcidx[vertex->id];
00466           }
00467 
00468           vertices_in_ridge++;
00469         }
00470 
00471         // make edges bidirectional and pointing to alpha_shape pointcloud...
00472         edges[pcd_indices[0]].push_back (pcd_indices[1]);
00473         edges[pcd_indices[1]].push_back (pcd_indices[0]);
00474       }
00475     }
00476 
00477     alpha_shape.points.resize (vertices);
00478 
00479     std::vector<std::vector<int> > connected;
00480     PointCloud alpha_shape_sorted;
00481     alpha_shape_sorted.points.resize (vertices);
00482 
00483     // iterate over edges until they are empty!
00484     std::map<int, std::vector<int> >::iterator curr = edges.begin ();
00485     int next = - 1;
00486     std::vector<bool> used (vertices, false);   // used to decide which direction should we take!
00487     std::vector<int> pcd_idx_start_polygons;
00488     pcd_idx_start_polygons.push_back (0);
00489 
00490     // start following edges and removing elements
00491     int sorted_idx = 0;
00492     while (!edges.empty ())
00493     {
00494       alpha_shape_sorted.points[sorted_idx] = alpha_shape.points[(*curr).first];
00495       // check where we can go from (*curr).first
00496       for (size_t i = 0; i < (*curr).second.size (); i++)
00497       {
00498         if (!used[((*curr).second)[i]])
00499         {
00500           // we can go there
00501           next = ((*curr).second)[i];
00502           break;
00503         }
00504       }
00505 
00506       used[(*curr).first] = true;
00507       edges.erase (curr);   // remove edges starting from curr
00508 
00509       sorted_idx++;
00510 
00511       if (edges.empty ())
00512         break;
00513 
00514       // reassign current
00515       curr = edges.find (next);   // if next is not found, then we have unconnected polygons.
00516       if (curr == edges.end ())
00517       {
00518         // set current to any of the remaining in edge!
00519         curr = edges.begin ();
00520         pcd_idx_start_polygons.push_back (sorted_idx);
00521       }
00522     }
00523 
00524     pcd_idx_start_polygons.push_back (sorted_idx);
00525 
00526     alpha_shape.points = alpha_shape_sorted.points;
00527 
00528     polygons.resize (pcd_idx_start_polygons.size () - 1);
00529 
00530     for (size_t poly_id = 0; poly_id < polygons.size (); poly_id++)
00531     {
00532       polygons[poly_id].vertices.resize (pcd_idx_start_polygons[poly_id + 1] - pcd_idx_start_polygons[poly_id] + 1);
00533       // populate points in the corresponding polygon
00534       for (size_t j = (size_t)pcd_idx_start_polygons[poly_id]; j < (size_t)pcd_idx_start_polygons[poly_id + 1]; ++j)
00535         polygons[poly_id].vertices[j - pcd_idx_start_polygons[poly_id]] = j;
00536 
00537       polygons[poly_id].vertices[polygons[poly_id].vertices.size () - 1] = pcd_idx_start_polygons[poly_id];
00538     }
00539 
00540     if (voronoi_centers_)
00541       voronoi_centers_->points.resize (dd);
00542   }
00543 
00544   qh_freeqhull (!qh_ALL);
00545   int curlong, totlong;
00546   qh_memfreeshort (&curlong, &totlong);
00547 
00548   Eigen::Affine3f transInverse = transform1.inverse ();
00549   pcl::transformPointCloud (alpha_shape, alpha_shape, transInverse);
00550   xyz_centroid[0] = - xyz_centroid[0];
00551   xyz_centroid[1] = - xyz_centroid[1];
00552   xyz_centroid[2] = - xyz_centroid[2];
00553   pcl::demeanPointCloud (alpha_shape, xyz_centroid, alpha_shape);
00554 
00555   // also transform voronoi_centers_...
00556   if (voronoi_centers_)
00557   {
00558     pcl::transformPointCloud (*voronoi_centers_, *voronoi_centers_, transInverse);
00559     pcl::demeanPointCloud (*voronoi_centers_, xyz_centroid, *voronoi_centers_);
00560   }
00561 
00562   if (keep_information_)
00563   {
00564     // build a tree with the original points
00565     pcl::KdTreeFLANN<PointInT> tree (true);
00566     tree.setInputCloud (input_, indices_);
00567 
00568     std::vector<int> neighbor;
00569     std::vector<float> distances;
00570     neighbor.resize (1);
00571     distances.resize (1);
00572 
00573     // for each point in the concave hull, search for the nearest neighbor in the original point cloud
00574 
00575     std::vector<int> indices;
00576     indices.resize (alpha_shape.points.size ());
00577 
00578     for (size_t i = 0; i < alpha_shape.points.size (); i++)
00579     {
00580       tree.nearestKSearch (alpha_shape.points[i], 1, neighbor, distances);
00581       indices[i] = (*indices_)[neighbor[0]];
00582     }
00583 
00584     // replace point with the closest neighbor in the original point cloud
00585     pcl::copyPointCloud (*input_, indices, alpha_shape);
00586   }
00587 }
00588 
00589 #define PCL_INSTANTIATE_ConcaveHull(T) template class PCL_EXPORTS pcl::ConcaveHull<T>;
00590 
00591 #endif    // PCL_SURFACE_IMPL_CONCAVE_HULL_H_
00592 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines