mls_smoothing.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, 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: mls_smoothing.cpp 5124 2012-03-16 03:09:41Z rusu $
00035  *
00036  */
00037 
00038 #include <sensor_msgs/PointCloud2.h>
00039 #include <pcl/point_types.h>
00040 #include <pcl/io/pcd_io.h>
00041 #include <pcl/console/print.h>
00042 #include <pcl/console/parse.h>
00043 #include <pcl/console/time.h>
00044 #include <pcl/surface/mls.h>
00045 #include <pcl/surface/mls_omp.h>
00046 #include <pcl/filters/voxel_grid.h>
00047 
00048 using namespace pcl;
00049 using namespace pcl::io;
00050 using namespace pcl::console;
00051 
00052 int default_polynomial_order = 2;
00053 bool default_use_polynomial_fit = false;
00054 double default_search_radius = 0.0,
00055     default_sqr_gauss_param = 0.0;
00056 
00057 
00058 void
00059 printHelp (int, char **argv)
00060 {
00061   print_error ("Syntax is: %s input.pcd output.pcd <options>\n", argv[0]);
00062   print_info ("  where options are:\n");
00063   print_info ("                     -search_radius X = sphere radius to be used for finding the k-nearest neighbors used for fitting (default: ");
00064   print_value ("%f", default_search_radius); print_info (")\n");
00065   print_info ("                     -sqr_gauss_param X = parameter used for the distance based weighting of neighbors (recommended = search_radius^2) (default: ");
00066   print_value ("%f", default_sqr_gauss_param); print_info (")\n");
00067   print_info ("                     -use_polynomial_fit X = decides whether the surface and normal are approximated using a polynomial or only via tangent estimation (default: ");
00068   print_value ("%d", default_use_polynomial_fit); print_info (")\n");
00069   print_info ("                     -polynomial_order X = order of the polynomial to be fit (implicitly, use_polynomial_fit = 1) (default: ");
00070   print_value ("%d", default_polynomial_order); print_info (")\n");
00071 }
00072 
00073 bool
00074 loadCloud (const std::string &filename, sensor_msgs::PointCloud2 &cloud)
00075 {
00076   TicToc tt;
00077   print_highlight ("Loading "); print_value ("%s ", filename.c_str ());
00078 
00079   tt.tic ();
00080   if (loadPCDFile (filename, cloud) < 0)
00081     return (false);
00082   print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]\n");
00083   print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (cloud).c_str ());
00084 
00085   return (true);
00086 }
00087 
00088 void
00089 compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
00090          double search_radius, bool sqr_gauss_param_set, double sqr_gauss_param,
00091          bool use_polynomial_fit, int polynomial_order)
00092 {
00093 
00094   PointCloud<PointXYZ>::Ptr xyz_cloud_pre (new pcl::PointCloud<PointXYZ> ()),
00095       xyz_cloud (new pcl::PointCloud<PointXYZ> ());
00096   fromROSMsg (*input, *xyz_cloud_pre);
00097 
00098   // Filter the NaNs from the cloud
00099   for (size_t i = 0; i < xyz_cloud_pre->size (); ++i)
00100     if (pcl_isfinite (xyz_cloud_pre->points[i].x))
00101       xyz_cloud->push_back (xyz_cloud_pre->points[i]);
00102   xyz_cloud->header = xyz_cloud_pre->header;
00103   xyz_cloud->height = 1;
00104   xyz_cloud->width = static_cast<uint32_t> (xyz_cloud->size ());
00105   xyz_cloud->is_dense = false;
00106   
00107   
00108 
00109   PointCloud<PointNormal>::Ptr xyz_cloud_smoothed (new PointCloud<PointNormal> ());
00110 
00111   MovingLeastSquaresOMP<PointXYZ, PointNormal> mls;
00112   mls.setInputCloud (xyz_cloud);
00113   mls.setSearchRadius (search_radius);
00114   if (sqr_gauss_param_set) mls.setSqrGaussParam (sqr_gauss_param);
00115   mls.setPolynomialFit (use_polynomial_fit);
00116   mls.setPolynomialOrder (polynomial_order);
00117 
00118 //  mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::SAMPLE_LOCAL_PLANE);
00119 //  mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::RANDOM_UNIFORM_DENSITY);
00120 //  mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::VOXEL_GRID_DILATION);
00121   mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::NONE);
00122   mls.setPointDensity (60000 * int (search_radius)); // 300 points in a 5 cm radius
00123   mls.setUpsamplingRadius (0.025);
00124   mls.setUpsamplingStepSize (0.015);
00125   mls.setDilationIterations (2);
00126   mls.setDilationVoxelSize (0.01f);
00127 
00128   search::KdTree<PointXYZ>::Ptr tree (new search::KdTree<PointXYZ> ());
00129   mls.setSearchMethod (tree);
00130   mls.setComputeNormals (true);
00131 
00132   PCL_INFO ("Computing smoothed surface and normals with search_radius %f , sqr_gaussian_param %f, polynomial fitting %d, polynomial order %d\n",
00133             mls.getSearchRadius(), mls.getSqrGaussParam(), mls.getPolynomialFit(), mls.getPolynomialOrder());
00134   TicToc tt;
00135   tt.tic ();
00136   mls.process (*xyz_cloud_smoothed);
00137   print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", xyz_cloud_smoothed->width * xyz_cloud_smoothed->height); print_info (" points]\n");
00138 
00139   toROSMsg (*xyz_cloud_smoothed, output);
00140 }
00141 
00142 void
00143 saveCloud (const std::string &filename, const sensor_msgs::PointCloud2 &output)
00144 {
00145   TicToc tt;
00146   tt.tic ();
00147 
00148   print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
00149 
00150   pcl::io::savePCDFile (filename, output,  Eigen::Vector4f::Zero (),
00151                         Eigen::Quaternionf::Identity (), true);
00152 
00153   print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n");
00154 }
00155 
00156 /* ---[ */
00157 int
00158 main (int argc, char** argv)
00159 {
00160   print_info ("Moving Least Squares smoothing of a point cloud. For more information, use: %s -h\n", argv[0]);
00161 
00162   if (argc < 3)
00163   {
00164     printHelp (argc, argv);
00165     return (-1);
00166   }
00167 
00168   // Parse the command line arguments for .pcd files
00169   std::vector<int> p_file_indices;
00170   p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
00171   if (p_file_indices.size () != 2)
00172   {
00173     print_error ("Need one input PCD file and one output PCD file to continue.\n");
00174     return (-1);
00175   }
00176 
00177   // Command line parsing
00178   double search_radius = default_search_radius;
00179   double sqr_gauss_param = default_sqr_gauss_param;
00180   bool sqr_gauss_param_set = true;
00181   int polynomial_order = default_polynomial_order;
00182   bool use_polynomial_fit = default_use_polynomial_fit;
00183 
00184   parse_argument (argc, argv, "-search_radius", search_radius);
00185   if (parse_argument (argc, argv, "-sqr_gauss_param", sqr_gauss_param) == -1)
00186     sqr_gauss_param_set = false;
00187   if (parse_argument (argc, argv, "-polynomial_order", polynomial_order) != -1 )
00188     use_polynomial_fit = true;
00189   parse_argument (argc, argv, "-use_polynomial_fit", use_polynomial_fit);
00190 
00191   // Load the first file
00192   sensor_msgs::PointCloud2::Ptr cloud (new sensor_msgs::PointCloud2);
00193   if (!loadCloud (argv[p_file_indices[0]], *cloud))
00194     return (-1);
00195 
00196   // Do the smoothing
00197   sensor_msgs::PointCloud2 output;
00198   compute (cloud, output, search_radius, sqr_gauss_param_set, sqr_gauss_param,
00199            use_polynomial_fit, polynomial_order);
00200 
00201   // Save into the second file
00202   saveCloud (argv[p_file_indices[1]], output);
00203 }


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