marching_cubes_reconstruction.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, 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: marching_cubes_reconstruction.cpp 5895 2012-06-12 12:59:02Z aichim $
00035  *
00036  */
00037 
00038 #include <sensor_msgs/PointCloud2.h>
00039 #include <pcl/io/pcd_io.h>
00040 #include <pcl/io/vtk_io.h>
00041 #include <pcl/surface/marching_cubes_hoppe.h>
00042 #include <pcl/surface/marching_cubes_rbf.h>
00043 #include <pcl/console/print.h>
00044 #include <pcl/console/parse.h>
00045 #include <pcl/console/time.h>
00046 
00047 using namespace pcl;
00048 using namespace pcl::io;
00049 using namespace pcl::console;
00050 
00051 float default_iso_level = 0.0f;
00052 int default_hoppe_or_rbf = 0;
00053 float default_extend_percentage = 0.0f;
00054 int default_grid_res = 50;
00055 float default_off_surface_displacement = 0.01f;
00056 
00057 void
00058 printHelp (int, char **argv)
00059 {
00060   print_error ("Syntax is: %s input.pcd output.vtk <options>\n", argv[0]);
00061   print_info ("  where options are:\n");
00062   print_info ("                     -grid_res X     = the resolution of the grid (cubic grid) (default: ");
00063   print_value ("%d", default_grid_res); print_info (")\n");
00064   print_info ("                     -iso_level X    = the iso level of the surface to be extracted (default: ");
00065   print_value ("%f", default_iso_level); print_info (")\n");
00066   print_info ("                     -hoppe X        = use the Hoppe signed distance function (MarchingCubesHoppe\n");
00067   print_info ("                     -rbf X          = use the RBF signed distance function (MarchingCubesRBF\n");
00068   print_info ("                     -extend X       = the percentage of the bounding box to extend the grid by (default: ");
00069   print_value ("%f", default_extend_percentage); print_info (")\n");
00070   print_info ("                     -displacement X = the displacement value for the off-surface points (only for RBF) (default: ");
00071   print_value ("%f", default_off_surface_displacement); print_info (")\n");
00072 }
00073 
00074 bool
00075 loadCloud (const std::string &filename, sensor_msgs::PointCloud2 &cloud)
00076 {
00077   TicToc tt;
00078   print_highlight ("Loading "); print_value ("%s ", filename.c_str ());
00079 
00080   tt.tic ();
00081   if (loadPCDFile (filename, cloud) < 0)
00082     return (false);
00083   print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]\n");
00084   print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (cloud).c_str ());
00085 
00086   return (true);
00087 }
00088 
00089 void
00090 compute (const sensor_msgs::PointCloud2::ConstPtr &input, PolygonMesh &output,
00091          int hoppe_or_rbf, float iso_level, int grid_res, float extend_percentage, float off_surface_displacement)
00092 {
00093   PointCloud<PointNormal>::Ptr xyz_cloud (new pcl::PointCloud<PointNormal> ());
00094   fromROSMsg (*input, *xyz_cloud);
00095 
00096   MarchingCubes<PointNormal> *mc;
00097   if (hoppe_or_rbf == 0)
00098     mc = new MarchingCubesHoppe<PointNormal> ();
00099   else
00100   {
00101     mc = new MarchingCubesRBF<PointNormal> ();
00102     ((MarchingCubesRBF<PointNormal>*) mc)->setOffSurfaceDisplacement (off_surface_displacement);
00103   }
00104 
00105   mc->setIsoLevel (iso_level);
00106   mc->setGridResolution (grid_res, grid_res, grid_res);
00107   mc->setPercentageExtendGrid (extend_percentage);
00108   mc->setInputCloud (xyz_cloud);
00109 
00110   TicToc tt;
00111   tt.tic ();
00112 
00113   print_highlight ("Computing ");
00114   mc->reconstruct (output);
00115   delete mc;
00116 
00117   print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms]\n");
00118 }
00119 
00120 void
00121 saveCloud (const std::string &filename, const PolygonMesh &output)
00122 {
00123   TicToc tt;
00124   tt.tic ();
00125 
00126   print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
00127   saveVTKFile (filename, output);
00128 
00129   print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms]\n");
00130 }
00131 
00132 /* ---[ */
00133 int
00134 main (int argc, char** argv)
00135 {
00136   print_info ("Compute the surface reconstruction of a point cloud using the marching cubes algorithm (pcl::surface::MarchingCubesHoppe or pcl::surface::MarchingCubesRBF. For more information, use: %s -h\n", argv[0]);
00137 
00138   if (argc < 3)
00139   {
00140     printHelp (argc, argv);
00141     return (-1);
00142   }
00143 
00144   // Parse the command line arguments for .pcd files
00145   std::vector<int> pcd_file_indices;
00146   pcd_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
00147   if (pcd_file_indices.size () != 1)
00148   {
00149     print_error ("Need one input PCD file and one output VTK file to continue.\n");
00150     return (-1);
00151   }
00152 
00153   std::vector<int> vtk_file_indices = parse_file_extension_argument (argc, argv, ".vtk");
00154   if (vtk_file_indices.size () != 1)
00155   {
00156     print_error ("Need one output VTK file to continue.\n");
00157     return (-1);
00158   }
00159 
00160 
00161   // Command line parsing
00162   int hoppe_or_rbf = default_hoppe_or_rbf;
00163   bool ok = false;
00164   parse_argument (argc, argv, "-hoppe", ok);
00165   if (ok)
00166   {
00167     hoppe_or_rbf = 0;
00168     print_info ("Selected algorithm: MarchingCubesHoppe\n");
00169   }
00170   ok = false;
00171   parse_argument (argc, argv, "-rbf", ok);
00172   if (ok)
00173   {
00174     hoppe_or_rbf = 1;
00175     print_info ("Selected algorithm: MarchingCubesRBF\n");
00176   }
00177 
00178   float iso_level = default_iso_level;
00179   parse_argument (argc, argv, "-iso_level", iso_level);
00180   print_info ("Setting an iso level of: "); print_value ("%f\n", iso_level);
00181 
00182   int grid_res = default_grid_res;
00183   parse_argument (argc, argv, "-grid_res", grid_res);
00184   print_info ("Setting a cubic grid resolution of: "); print_value ("%d\n", grid_res);
00185 
00186   float extend_percentage = default_extend_percentage;
00187   parse_argument (argc, argv, "-extend", extend_percentage);
00188   print_info ("Setting an extend percentage of: "); print_value ("%f\n", extend_percentage);
00189 
00190   float off_surface_displacement = default_off_surface_displacement;
00191   parse_argument (argc, argv, "-displacement", off_surface_displacement);
00192   print_info ("Setting an off-surface displacement of: "); print_value ("%f\n", off_surface_displacement);
00193 
00194   // Load the first file
00195   sensor_msgs::PointCloud2::Ptr cloud (new sensor_msgs::PointCloud2);
00196   if (!loadCloud (argv[pcd_file_indices[0]], *cloud))
00197     return (-1);
00198 
00199   // Apply the marching cubes algorithm
00200   PolygonMesh output;
00201   compute (cloud, output, hoppe_or_rbf, iso_level, grid_res, extend_percentage, off_surface_displacement);
00202 
00203   // Save into the second file
00204   saveCloud (argv[vtk_file_indices[0]], output);
00205 }
00206 


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:15:39