bilateral_upsampling.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  * Point Cloud Library (PCL) - www.pointclouds.org
00005  * Copyright (c) 2009-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 
00041 #include <sensor_msgs/PointCloud2.h>
00042 #include <pcl/point_types.h>
00043 #include <pcl/io/pcd_io.h>
00044 #include <pcl/console/print.h>
00045 #include <pcl/console/parse.h>
00046 #include <pcl/console/time.h>
00047 #include <pcl/surface/bilateral_upsampling.h>
00048 
00049 using namespace pcl;
00050 using namespace pcl::io;
00051 using namespace pcl::console;
00052 
00053 
00054 int default_window_size = 15;
00055 double default_sigma_color = 15;
00056 double default_sigma_depth = 1.5;
00057 
00058 
00059 void
00060 printHelp (int, char **argv)
00061 {
00062   print_error ("Syntax is: %s input.pcd output.pcd <options>\n", argv[0]);
00063   print_info ("  where options are:\n");
00064   print_info ("                     -window_size X =  (default: ");
00065   print_value ("%d", default_window_size); print_info (")\n");
00066   print_info ("                     -sigma_color X =  (default: ");
00067   print_value ("%f", default_sigma_color); print_info (")\n");
00068   print_info ("                     -sigma_depth X =  (default: ");
00069   print_value ("%d", default_sigma_depth); print_info (")\n");
00070 }
00071 
00072 bool
00073 loadCloud (const std::string &filename, sensor_msgs::PointCloud2 &cloud)
00074 {
00075   TicToc tt;
00076   print_highlight ("Loading "); print_value ("%s ", filename.c_str ());
00077 
00078   tt.tic ();
00079   if (loadPCDFile (filename, cloud) < 0)
00080     return (false);
00081   print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]\n");
00082   print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (cloud).c_str ());
00083 
00084   return (true);
00085 }
00086 
00087 void
00088 compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
00089          int window_size, double sigma_color, double sigma_depth)
00090 {
00091   PointCloud<PointXYZRGBA>::Ptr cloud (new pcl::PointCloud<PointXYZRGBA> ());
00092   fromROSMsg (*input, *cloud);
00093 
00094   PointCloud<PointXYZRGBA>::Ptr cloud_upsampled (new PointCloud<PointXYZRGBA> ());
00095 
00096   BilateralUpsampling<PointXYZRGBA, PointXYZRGBA> bu;
00097   bu.setInputCloud (cloud);
00098   bu.setWindowSize (window_size);
00099   bu.setSigmaColor (sigma_color);
00100   bu.setSigmaDepth (sigma_depth);
00101 
00102   // TODO need to fix this somehow
00103   bu.setProjectionMatrix (bu.KinectSXGAProjectionMatrix);
00104 
00105 
00106   TicToc tt;
00107   tt.tic ();
00108   bu.process (*cloud_upsampled);
00109   print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud_upsampled->width * cloud_upsampled->height); print_info (" points]\n");
00110 
00111   toROSMsg (*cloud_upsampled, output);
00112 }
00113 
00114 void
00115 saveCloud (const std::string &filename, const sensor_msgs::PointCloud2 &output)
00116 {
00117   TicToc tt;
00118   tt.tic ();
00119 
00120   print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
00121 
00122   pcl::io::savePCDFile (filename, output,  Eigen::Vector4f::Zero (),
00123                         Eigen::Quaternionf::Identity (), true);
00124 
00125   print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n");
00126 }
00127 
00128 /* ---[ */
00129 int
00130 main (int argc, char** argv)
00131 {
00132   print_info ("Bilateral Filter Upsampling of an organized point cloud containing color information. For more information, use: %s -h\n", argv[0]);
00133 
00134   if (argc < 3)
00135   {
00136     printHelp (argc, argv);
00137     return (-1);
00138   }
00139 
00140   // Parse the command line arguments for .pcd files
00141   std::vector<int> p_file_indices;
00142   p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
00143   if (p_file_indices.size () != 2)
00144   {
00145     print_error ("Need one input PCD file and one output PCD file to continue.\n");
00146     return (-1);
00147   }
00148 
00149   // Command line parsing
00150   int window_size = default_window_size;
00151   double sigma_color = default_sigma_color;
00152   double sigma_depth = default_sigma_depth;
00153 
00154   parse_argument (argc, argv, "-window_size", window_size);
00155   parse_argument (argc, argv, "-sigma_color", sigma_color);
00156   parse_argument (argc, argv, "-sigma_depth", sigma_depth);
00157 
00158   // Load the first file
00159   sensor_msgs::PointCloud2::Ptr cloud (new sensor_msgs::PointCloud2);
00160   if (!loadCloud (argv[p_file_indices[0]], *cloud))
00161     return (-1);
00162 
00163   // Do the smoothing
00164   sensor_msgs::PointCloud2 output;
00165   compute (cloud, output, window_size, sigma_color, sigma_depth);
00166 
00167   // Save into the second file
00168   saveCloud (argv[p_file_indices[1]], output);
00169 }


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:14:40