1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2012-, Open Perception, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of the copyright holder(s) nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 */
37
38 #include <pcl/PCLPointCloud2.h>
39 #include <pcl/io/pcd_io.h>
40 #include <pcl/sample_consensus/ransac.h>
41 #include <pcl/sample_consensus/sac_model_plane.h>
42 #include <pcl/segmentation/extract_clusters.h>
43 #include <pcl/console/print.h>
44 #include <pcl/console/parse.h>
45 #include <pcl/console/time.h>
46 #include <boost/filesystem.hpp> // for path, exists, ...
47 #include <boost/algorithm/string/case_conv.hpp> // for to_upper_copy
48
49 using namespace pcl;
50 using namespace pcl::io;
51 using namespace pcl::console;
52
53 int default_max_iterations = 1000;
54 double default_threshold = 0.05;
55 bool default_negative = false;
56
57 Eigen::Vector4f translation;
58 Eigen::Quaternionf orientation;
59
60 void
printHelp(int,char ** argv)61 printHelp (int, char **argv)
62 {
63 print_error ("Syntax is: %s input.pcd output.pcd <options> [optional_arguments]\n", argv[0]);
64 print_info (" where options are:\n");
65 print_info (" -thresh X = set the inlier threshold from the plane to (default: ");
66 print_value ("%g", default_threshold); print_info (")\n");
67 print_info (" -max_it X = set the maximum number of RANSAC iterations to X (default: ");
68 print_value ("%d", default_max_iterations); print_info (")\n");
69 print_info (" -neg 0/1 = if true (1), instead of the plane, it returns the largest cluster on top of the plane (default: ");
70 print_value ("%s", default_negative ? "true" : "false"); print_info (")\n");
71 print_info ("\nOptional arguments are:\n");
72 print_info (" -input_dir X = batch process all PCD files found in input_dir\n");
73 print_info (" -output_dir X = save the processed files from input_dir in this directory\n");
74 }
75
76 bool
loadCloud(const std::string & filename,pcl::PCLPointCloud2 & cloud)77 loadCloud (const std::string &filename, pcl::PCLPointCloud2 &cloud)
78 {
79 TicToc tt;
80 print_highlight ("Loading "); print_value ("%s ", filename.c_str ());
81
82 tt.tic ();
83 if (loadPCDFile (filename, cloud, translation, orientation) < 0)
84 return (false);
85 print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]\n");
86 print_info ("Available dimensions: "); print_value ("%s\n", getFieldsList (cloud).c_str ());
87
88 return (true);
89 }
90
91 void
compute(const pcl::PCLPointCloud2::ConstPtr & input,pcl::PCLPointCloud2 & output,int max_iterations=1000,double threshold=0.05,bool negative=false)92 compute (const pcl::PCLPointCloud2::ConstPtr &input, pcl::PCLPointCloud2 &output,
93 int max_iterations = 1000, double threshold = 0.05, bool negative = false)
94 {
95 // Convert data to PointCloud<T>
96 PointCloud<PointXYZ>::Ptr xyz (new PointCloud<PointXYZ>);
97 fromPCLPointCloud2 (*input, *xyz);
98
99 // Estimate
100 TicToc tt;
101 print_highlight (stderr, "Computing ");
102
103 tt.tic ();
104
105 // Refine the plane indices
106 using SampleConsensusModelPlanePtr = SampleConsensusModelPlane<PointXYZ>::Ptr;
107 SampleConsensusModelPlanePtr model (new SampleConsensusModelPlane<PointXYZ> (xyz));
108 RandomSampleConsensus<PointXYZ> sac (model, threshold);
109 sac.setMaxIterations (max_iterations);
110 bool res = sac.computeModel ();
111
112 pcl::Indices inliers;
113 sac.getInliers (inliers);
114 Eigen::VectorXf coefficients;
115 sac.getModelCoefficients (coefficients);
116
117 if (!res || inliers.empty ())
118 {
119 PCL_ERROR ("No planar model found. Relax thresholds and continue.\n");
120 return;
121 }
122 sac.refineModel (2, 50);
123 sac.getInliers (inliers);
124 sac.getModelCoefficients (coefficients);
125
126 print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms, plane has : "); print_value ("%lu", inliers.size ()); print_info (" points]\n");
127
128 print_info ("Model coefficients: [");
129 print_value ("%g %g %g %g", coefficients[0], coefficients[1], coefficients[2], coefficients[3]); print_info ("]\n");
130
131 // Instead of returning the planar model as a set of inliers, return the outliers, but perform a cluster segmentation first
132 if (negative)
133 {
134 // Remove the plane indices from the data
135 PointIndices::Ptr everything_but_the_plane (new PointIndices);
136 std::vector<int> indices_fullset (xyz->size ());
137 for (int p_it = 0; p_it < static_cast<int> (indices_fullset.size ()); ++p_it)
138 indices_fullset[p_it] = p_it;
139
140 std::sort (inliers.begin (), inliers.end ());
141 set_difference (indices_fullset.begin (), indices_fullset.end (),
142 inliers.begin (), inliers.end (),
143 inserter (everything_but_the_plane->indices, everything_but_the_plane->indices.begin ()));
144
145 // Extract largest cluster minus the plane
146 std::vector<PointIndices> cluster_indices;
147 EuclideanClusterExtraction<PointXYZ> ec;
148 ec.setClusterTolerance (0.02); // 2cm
149 ec.setMinClusterSize (100);
150 ec.setInputCloud (xyz);
151 ec.setIndices (everything_but_the_plane);
152 ec.extract (cluster_indices);
153
154 // Convert data back
155 copyPointCloud (*input, cluster_indices[0].indices, output);
156 }
157 else
158 {
159 // Convert data back
160 PointCloud<PointXYZ> output_inliers;
161 copyPointCloud (*input, inliers, output);
162 }
163 }
164
165 void
saveCloud(const std::string & filename,const pcl::PCLPointCloud2 & output)166 saveCloud (const std::string &filename, const pcl::PCLPointCloud2 &output)
167 {
168 TicToc tt;
169 tt.tic ();
170
171 print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
172
173 PCDWriter w;
174 w.writeBinaryCompressed (filename, output, translation, orientation);
175
176 print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n");
177 }
178
179 int
batchProcess(const std::vector<std::string> & pcd_files,std::string & output_dir,int max_it,double thresh,bool negative)180 batchProcess (const std::vector<std::string> &pcd_files, std::string &output_dir, int max_it, double thresh, bool negative)
181 {
182 std::vector<std::string> st;
183 for (const auto &pcd_file : pcd_files)
184 {
185 // Load the first file
186 pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2);
187 if (!loadCloud (pcd_file, *cloud))
188 return (-1);
189
190 // Perform the feature estimation
191 pcl::PCLPointCloud2 output;
192 compute (cloud, output, max_it, thresh, negative);
193
194 // Prepare output file name
195 std::string filename = boost::filesystem::path(pcd_file).filename().string();
196
197 // Save into the second file
198 const std::string filepath = output_dir + '/' + filename;
199 saveCloud (filepath, output);
200 }
201 return (0);
202 }
203
204 /* ---[ */
205 int
main(int argc,char ** argv)206 main (int argc, char** argv)
207 {
208 print_info ("Estimate the largest planar component using SACSegmentation. For more information, use: %s -h\n", argv[0]);
209
210 if (argc < 3)
211 {
212 printHelp (argc, argv);
213 return (-1);
214 }
215
216 bool debug = false;
217 console::parse_argument (argc, argv, "-debug", debug);
218 if (debug)
219 {
220 print_highlight ("Enabling debug mode.\n");
221 console::setVerbosityLevel (console::L_DEBUG);
222 if (!isVerbosityLevelEnabled (L_DEBUG))
223 PCL_ERROR ("Error enabling debug mode.\n");
224 }
225
226 bool batch_mode = false;
227
228 // Command line parsing
229 int max_it = default_max_iterations;
230 double thresh = default_threshold;
231 bool negative = default_negative;
232 parse_argument (argc, argv, "-max_it", max_it);
233 parse_argument (argc, argv, "-thresh", thresh);
234 parse_argument (argc, argv, "-neg", negative);
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 if (!batch_mode)
250 {
251 // Parse the command line arguments for .pcd files
252 std::vector<int> p_file_indices;
253 p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
254 if (p_file_indices.size () != 2)
255 {
256 print_error ("Need one input PCD file and one output PCD file to continue.\n");
257 return (-1);
258 }
259
260 print_info ("Estimating planes with a threshold of: ");
261 print_value ("%g\n", thresh);
262
263 print_info ("Planar model segmentation: ");
264 print_value ("%s\n", negative ? "false" : "true");
265
266 // Load the first file
267 pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2);
268 if (!loadCloud (argv[p_file_indices[0]], *cloud))
269 return (-1);
270
271 // Perform the feature estimation
272 pcl::PCLPointCloud2 output;
273 compute (cloud, output, max_it, thresh, negative);
274
275 // Save into the second file
276 saveCloud (argv[p_file_indices[1]], output);
277 }
278 else
279 {
280 if (!input_dir.empty() && boost::filesystem::exists (input_dir))
281 {
282 std::vector<std::string> pcd_files;
283 boost::filesystem::directory_iterator end_itr;
284 for (boost::filesystem::directory_iterator itr (input_dir); itr != end_itr; ++itr)
285 {
286 // Only add PCD files
287 if (!is_directory (itr->status ()) && boost::algorithm::to_upper_copy (boost::filesystem::extension (itr->path ())) == ".PCD" )
288 {
289 pcd_files.push_back (itr->path ().string ());
290 PCL_INFO ("[Batch processing mode] Added %s for processing.\n", itr->path ().string ().c_str ());
291 }
292 }
293 batchProcess (pcd_files, output_dir, max_it, thresh, negative);
294 }
295 else
296 {
297 PCL_ERROR ("Batch processing mode enabled, but invalid input directory (%s) given!\n", input_dir.c_str ());
298 return (-1);
299 }
300 }
301 }
302
303