Point Cloud Library (PCL)
1.3.1
|
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