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