1 /*
2  * Software License Agreement (BSD License)
3  *
4  *  Point Cloud Library (PCL) - www.pointclouds.org
5  *  Copyright (c) 2009-2012, Willow Garage, Inc.
6  *  Copyright (c) 2012-, Open Perception, Inc.
7  *  Copyright (c) 2014, RadiantBlue Technologies, Inc.
8  *
9  *  All rights reserved.
10  *
11  *  Redistribution and use in source and binary forms, with or without
12  *  modification, are permitted provided that the following conditions
13  *  are met:
14  *
15  *   * Redistributions of source code must retain the above copyright
16  *     notice, this list of conditions and the following disclaimer.
17  *   * Redistributions in binary form must reproduce the above
18  *     copyright notice, this list of conditions and the following
19  *     disclaimer in the documentation and/or other materials provided
20  *     with the distribution.
21  *   * Neither the name of the copyright holder(s) nor the names of its
22  *     contributors may be used to endorse or promote products derived
23  *     from this software without specific prior written permission.
24  *
25  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
28  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
29  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
30  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
31  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
32  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
33  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
34  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
35  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36  *  POSSIBILITY OF SUCH DAMAGE.
37  *
38  * $Id$
39  */
40 
41 #include <pcl/point_types.h>
42 #include <pcl/io/pcd_io.h>
43 #include <pcl/console/print.h>
44 #include <pcl/console/parse.h>
45 #include <pcl/console/time.h>
46 #include <pcl/filters/extract_indices.h>
47 #include <pcl/segmentation/approximate_progressive_morphological_filter.h>
48 #include <pcl/segmentation/progressive_morphological_filter.h>
49 #include <boost/filesystem.hpp> // for path, exists, ...
50 #include <boost/algorithm/string/case_conv.hpp> // for to_upper_copy
51 
52 using namespace pcl;
53 using namespace pcl::io;
54 using namespace pcl::console;
55 
56 using PointType = PointXYZ;
57 using Cloud = PointCloud<PointXYZ>;
58 using ConstCloudPtr = const Cloud::ConstPtr;
59 
60 int default_max_window_size = 33;
61 float default_slope = 0.7f;
62 float default_max_distance = 10.0f;
63 float default_initial_distance = 0.15f;
64 float default_cell_size = 1.0f;
65 float default_base = 2.0f;
66 bool default_exponential = true;
67 int default_verbosity_level = 3;
68 
69 void
printHelp(int,char ** argv)70 printHelp (int, char **argv)
71 {
72   print_error ("Syntax is: %s input.pcd output.pcd <options>\n", argv[0]);
73   print_info ("  where options are:\n");
74   print_info ("                     -max_window_size X = maximum window size (default: ");
75   print_value ("%d", default_max_window_size); print_info (")\n");
76   print_info ("                     -slope X = slope value to compute threshold (default: ");
77   print_value ("%f", default_slope); print_info (")\n");
78   print_info ("                     -max_distnace X = maximum distance from parameterized ground surface to be considered ground (default: ");
79   print_value ("%f", default_max_distance); print_info (")\n");
80   print_info ("                     -initial_distance X = initial distance from parameterized ground surface to be considered ground (default: ");
81   print_value ("%f", default_initial_distance); print_info (")\n");
82   print_info ("                     -cell_size X = cell size (default: ");
83   print_value ("%f", default_cell_size); print_info (")\n");
84   print_info ("                     -base X = base to be used in computing progressive window sizes (default: ");
85   print_value ("%f", default_base); print_info (")\n");
86   print_info ("                     -exponential X = use exponential growth? (default: ");
87   print_value ("%s", default_exponential?"true":"false"); print_info (")\n");
88   print_info ("                     -approximate X = use approximate? (default: false\n");
89   print_info ("                     -input_dir X  = batch process all PCD files found in input_dir\n");
90   print_info ("                     -output_dir X = save the processed files from input_dir in this directory\n");
91   print_info ("                     -verbosity X = verbosity level (default: ");
92   print_value ("%d", default_verbosity_level); print_info (")\n");
93 }
94 
95 bool
loadCloud(const std::string & filename,Cloud & cloud)96 loadCloud (const std::string &filename, Cloud &cloud)
97 {
98   TicToc tt;
99   print_highlight ("Loading "); print_value ("%s ", filename.c_str ());
100 
101   tt.tic ();
102   if (loadPCDFile (filename, cloud) < 0)
103     return (false);
104   print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]\n");
105   print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (cloud).c_str ());
106 
107   return (true);
108 }
109 
110 void
compute(ConstCloudPtr & input,Cloud & output,int max_window_size,float slope,float max_distance,float initial_distance,float cell_size,float base,bool exponential,bool approximate)111 compute (ConstCloudPtr &input, Cloud &output, int max_window_size, float slope, float max_distance, float initial_distance, float cell_size, float base, bool exponential, bool approximate)
112 {
113   // Estimate
114   TicToc tt;
115   tt.tic ();
116 
117   print_highlight (stderr, "Computing ");
118 
119   pcl::Indices ground;
120 
121   if (approximate)
122   {
123     PCL_DEBUG("approx with %zu points\n", static_cast<std::size_t>(input->size()));
124     ApproximateProgressiveMorphologicalFilter<PointType> pmf;
125     pmf.setInputCloud (input);
126     pmf.setMaxWindowSize (max_window_size);
127     pmf.setSlope (slope);
128     pmf.setMaxDistance (max_distance);
129     pmf.setInitialDistance (initial_distance);
130     pmf.setCellSize (cell_size);
131     pmf.setBase (base);
132     pmf.setExponential (exponential);
133     pmf.extract (ground);
134   }
135   else
136   {
137     PCL_DEBUG ("full\n");
138     ProgressiveMorphologicalFilter<PointType> pmf;
139     pmf.setInputCloud (input);
140     pmf.setMaxWindowSize (max_window_size);
141     pmf.setSlope (slope);
142     pmf.setMaxDistance (max_distance);
143     pmf.setInitialDistance (initial_distance);
144     pmf.setCellSize (cell_size);
145     pmf.setBase (base);
146     pmf.setExponential (exponential);
147     pmf.extract (ground);
148   }
149 
150   PointIndicesPtr idx (new PointIndices);
151   idx->indices = ground;
152 
153   ExtractIndices<PointType> extract;
154   extract.setInputCloud (input);
155   extract.setIndices (idx);
156   extract.setNegative (false);
157   extract.filter (output);
158 
159   print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n");
160 }
161 
162 void
saveCloud(const std::string & filename,const Cloud & output)163 saveCloud (const std::string &filename, const Cloud &output)
164 {
165   TicToc tt;
166   tt.tic ();
167 
168   print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
169 
170   PCDWriter w;
171   w.writeBinaryCompressed (filename, output);
172 
173   print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n");
174 }
175 
176 int
batchProcess(const std::vector<std::string> & pcd_files,std::string & output_dir,int max_window_size,float slope,float max_distance,float initial_distance,float cell_size,float base,bool exponential,bool approximate)177 batchProcess (const std::vector<std::string> &pcd_files, std::string &output_dir, int max_window_size, float slope, float max_distance, float initial_distance, float cell_size, float base, bool exponential, bool approximate)
178 {
179   std::vector<std::string> st;
180   for (const auto &pcd_file : pcd_files)
181   {
182     // Load the first file
183     Cloud::Ptr cloud (new Cloud);
184     if (!loadCloud (pcd_file, *cloud))
185       return (-1);
186 
187     // Perform the feature estimation
188     Cloud output;
189     compute (cloud, output, max_window_size, slope, max_distance, initial_distance, cell_size, base, exponential, approximate);
190 
191     // Prepare output file name
192     std::string filename = boost::filesystem::path(pcd_file).filename().string();
193 
194     // Save into the second file
195     const std::string filepath = output_dir + '/' + filename;
196     saveCloud (filepath, output);
197   }
198   return (0);
199 }
200 
201 
202 /* ---[ */
203 int
main(int argc,char ** argv)204 main (int argc, char** argv)
205 {
206   print_info ("Filter a point cloud using the pcl::ProgressiveMorphologicalFilter. For more information, use: %s -h\n", argv[0]);
207 
208   if (argc < 3)
209   {
210     printHelp (argc, argv);
211     return (-1);
212   }
213 
214   bool batch_mode = false;
215 
216   // Command line parsing
217   int max_window_size = default_max_window_size;
218   float slope = default_slope;
219   float max_distance = default_max_distance;
220   float initial_distance = default_initial_distance;
221   float cell_size = default_cell_size;
222   float base = default_base;
223   bool exponential = default_exponential;
224   bool approximate;
225   int verbosity_level = default_verbosity_level;
226   parse_argument (argc, argv, "-max_window_size", max_window_size);
227   parse_argument (argc, argv, "-slope", slope);
228   parse_argument (argc, argv, "-max_distance", max_distance);
229   parse_argument (argc, argv, "-initial_distance", initial_distance);
230   parse_argument (argc, argv, "-cell_size", cell_size);
231   parse_argument (argc, argv, "-base", base);
232   parse_argument (argc, argv, "-exponential", exponential);
233   approximate = find_switch (argc, argv, "-approximate");
234   parse_argument (argc, argv, "-verbosity", verbosity_level);
235   std::string input_dir, output_dir;
236   if (parse_argument (argc, argv, "-input_dir", input_dir) != -1)
237   {
238     PCL_INFO ("Input directory given as %s. Batch process mode on.\n", input_dir.c_str ());
239     if (parse_argument (argc, argv, "-output_dir", output_dir) == -1)
240     {
241       PCL_ERROR ("Need an output directory! Please use -output_dir to continue.\n");
242       return (-1);
243     }
244 
245     // Both input dir and output dir given, switch into batch processing mode
246     batch_mode = true;
247   }
248 
249   switch (verbosity_level)
250   {
251     case 0:
252       pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS);
253       break;
254 
255     case 1:
256       pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
257       break;
258 
259     case 2:
260       pcl::console::setVerbosityLevel(pcl::console::L_WARN);
261       break;
262 
263     case 3:
264       pcl::console::setVerbosityLevel(pcl::console::L_INFO);
265       break;
266 
267     case 4:
268       pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
269       break;
270 
271     default:
272       pcl::console::setVerbosityLevel(pcl::console::L_VERBOSE);
273       break;
274   }
275 
276   if (!batch_mode)
277   {
278     // Parse the command line arguments for .pcd files
279     std::vector<int> p_file_indices;
280     p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
281     if (p_file_indices.size () != 2)
282     {
283       print_error ("Need one input PCD file and one output PCD file to continue.\n");
284       return (-1);
285     }
286 
287     // Load the first file
288     Cloud::Ptr cloud (new Cloud);
289     if (!loadCloud (argv[p_file_indices[0]], *cloud))
290       return (-1);
291 
292     // Perform the feature estimation
293     Cloud output;
294     compute (cloud, output, max_window_size, slope, max_distance, initial_distance, cell_size, base, exponential, approximate);
295 
296     // Save into the second file
297     saveCloud (argv[p_file_indices[1]], output);
298   }
299   else
300   {
301     if (!input_dir.empty() && boost::filesystem::exists (input_dir))
302     {
303       std::vector<std::string> pcd_files;
304       boost::filesystem::directory_iterator end_itr;
305       for (boost::filesystem::directory_iterator itr (input_dir); itr != end_itr; ++itr)
306       {
307         // Only add PCD files
308         if (!is_directory (itr->status ()) && boost::algorithm::to_upper_copy (boost::filesystem::extension (itr->path ())) == ".PCD" )
309         {
310           pcd_files.push_back (itr->path ().string ());
311           PCL_INFO ("[Batch processing mode] Added %s for processing.\n", itr->path ().string ().c_str ());
312         }
313       }
314       batchProcess (pcd_files, output_dir, max_window_size, slope, max_distance, initial_distance, cell_size, base, exponential, approximate);
315     }
316     else
317     {
318       PCL_ERROR ("Batch processing mode enabled, but invalid input directory (%s) given!\n", input_dir.c_str ());
319       return (-1);
320     }
321   }
322 }
323 
324