Point Cloud Library (PCL)  1.3.1
convex_hull.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: convex_hull.hpp 3031 2011-11-01 04:25:15Z rusu $
00035  *
00036  */
00037 
00038 #include <pcl/pcl_config.h>
00039 #ifdef HAVE_QHULL
00040 
00041 #ifndef PCL_SURFACE_IMPL_CONVEX_HULL_H_
00042 #define PCL_SURFACE_IMPL_CONVEX_HULL_H_
00043 
00044 #include "pcl/surface/convex_hull.h"
00045 #include <pcl/common/common.h>
00046 #include <pcl/common/eigen.h>
00047 #include <pcl/common/transforms.h>
00048 #include <pcl/common/io.h>
00049 #include <pcl/kdtree/kdtree.h>
00050 #include <pcl/kdtree/kdtree_flann.h>
00051 #include <stdio.h>
00052 #include <stdlib.h>
00053 
00054 extern "C"
00055 {
00056 #ifdef HAVE_QHULL_2011
00057 #  include "libqhull/libqhull.h"
00058 #  include "libqhull/mem.h"
00059 #  include "libqhull/qset.h"
00060 #  include "libqhull/geom.h"
00061 #  include "libqhull/merge.h"
00062 #  include "libqhull/poly.h"
00063 #  include "libqhull/io.h"
00064 #  include "libqhull/stat.h"
00065 #else
00066 #  include "qhull/qhull.h"
00067 #  include "qhull/mem.h"
00068 #  include "qhull/qset.h"
00069 #  include "qhull/geom.h"
00070 #  include "qhull/merge.h"
00071 #  include "qhull/poly.h"
00072 #  include "qhull/io.h"
00073 #  include "qhull/stat.h"
00074 #endif
00075 }
00076 
00078 template <typename PointInT> void
00079 pcl::ConvexHull<PointInT>::performReconstruction (PointCloud &hull, std::vector<pcl::Vertices> &polygons,
00080                                                   bool fill_polygon_data)
00081 {
00082   // FInd the principal directions
00083   EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
00084   Eigen::Vector4f xyz_centroid;
00085   compute3DCentroid (*input_, *indices_, xyz_centroid);
00086   computeCovarianceMatrix (*input_, *indices_, xyz_centroid, covariance_matrix);
00087   EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
00088   EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
00089   pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);
00090 
00091   Eigen::Affine3f transform1;
00092   transform1.setIdentity ();
00093   int dim = 3;
00094 
00095   if (eigen_values[0] / eigen_values[2] < 1.0e-5)
00096   {
00097     // We have points laying on a plane, using 2d convex hull
00098     // Compute transformation bring eigen_vectors.col(i) to z-axis
00099 
00100     eigen_vectors.col (2) = eigen_vectors.col (0).cross (eigen_vectors.col (1));
00101     eigen_vectors.col (1) = eigen_vectors.col (2).cross (eigen_vectors.col (0));
00102 
00103     transform1 (0, 2) = eigen_vectors (0, 0);
00104     transform1 (1, 2) = eigen_vectors (1, 0);
00105     transform1 (2, 2) = eigen_vectors (2, 0);
00106 
00107     transform1 (0, 1) = eigen_vectors (0, 1);
00108     transform1 (1, 1) = eigen_vectors (1, 1);
00109     transform1 (2, 1) = eigen_vectors (2, 1);
00110     transform1 (0, 0) = eigen_vectors (0, 2);
00111     transform1 (1, 0) = eigen_vectors (1, 2);
00112     transform1 (2, 0) = eigen_vectors (2, 2);
00113 
00114     transform1 = transform1.inverse ();
00115     dim = 2;
00116   }
00117   else
00118     transform1.setIdentity ();
00119 
00120   PointCloud cloud_transformed;
00121   pcl::demeanPointCloud (*input_, *indices_, xyz_centroid, cloud_transformed);
00122   pcl::transformPointCloud (cloud_transformed, cloud_transformed, transform1);
00123 
00124   // True if qhull should free points in qh_freeqhull() or reallocation
00125   boolT ismalloc = True;
00126   // output from qh_produce_output(), use NULL to skip qh_produce_output()
00127   FILE *outfile = NULL;
00128 
00129   std::string flags_str;
00130   flags_str = "qhull Tc";
00131 
00132   if (compute_area_)
00133   {
00134     flags_str.append (" FA");
00135     outfile = stderr;
00136   }
00137 
00138   // option flags for qhull, see qh_opt.htm
00139   //char flags[] = "qhull Tc FA";
00140   char * flags = (char *)flags_str.c_str();
00141   // error messages from qhull code
00142   FILE *errfile = stderr;
00143   // 0 if no error from qhull
00144   int exitcode;
00145 
00146   // Array of coordinates for each point
00147   coordT *points = (coordT *)calloc (cloud_transformed.points.size () * dim, sizeof(coordT));
00148 
00149   for (size_t i = 0; i < cloud_transformed.points.size (); ++i)
00150   {
00151     points[i * dim + 0] = (coordT)cloud_transformed.points[i].x;
00152     points[i * dim + 1] = (coordT)cloud_transformed.points[i].y;
00153 
00154     if (dim > 2)
00155       points[i * dim + 2] = (coordT)cloud_transformed.points[i].z;
00156   }
00157 
00158   // Compute convex hull
00159   exitcode = qh_new_qhull (dim, cloud_transformed.points.size (), points, ismalloc, flags, outfile, errfile);
00160 
00161   if (exitcode != 0)
00162   {
00163     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 ());
00164 
00165     //check if it fails because of NaN values...
00166     if (!cloud_transformed.is_dense)
00167     {
00168       bool NaNvalues = false;
00169       for (size_t i = 0; i < cloud_transformed.size (); ++i)
00170       {
00171         if (!pcl_isfinite (cloud_transformed.points[i].x) || 
00172             !pcl_isfinite (cloud_transformed.points[i].y) ||
00173             !pcl_isfinite (cloud_transformed.points[i].z))
00174         {
00175           NaNvalues = true;
00176           break;
00177         }
00178       }
00179 
00180       if (NaNvalues)
00181         PCL_ERROR ("[pcl::%s::performReconstruction] ERROR: point cloud contains NaN values, consider running pcl::PassThrough filter first to remove NaNs!\n", getClassName ().c_str ());
00182     }
00183 
00184     hull.points.resize (0);
00185     hull.width = hull.height = 0;
00186     polygons.resize (0);
00187 
00188     qh_freeqhull (!qh_ALL);
00189     int curlong, totlong;
00190     qh_memfreeshort (&curlong, &totlong);
00191 
00192     return;
00193   }
00194 
00195   qh_triangulate ();
00196 
00197   int num_facets = qh num_facets;
00198 
00199   int num_vertices = qh num_vertices;
00200   hull.points.resize (num_vertices);
00201 
00202   vertexT * vertex;
00203   int i = 0;
00204   // Max vertex id
00205   int max_vertex_id = -1;
00206   FORALLvertices
00207   {
00208     if ((int)vertex->id > max_vertex_id)
00209     max_vertex_id = vertex->id;
00210   }
00211 
00212   ++max_vertex_id;
00213   std::vector<int> qhid_to_pcidx (max_vertex_id);
00214 
00215   FORALLvertices
00216   {
00217     // Add vertices to hull point_cloud
00218     hull.points[i].x = vertex->point[0];
00219     hull.points[i].y = vertex->point[1];
00220 
00221     if (dim>2)
00222     hull.points[i].z = vertex->point[2];
00223     else
00224     hull.points[i].z = 0;
00225 
00226     qhid_to_pcidx[vertex->id] = i; // map the vertex id of qhull to the point cloud index
00227     ++i;
00228   }
00229 
00230   if (compute_area_)
00231   {
00232     total_area_  = qh totarea;
00233     total_volume_ = qh totvol;
00234   }
00235 
00236   if (fill_polygon_data)
00237   {
00238     if (dim == 3)
00239     {
00240       polygons.resize (num_facets);
00241       int dd = 0;
00242 
00243       facetT * facet;
00244       FORALLfacets
00245       {
00246         polygons[dd].vertices.resize (3);
00247 
00248         // Needed by FOREACHvertex_i_
00249         int vertex_n, vertex_i;
00250         FOREACHvertex_i_((*facet).vertices)
00251         //facet_vertices.vertices.push_back (qhid_to_pcidx[vertex->id]);
00252         polygons[dd].vertices[vertex_i] = qhid_to_pcidx[vertex->id];
00253         ++dd;
00254       }
00255 
00256 
00257     }
00258     else
00259     {
00260       // dim=2, we want to return just a polygon with all vertices sorted
00261       // so that they form a non-intersecting polygon...
00262       // this can be moved to the upper loop probably and here just
00263       // the sorting + populate
00264 
00265       Eigen::Vector4f centroid;
00266       pcl::compute3DCentroid (hull, centroid);
00267       centroid[3] = 0;
00268       polygons.resize (1);
00269 
00270       int num_vertices = qh num_vertices, dd = 0;
00271 
00272       // Copy all vertices
00273       std::vector<std::pair<int, Eigen::Vector4f>, Eigen::aligned_allocator<std::pair<int, Eigen::Vector4f> > > idx_points (num_vertices);
00274 
00275       FORALLvertices
00276       {
00277         idx_points[dd].first = qhid_to_pcidx[vertex->id];
00278         idx_points[dd].second = hull.points[idx_points[dd].first].getVector4fMap () - centroid;
00279         ++dd;
00280       }
00281 
00282       // Sort idx_points
00283       std::sort (idx_points.begin (), idx_points.end (), comparePoints2D);
00284       polygons[0].vertices.resize (idx_points.size () + 1);
00285 
00286       //Sort also points...
00287       PointCloud hull_sorted;
00288       hull_sorted.points.resize (hull.points.size ());
00289 
00290       for (size_t j = 0; j < idx_points.size (); ++j)
00291       hull_sorted.points[j] = hull.points[idx_points[j].first];
00292 
00293       hull.points = hull_sorted.points;
00294 
00295       // Populate points
00296       for (size_t j = 0; j < idx_points.size (); ++j)
00297       polygons[0].vertices[j] = j;
00298 
00299       polygons[0].vertices[idx_points.size ()] = 0;
00300     }
00301   }
00302   else
00303   {
00304     if (dim == 2)
00305     {
00306       // We want to sort the points
00307       Eigen::Vector4f centroid;
00308       pcl::compute3DCentroid (hull, centroid);
00309       centroid[3] = 0;
00310       polygons.resize (1);
00311 
00312       int num_vertices = qh num_vertices, dd = 0;
00313 
00314       // Copy all vertices
00315       std::vector<std::pair<int, Eigen::Vector4f>, Eigen::aligned_allocator<std::pair<int, Eigen::Vector4f> > > idx_points (num_vertices);
00316 
00317       FORALLvertices
00318       {
00319         idx_points[dd].first = qhid_to_pcidx[vertex->id];
00320         idx_points[dd].second = hull.points[idx_points[dd].first].getVector4fMap () - centroid;
00321         ++dd;
00322       }
00323 
00324       // Sort idx_points
00325       std::sort (idx_points.begin (), idx_points.end (), comparePoints2D);
00326 
00327       //Sort also points...
00328       PointCloud hull_sorted;
00329       hull_sorted.points.resize (hull.points.size ());
00330 
00331       for (size_t j = 0; j < idx_points.size (); ++j)
00332       hull_sorted.points[j] = hull.points[idx_points[j].first];
00333 
00334       hull.points = hull_sorted.points;
00335     }
00336   }
00337 
00338   // Deallocates memory (also the points)
00339   qh_freeqhull(!qh_ALL);
00340   int curlong, totlong;
00341   qh_memfreeshort (&curlong, &totlong);
00342 
00343   // Rotate the hull point cloud by transform's inverse
00344   // If the input point cloud has been rotated
00345   if (dim == 2)
00346   {
00347     Eigen::Affine3f transInverse = transform1.inverse ();
00348     pcl::transformPointCloud (hull, hull, transInverse);
00349 
00350     //for 2D sets, the qhull library delivers the actual area of the 2d hull in the volume
00351     if(compute_area_)
00352     {
00353       total_area_ = total_volume_;
00354       total_volume_ = 0.0;
00355     }
00356 
00357   }
00358 
00359   xyz_centroid[0] = -xyz_centroid[0];
00360   xyz_centroid[1] = -xyz_centroid[1];
00361   xyz_centroid[2] = -xyz_centroid[2];
00362   pcl::demeanPointCloud (hull, xyz_centroid, hull);
00363 
00364   //if keep_information_
00365   if (keep_information_)
00366   {
00367     //build a tree with the original points
00368     pcl::KdTreeFLANN<PointInT> tree (true);
00369     tree.setInputCloud (input_, indices_);
00370 
00371     std::vector<int> neighbor;
00372     std::vector<float> distances;
00373     neighbor.resize (1);
00374     distances.resize (1);
00375 
00376     //for each point in the convex hull, search for the nearest neighbor
00377 
00378     std::vector<int> indices;
00379     indices.resize (hull.points.size ());
00380 
00381     for (size_t i = 0; i < hull.points.size (); i++)
00382     {
00383       tree.nearestKSearch (hull.points[i], 1, neighbor, distances);
00384       indices[i] = (*indices_)[neighbor[0]];
00385     }
00386 
00387     //replace point with the closest neighbor in the original point cloud
00388     pcl::copyPointCloud (*input_, indices, hull);
00389   }
00390 
00391   hull.width = hull.points.size ();
00392   hull.height = 1;
00393   hull.is_dense = false;
00394 }
00395 
00397 template <typename PointInT> void
00398 pcl::ConvexHull<PointInT>::reconstruct (PointCloud &output)
00399 {
00400   output.header = input_->header;
00401   if (!initCompute ())
00402   {
00403     output.points.clear ();
00404     return;
00405   }
00406 
00407   // Perform the actual surface reconstruction
00408   std::vector<pcl::Vertices> polygons;
00409   performReconstruction (output, polygons, false);
00410 
00411   output.width = output.points.size ();
00412   output.height = 1;
00413   output.is_dense = true;
00414 
00415   deinitCompute ();
00416 }
00417 
00419 template <typename PointInT> void
00420 pcl::ConvexHull<PointInT>::reconstruct (PointCloud &points, std::vector<pcl::Vertices> &polygons)
00421 {
00422   points.header = input_->header;
00423   if (!initCompute ())
00424   {
00425     points.points.clear ();
00426     return;
00427   }
00428 
00429   // Perform the actual surface reconstruction
00430   performReconstruction (points, polygons, true);
00431 
00432   points.width = points.points.size ();
00433   points.height = 1;
00434   points.is_dense = true;
00435 
00436   deinitCompute ();
00437 }
00438 
00439 #define PCL_INSTANTIATE_ConvexHull(T) template class PCL_EXPORTS pcl::ConvexHull<T>;
00440 
00441 #endif    // PCL_SURFACE_IMPL_CONVEX_HULL_H_
00442 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines