00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include <pcl/PCLPointCloud2.h>
00039 #include <pcl/io/pcd_io.h>
00040 #include <pcl/filters/fast_bilateral.h>
00041 #include <pcl/console/print.h>
00042 #include <pcl/console/parse.h>
00043 #include <pcl/console/time.h>
00044
00045 using namespace std;
00046 using namespace pcl;
00047 using namespace pcl::io;
00048 using namespace pcl::console;
00049
00050 float default_sigma_s = 5.0f;
00051 float default_sigma_r = 0.03f;
00052
00053 void
00054 printHelp (int, char **argv)
00055 {
00056 print_error ("Syntax is: %s input.pcd output.pcd <options> [optional_arguments]\n", argv[0]);
00057 print_info (" where options are:\n");
00058 print_info (" -sigma_s X = use a sigma S value of X (default: ");
00059 print_value ("%f", default_sigma_s); print_info (")\n");
00060 print_info (" -sigma_r X = use a sigma R value of X (default: ");
00061 print_value ("%f", default_sigma_r); print_info (")\n");
00062 print_info ("\nOptional arguments are:\n");
00063 print_info (" -input_dir X = batch process all PCD files found in input_dir\n");
00064 print_info (" -output_dir X = save the processed files from input_dir in this directory\n");
00065 }
00066
00067 bool
00068 loadCloud (const string &filename, pcl::PCLPointCloud2 &cloud,
00069 Eigen::Vector4f &translation, Eigen::Quaternionf &orientation)
00070 {
00071 if (loadPCDFile (filename, cloud, translation, orientation) < 0)
00072 return (false);
00073
00074 return (true);
00075 }
00076
00077 void
00078 compute (const pcl::PCLPointCloud2::ConstPtr &input, pcl::PCLPointCloud2 &output,
00079 float sigma_s = 5.f, float sigma_r = 0.03f)
00080 {
00081
00082 PointCloud<PointXYZ>::Ptr xyz (new PointCloud<PointXYZ>);
00083 fromPCLPointCloud2 (*input, *xyz);
00084
00085 TicToc tt;
00086 tt.tic ();
00087
00088
00089 FastBilateralFilter<PointXYZ> fbf;
00090 fbf.setInputCloud (xyz);
00091 fbf.setSigmaS (sigma_s);
00092 fbf.setSigmaR (sigma_r);
00093 PointCloud<PointXYZ> xyz_filtered;
00094 fbf.filter (xyz_filtered);
00095
00096 print_highlight ("Filtered data in "); print_value ("%g", tt.toc ()); print_info (" ms for "); print_value ("%zu", xyz_filtered.size ()); print_info (" points.\n");
00097
00098
00099 pcl::PCLPointCloud2 output_xyz;
00100 toPCLPointCloud2 (xyz_filtered, output_xyz);
00101 concatenateFields (*input, output_xyz, output);
00102 }
00103
00104 void
00105 saveCloud (const string &filename, const pcl::PCLPointCloud2 &output,
00106 const Eigen::Vector4f &translation, const Eigen::Quaternionf &orientation)
00107 {
00108 PCDWriter w;
00109 w.writeBinaryCompressed (filename, output, translation, orientation);
00110 }
00111
00112 int
00113 batchProcess (const vector<string> &pcd_files, string &output_dir, float sigma_s, float sigma_r)
00114 {
00115 #if _OPENMP
00116 #pragma omp parallel for
00117 #endif
00118 for (int i = 0; i < int (pcd_files.size ()); ++i)
00119 {
00120
00121 Eigen::Vector4f translation;
00122 Eigen::Quaternionf rotation;
00123 pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2);
00124 if (!loadCloud (pcd_files[i], *cloud, translation, rotation))
00125 continue;
00126
00127
00128 pcl::PCLPointCloud2 output;
00129 compute (cloud, output, sigma_s, sigma_r);
00130
00131
00132 string filename = pcd_files[i];
00133 boost::trim (filename);
00134 vector<string> st;
00135 boost::split (st, filename, boost::is_any_of ("/\\"), boost::token_compress_on);
00136
00137
00138 stringstream ss;
00139 ss << output_dir << "/" << st.at (st.size () - 1);
00140 saveCloud (ss.str (), output, translation, rotation);
00141 }
00142 return (0);
00143 }
00144
00145
00146 int
00147 main (int argc, char** argv)
00148 {
00149 print_info ("Smooth depth data using a FastBilateralFilter. For more information, use: %s -h\n", argv[0]);
00150
00151 if (argc < 3)
00152 {
00153 printHelp (argc, argv);
00154 return (-1);
00155 }
00156
00157 bool batch_mode = false;
00158
00159
00160 float sigma_s = default_sigma_s;
00161 float sigma_r = default_sigma_r;
00162 parse_argument (argc, argv, "-sigma_s", sigma_s);
00163 parse_argument (argc, argv, "-sigma_r", sigma_r);
00164 string input_dir, output_dir;
00165 if (parse_argument (argc, argv, "-input_dir", input_dir) != -1)
00166 {
00167 PCL_INFO ("Input directory given as %s. Batch process mode on.\n", input_dir.c_str ());
00168 if (parse_argument (argc, argv, "-output_dir", output_dir) == -1)
00169 {
00170 PCL_ERROR ("Need an output directory! Please use -output_dir to continue.\n");
00171 return (-1);
00172 }
00173
00174
00175 batch_mode = true;
00176 }
00177
00178 if (!batch_mode)
00179 {
00180
00181 vector<int> p_file_indices;
00182 p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
00183 if (p_file_indices.size () != 2)
00184 {
00185 print_error ("Need one input PCD file and one output PCD file to continue.\n");
00186 return (-1);
00187 }
00188
00189 print_info ("Smoothing data with a sigma S/R of: ");
00190 print_value ("%f / %f\n", sigma_s, sigma_r);
00191
00192
00193 Eigen::Vector4f translation;
00194 Eigen::Quaternionf rotation;
00195 pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2);
00196 if (!loadCloud (argv[p_file_indices[0]], *cloud, translation, rotation))
00197 return (-1);
00198
00199
00200 pcl::PCLPointCloud2 output;
00201 compute (cloud, output, sigma_s, sigma_r);
00202
00203
00204 saveCloud (argv[p_file_indices[1]], output, translation, rotation);
00205 }
00206 else
00207 {
00208 if (input_dir != "" && boost::filesystem::exists (input_dir))
00209 {
00210 vector<string> pcd_files;
00211 boost::filesystem::directory_iterator end_itr;
00212 for (boost::filesystem::directory_iterator itr (input_dir); itr != end_itr; ++itr)
00213 {
00214
00215 if (!is_directory (itr->status ()) && boost::algorithm::to_upper_copy (boost::filesystem::extension (itr->path ())) == ".PCD" )
00216 {
00217 pcd_files.push_back (itr->path ().string ());
00218 PCL_INFO ("[Batch processing mode] Added %s for processing.\n", itr->path ().string ().c_str ());
00219 }
00220 }
00221 batchProcess (pcd_files, output_dir, sigma_s, sigma_r);
00222 }
00223 else
00224 {
00225 PCL_ERROR ("Batch processing mode enabled, but invalid input directory (%s) given!\n", input_dir.c_str ());
00226 return (-1);
00227 }
00228 }
00229 }
00230