test_surface.cpp
Go to the documentation of this file.
00001 #include "surface.h"
00002 
00003 #include <string>
00004 #include <pcl/console/parse.h>
00005 #include <pcl/io/pcd_io.h>
00006 #include <pcl/visualization/pcl_visualizer.h>
00007 
00008 int 
00009 main (int argc, char ** argv)
00010 {
00011   if (argc < 2) 
00012   {
00013     pcl::console::print_info ("Syntax is: %s input.pcd <options>\n", argv[0]);
00014     pcl::console::print_info ("  where options are:\n");
00015     pcl::console::print_info ("    --surfel radius,order............... Compute surface elements\n");
00016     pcl::console::print_info ("    --convex ........................... Compute convex hull\n");
00017     pcl::console::print_info ("    --concave alpha .................... Compute concave hull\n");
00018     pcl::console::print_info ("    --greedy radius,max_nn,mu,surf_angle,min_angle,max_angle ... Compute greedy triangulation\n");
00019     pcl::console::print_info ("    -s output.pcd ...................... Save the output cloud\n");
00020 
00021     return (1);
00022   }
00023 
00024   // Load the points
00025   PointCloudPtr cloud (new PointCloud);
00026   pcl::io::loadPCDFile (argv[1], *cloud);
00027   pcl::console::print_info ("Loaded %s (%zu points)\n", argv[1], cloud->size ());
00028 
00029   // Compute surface elements
00030   SurfaceElementsPtr surfels (new SurfaceElements);
00031   double mls_radius, polynomial_order;
00032   bool compute_surface_elements = 
00033     pcl::console::parse_2x_arguments (argc, argv, "--surfel", mls_radius, polynomial_order) > 0;
00034   if (compute_surface_elements)
00035   {
00036     surfels = computeSurfaceElements (cloud, mls_radius, polynomial_order);
00037     pcl::console::print_info ("Computed surface elements\n");
00038   }
00039 
00040   // Find the convex hull
00041   MeshPtr convex_hull;
00042   bool compute_convex_hull = pcl::console::find_argument (argc, argv, "--convex") > 0;
00043   if (compute_convex_hull)
00044   {
00045     convex_hull = computeConvexHull (cloud);
00046     pcl::console::print_info ("Computed convex hull\n");
00047   }
00048 
00049   // Find the concave hull
00050   MeshPtr concave_hull;
00051   double alpha;
00052   bool compute_concave_hull = pcl::console::parse_argument (argc, argv, "--concave", alpha) > 0;
00053   if (compute_concave_hull)
00054   {
00055     concave_hull = computeConcaveHull (cloud, alpha);
00056     pcl::console::print_info ("Computed concave hull\n");
00057   }
00058 
00059   // Compute a greedy surface triangulation
00060   pcl::PolygonMesh::Ptr greedy_mesh;
00061   std::string params_string;
00062   bool perform_greedy_triangulation = pcl::console::parse_argument (argc, argv, "--greedy", params_string) > 0;
00063   if (perform_greedy_triangulation)
00064   {
00065     assert (surfels);
00066 
00067     std::vector<std::string> tokens;
00068     boost::split (tokens, params_string, boost::is_any_of (","), boost::token_compress_on);
00069     assert (tokens.size () == 6);
00070     float radius = atof(tokens[0].c_str ());
00071     int max_nearest_neighbors = atoi(tokens[1].c_str ());
00072     float mu = atof(tokens[2].c_str ());
00073     float max_surface_angle = atof(tokens[3].c_str ());
00074     float min_angle = atof(tokens[4].c_str ());
00075     float max_angle = atof(tokens[5].c_str ());
00076 
00077     greedy_mesh = greedyTriangulation (surfels, radius, max_nearest_neighbors, mu, 
00078                                        max_surface_angle, min_angle, max_angle);
00079 
00080     pcl::console::print_info ("Performed greedy surface triangulation\n");
00081   }
00082 
00083 
00084   // Compute a greedy surface triangulation
00085   pcl::PolygonMesh::Ptr marching_cubes_mesh;
00086   double leaf_size, iso_level;
00087   bool perform_marching_cubes = pcl::console::parse_2x_arguments (argc, argv, "--mc", leaf_size, iso_level) > 0;
00088   if (perform_marching_cubes)
00089   {
00090     assert (surfels);
00091 
00092     marching_cubes_mesh = marchingCubesTriangulation (surfels, leaf_size, iso_level);
00093 
00094     pcl::console::print_info ("Performed marching cubes surface triangulation\n");
00095   }
00096 
00097   // Save output
00098   std::string output_filename;
00099   bool save_output = pcl::console::parse_argument (argc, argv, "-s", output_filename) > 0;
00100   if (save_output)
00101   {
00102     // Save the result
00103     pcl::io::savePCDFile (output_filename, *cloud);
00104 
00105     pcl::console::print_info ("Saved result as %s\n", output_filename.c_str ());    
00106   }
00107   // Or visualize the result
00108   else
00109   {
00110     pcl::console::print_info ("Starting visualizer... Close window to exit.\n");
00111     pcl::visualization::PCLVisualizer vis;
00112     vis.addPointCloud (cloud);
00113     vis.resetCamera ();
00114 
00115     if (compute_convex_hull)
00116     {
00117       vis.addPolygonMesh<PointT> (convex_hull->points, convex_hull->faces, "convex_hull");
00118     }
00119     if (compute_concave_hull)
00120     {
00121       vis.addPolygonMesh<PointT> (concave_hull->points, concave_hull->faces, "concave_hull");
00122     }
00123     if (perform_greedy_triangulation)
00124     {
00125       vis.addPolygonMesh(*greedy_mesh, "greedy_mesh");
00126     }
00127     if (perform_marching_cubes)
00128     {
00129       vis.addPolygonMesh(*marching_cubes_mesh, "marching_cubes_mesh");
00130     }
00131 
00132     vis.spin ();
00133   }
00134 }


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:18:37