Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2012, Willow Garage, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of Willow Garage, Inc. nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * $Id$ 00037 * 00038 */ 00039 00040 #include <pcl/pcl_config.h> 00041 #ifdef HAVE_QHULL 00042 00043 #ifndef PCL_SURFACE_IMPL_CONVEX_HULL_H_ 00044 #define PCL_SURFACE_IMPL_CONVEX_HULL_H_ 00045 00046 #include <pcl/surface/convex_hull.h> 00047 #include <pcl/common/common.h> 00048 #include <pcl/common/eigen.h> 00049 #include <pcl/common/transforms.h> 00050 #include <pcl/common/io.h> 00051 #include <stdio.h> 00052 #include <stdlib.h> 00053 #include <pcl/surface/qhull.h> 00054 00055 ////////////////////////////////////////////////////////////////////////// 00056 template <typename PointInT> void 00057 pcl::ConvexHull<PointInT>::calculateInputDimension () 00058 { 00059 PCL_DEBUG ("[pcl::%s::calculateInputDimension] WARNING: Input dimension not specified. Automatically determining input dimension.\n", getClassName ().c_str ()); 00060 EIGEN_ALIGN16 Eigen::Matrix3d covariance_matrix; 00061 Eigen::Vector4d xyz_centroid; 00062 computeMeanAndCovarianceMatrix (*input_, *indices_, covariance_matrix, xyz_centroid); 00063 00064 EIGEN_ALIGN16 Eigen::Vector3d eigen_values; 00065 pcl::eigen33 (covariance_matrix, eigen_values); 00066 00067 if (eigen_values[0] / eigen_values[2] < 1.0e-3) 00068 dimension_ = 2; 00069 else 00070 dimension_ = 3; 00071 } 00072 00073 ////////////////////////////////////////////////////////////////////////// 00074 template <typename PointInT> void 00075 pcl::ConvexHull<PointInT>::performReconstruction2D (PointCloud &hull, std::vector<pcl::Vertices> &polygons, 00076 bool) 00077 { 00078 int dimension = 2; 00079 bool xy_proj_safe = true; 00080 bool yz_proj_safe = true; 00081 bool xz_proj_safe = true; 00082 00083 // Check the input's normal to see which projection to use 00084 PointInT p0 = input_->points[(*indices_)[0]]; 00085 PointInT p1 = input_->points[(*indices_)[indices_->size () - 1]]; 00086 PointInT p2 = input_->points[(*indices_)[indices_->size () / 2]]; 00087 Eigen::Array4f dy1dy2 = (p1.getArray4fMap () - p0.getArray4fMap ()) / (p2.getArray4fMap () - p0.getArray4fMap ()); 00088 while (!( (dy1dy2[0] != dy1dy2[1]) || (dy1dy2[2] != dy1dy2[1]) ) ) 00089 { 00090 p0 = input_->points[(*indices_)[rand () % indices_->size ()]]; 00091 p1 = input_->points[(*indices_)[rand () % indices_->size ()]]; 00092 p2 = input_->points[(*indices_)[rand () % indices_->size ()]]; 00093 dy1dy2 = (p1.getArray4fMap () - p0.getArray4fMap ()) / (p2.getArray4fMap () - p0.getArray4fMap ()); 00094 } 00095 00096 pcl::PointCloud<PointInT> normal_calc_cloud; 00097 normal_calc_cloud.points.resize (3); 00098 normal_calc_cloud.points[0] = p0; 00099 normal_calc_cloud.points[1] = p1; 00100 normal_calc_cloud.points[2] = p2; 00101 00102 Eigen::Vector4d normal_calc_centroid; 00103 Eigen::Matrix3d normal_calc_covariance; 00104 pcl::computeMeanAndCovarianceMatrix (normal_calc_cloud, normal_calc_covariance, normal_calc_centroid); 00105 // Need to set -1 here. See eigen33 for explanations. 00106 Eigen::Vector3d::Scalar eigen_value; 00107 Eigen::Vector3d plane_params; 00108 pcl::eigen33 (normal_calc_covariance, eigen_value, plane_params); 00109 float theta_x = fabsf (static_cast<float> (plane_params.dot (x_axis_))); 00110 float theta_y = fabsf (static_cast<float> (plane_params.dot (y_axis_))); 00111 float theta_z = fabsf (static_cast<float> (plane_params.dot (z_axis_))); 00112 00113 // Check for degenerate cases of each projection 00114 // We must avoid projections in which the plane projects as a line 00115 if (theta_z > projection_angle_thresh_) 00116 { 00117 xz_proj_safe = false; 00118 yz_proj_safe = false; 00119 } 00120 if (theta_x > projection_angle_thresh_) 00121 { 00122 xz_proj_safe = false; 00123 xy_proj_safe = false; 00124 } 00125 if (theta_y > projection_angle_thresh_) 00126 { 00127 xy_proj_safe = false; 00128 yz_proj_safe = false; 00129 } 00130 00131 // True if qhull should free points in qh_freeqhull() or reallocation 00132 boolT ismalloc = True; 00133 // output from qh_produce_output(), use NULL to skip qh_produce_output() 00134 FILE *outfile = NULL; 00135 00136 #ifndef HAVE_QHULL_2011 00137 if (compute_area_) 00138 outfile = stderr; 00139 #endif 00140 00141 // option flags for qhull, see qh_opt.htm 00142 const char* flags = qhull_flags.c_str (); 00143 // error messages from qhull code 00144 FILE *errfile = stderr; 00145 00146 // Array of coordinates for each point 00147 coordT *points = reinterpret_cast<coordT*> (calloc (indices_->size () * dimension, sizeof (coordT))); 00148 00149 // Build input data, using appropriate projection 00150 int j = 0; 00151 if (xy_proj_safe) 00152 { 00153 for (size_t i = 0; i < indices_->size (); ++i, j+=dimension) 00154 { 00155 points[j + 0] = static_cast<coordT> (input_->points[(*indices_)[i]].x); 00156 points[j + 1] = static_cast<coordT> (input_->points[(*indices_)[i]].y); 00157 } 00158 } 00159 else if (yz_proj_safe) 00160 { 00161 for (size_t i = 0; i < indices_->size (); ++i, j+=dimension) 00162 { 00163 points[j + 0] = static_cast<coordT> (input_->points[(*indices_)[i]].y); 00164 points[j + 1] = static_cast<coordT> (input_->points[(*indices_)[i]].z); 00165 } 00166 } 00167 else if (xz_proj_safe) 00168 { 00169 for (size_t i = 0; i < indices_->size (); ++i, j+=dimension) 00170 { 00171 points[j + 0] = static_cast<coordT> (input_->points[(*indices_)[i]].x); 00172 points[j + 1] = static_cast<coordT> (input_->points[(*indices_)[i]].z); 00173 } 00174 } 00175 else 00176 { 00177 // This should only happen if we had invalid input 00178 PCL_ERROR ("[pcl::%s::performReconstruction2D] Invalid input!\n", getClassName ().c_str ()); 00179 } 00180 00181 // Compute convex hull 00182 int exitcode = qh_new_qhull (dimension, static_cast<int> (indices_->size ()), points, ismalloc, const_cast<char*> (flags), outfile, errfile); 00183 #ifdef HAVE_QHULL_2011 00184 if (compute_area_) 00185 { 00186 qh_prepare_output(); 00187 } 00188 #endif 00189 00190 // 0 if no error from qhull or it doesn't find any vertices 00191 if (exitcode != 0 || qh num_vertices == 0) 00192 { 00193 PCL_ERROR ("[pcl::%s::performReconstrution2D] ERROR: qhull was unable to compute a convex hull for the given point cloud (%zu)!\n", getClassName ().c_str (), indices_->size ()); 00194 00195 hull.points.resize (0); 00196 hull.width = hull.height = 0; 00197 polygons.resize (0); 00198 00199 qh_freeqhull (!qh_ALL); 00200 int curlong, totlong; 00201 qh_memfreeshort (&curlong, &totlong); 00202 00203 return; 00204 } 00205 00206 // Qhull returns the area in volume for 2D 00207 if (compute_area_) 00208 { 00209 total_area_ = qh totvol; 00210 total_volume_ = 0.0; 00211 } 00212 00213 int num_vertices = qh num_vertices; 00214 hull.points.resize (num_vertices); 00215 memset (&hull.points[0], static_cast<int> (hull.points.size ()), sizeof (PointInT)); 00216 00217 vertexT * vertex; 00218 int i = 0; 00219 00220 std::vector<std::pair<int, Eigen::Vector4f>, Eigen::aligned_allocator<std::pair<int, Eigen::Vector4f> > > idx_points (num_vertices); 00221 idx_points.resize (hull.points.size ()); 00222 memset (&idx_points[0], static_cast<int> (hull.points.size ()), sizeof (std::pair<int, Eigen::Vector4f>)); 00223 00224 FORALLvertices 00225 { 00226 hull.points[i] = input_->points[(*indices_)[qh_pointid (vertex->point)]]; 00227 idx_points[i].first = qh_pointid (vertex->point); 00228 ++i; 00229 } 00230 00231 // Sort 00232 Eigen::Vector4f centroid; 00233 pcl::compute3DCentroid (hull, centroid); 00234 if (xy_proj_safe) 00235 { 00236 for (size_t j = 0; j < hull.points.size (); j++) 00237 { 00238 idx_points[j].second[0] = hull.points[j].x - centroid[0]; 00239 idx_points[j].second[1] = hull.points[j].y - centroid[1]; 00240 } 00241 } 00242 else if (yz_proj_safe) 00243 { 00244 for (size_t j = 0; j < hull.points.size (); j++) 00245 { 00246 idx_points[j].second[0] = hull.points[j].y - centroid[1]; 00247 idx_points[j].second[1] = hull.points[j].z - centroid[2]; 00248 } 00249 } 00250 else if (xz_proj_safe) 00251 { 00252 for (size_t j = 0; j < hull.points.size (); j++) 00253 { 00254 idx_points[j].second[0] = hull.points[j].x - centroid[0]; 00255 idx_points[j].second[1] = hull.points[j].z - centroid[2]; 00256 } 00257 } 00258 std::sort (idx_points.begin (), idx_points.end (), comparePoints2D); 00259 00260 polygons.resize (1); 00261 polygons[0].vertices.resize (hull.points.size ()); 00262 00263 for (int j = 0; j < static_cast<int> (hull.points.size ()); j++) 00264 { 00265 hull.points[j] = input_->points[(*indices_)[idx_points[j].first]]; 00266 polygons[0].vertices[j] = static_cast<unsigned int> (j); 00267 } 00268 00269 qh_freeqhull (!qh_ALL); 00270 int curlong, totlong; 00271 qh_memfreeshort (&curlong, &totlong); 00272 00273 hull.width = static_cast<uint32_t> (hull.points.size ()); 00274 hull.height = 1; 00275 hull.is_dense = false; 00276 return; 00277 } 00278 00279 #ifdef __GNUC__ 00280 #pragma GCC diagnostic ignored "-Wold-style-cast" 00281 #endif 00282 ////////////////////////////////////////////////////////////////////////// 00283 template <typename PointInT> void 00284 pcl::ConvexHull<PointInT>::performReconstruction3D ( 00285 PointCloud &hull, std::vector<pcl::Vertices> &polygons, bool fill_polygon_data) 00286 { 00287 int dimension = 3; 00288 00289 // True if qhull should free points in qh_freeqhull() or reallocation 00290 boolT ismalloc = True; 00291 // output from qh_produce_output(), use NULL to skip qh_produce_output() 00292 FILE *outfile = NULL; 00293 00294 #ifndef HAVE_QHULL_2011 00295 if (compute_area_) 00296 outfile = stderr; 00297 #endif 00298 00299 // option flags for qhull, see qh_opt.htm 00300 const char *flags = qhull_flags.c_str (); 00301 // error messages from qhull code 00302 FILE *errfile = stderr; 00303 00304 // Array of coordinates for each point 00305 coordT *points = reinterpret_cast<coordT*> (calloc (indices_->size () * dimension, sizeof (coordT))); 00306 00307 int j = 0; 00308 for (size_t i = 0; i < indices_->size (); ++i, j+=dimension) 00309 { 00310 points[j + 0] = static_cast<coordT> (input_->points[(*indices_)[i]].x); 00311 points[j + 1] = static_cast<coordT> (input_->points[(*indices_)[i]].y); 00312 points[j + 2] = static_cast<coordT> (input_->points[(*indices_)[i]].z); 00313 } 00314 00315 // Compute convex hull 00316 int exitcode = qh_new_qhull (dimension, static_cast<int> (indices_->size ()), points, ismalloc, const_cast<char*> (flags), outfile, errfile); 00317 #ifdef HAVE_QHULL_2011 00318 if (compute_area_) 00319 { 00320 qh_prepare_output(); 00321 } 00322 #endif 00323 00324 // 0 if no error from qhull 00325 if (exitcode != 0) 00326 { 00327 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 ()); 00328 00329 hull.points.resize (0); 00330 hull.width = hull.height = 0; 00331 polygons.resize (0); 00332 00333 qh_freeqhull (!qh_ALL); 00334 int curlong, totlong; 00335 qh_memfreeshort (&curlong, &totlong); 00336 00337 return; 00338 } 00339 00340 qh_triangulate (); 00341 00342 int num_facets = qh num_facets; 00343 00344 int num_vertices = qh num_vertices; 00345 hull.points.resize (num_vertices); 00346 00347 vertexT * vertex; 00348 int i = 0; 00349 // Max vertex id 00350 unsigned int max_vertex_id = 0; 00351 FORALLvertices 00352 { 00353 if (vertex->id + 1 > max_vertex_id) 00354 max_vertex_id = vertex->id + 1; 00355 } 00356 00357 ++max_vertex_id; 00358 std::vector<int> qhid_to_pcidx (max_vertex_id); 00359 00360 FORALLvertices 00361 { 00362 // Add vertices to hull point_cloud 00363 hull.points[i] = input_->points[(*indices_)[qh_pointid (vertex->point)]]; 00364 00365 qhid_to_pcidx[vertex->id] = i; // map the vertex id of qhull to the point cloud index 00366 ++i; 00367 } 00368 00369 if (compute_area_) 00370 { 00371 total_area_ = qh totarea; 00372 total_volume_ = qh totvol; 00373 } 00374 00375 if (fill_polygon_data) 00376 { 00377 polygons.resize (num_facets); 00378 int dd = 0; 00379 00380 facetT * facet; 00381 FORALLfacets 00382 { 00383 polygons[dd].vertices.resize (3); 00384 00385 // Needed by FOREACHvertex_i_ 00386 int vertex_n, vertex_i; 00387 FOREACHvertex_i_ ((*facet).vertices) 00388 //facet_vertices.vertices.push_back (qhid_to_pcidx[vertex->id]); 00389 polygons[dd].vertices[vertex_i] = qhid_to_pcidx[vertex->id]; 00390 ++dd; 00391 } 00392 } 00393 // Deallocates memory (also the points) 00394 qh_freeqhull (!qh_ALL); 00395 int curlong, totlong; 00396 qh_memfreeshort (&curlong, &totlong); 00397 00398 hull.width = static_cast<uint32_t> (hull.points.size ()); 00399 hull.height = 1; 00400 hull.is_dense = false; 00401 } 00402 #ifdef __GNUC__ 00403 #pragma GCC diagnostic warning "-Wold-style-cast" 00404 #endif 00405 00406 ////////////////////////////////////////////////////////////////////////// 00407 template <typename PointInT> void 00408 pcl::ConvexHull<PointInT>::performReconstruction (PointCloud &hull, std::vector<pcl::Vertices> &polygons, 00409 bool fill_polygon_data) 00410 { 00411 if (dimension_ == 0) 00412 calculateInputDimension (); 00413 if (dimension_ == 2) 00414 performReconstruction2D (hull, polygons, fill_polygon_data); 00415 else if (dimension_ == 3) 00416 performReconstruction3D (hull, polygons, fill_polygon_data); 00417 else 00418 PCL_ERROR ("[pcl::%s::performReconstruction] Error: invalid input dimension requested: %d\n",getClassName ().c_str (),dimension_); 00419 } 00420 00421 ////////////////////////////////////////////////////////////////////////// 00422 template <typename PointInT> void 00423 pcl::ConvexHull<PointInT>::reconstruct (PointCloud &points) 00424 { 00425 points.header = input_->header; 00426 if (!initCompute () || input_->points.empty () || indices_->empty ()) 00427 { 00428 points.points.clear (); 00429 return; 00430 } 00431 00432 // Perform the actual surface reconstruction 00433 std::vector<pcl::Vertices> polygons; 00434 performReconstruction (points, polygons, false); 00435 00436 points.width = static_cast<uint32_t> (points.points.size ()); 00437 points.height = 1; 00438 points.is_dense = true; 00439 00440 deinitCompute (); 00441 } 00442 00443 00444 ////////////////////////////////////////////////////////////////////////// 00445 template <typename PointInT> void 00446 pcl::ConvexHull<PointInT>::performReconstruction (PolygonMesh &output) 00447 { 00448 // Perform reconstruction 00449 pcl::PointCloud<PointInT> hull_points; 00450 performReconstruction (hull_points, output.polygons, true); 00451 00452 // Convert the PointCloud into a PCLPointCloud2 00453 pcl::toPCLPointCloud2 (hull_points, output.cloud); 00454 } 00455 00456 ////////////////////////////////////////////////////////////////////////// 00457 template <typename PointInT> void 00458 pcl::ConvexHull<PointInT>::performReconstruction (std::vector<pcl::Vertices> &polygons) 00459 { 00460 pcl::PointCloud<PointInT> hull_points; 00461 performReconstruction (hull_points, polygons, true); 00462 } 00463 00464 ////////////////////////////////////////////////////////////////////////// 00465 template <typename PointInT> void 00466 pcl::ConvexHull<PointInT>::reconstruct (PointCloud &points, std::vector<pcl::Vertices> &polygons) 00467 { 00468 points.header = input_->header; 00469 if (!initCompute () || input_->points.empty () || indices_->empty ()) 00470 { 00471 points.points.clear (); 00472 return; 00473 } 00474 00475 // Perform the actual surface reconstruction 00476 performReconstruction (points, polygons, true); 00477 00478 points.width = static_cast<uint32_t> (points.points.size ()); 00479 points.height = 1; 00480 points.is_dense = true; 00481 00482 deinitCompute (); 00483 } 00484 00485 #define PCL_INSTANTIATE_ConvexHull(T) template class PCL_EXPORTS pcl::ConvexHull<T>; 00486 00487 #endif // PCL_SURFACE_IMPL_CONVEX_HULL_H_ 00488 #endif