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