1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2013, Willow Garage, 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 #include <pcl/point_types.h>
38 #include <pcl/io/pcd_io.h>
39 #include <pcl/console/print.h>
40 #include <pcl/console/parse.h>
41 #include <pcl/console/time.h>
42 #include <pcl/filters/conditional_removal.h>
43 #include <boost/filesystem.hpp> // for path, exists, ...
44 #include <boost/algorithm/string/case_conv.hpp> // for to_upper_copy
45
46
47 using PointType = pcl::PointXYZ;
48 using Cloud = pcl::PointCloud<PointType>;
49 using CloudConstPtr = Cloud::ConstPtr;
50
51 float default_radius = 1.0f;
52 bool default_inside = true;
53 bool default_keep_organized = true;
54
55 void
printHelp(int,char ** argv)56 printHelp (int, char **argv)
57 {
58 pcl::console::print_error ("Syntax is: %s input.pcd output.pcd <options>\n", argv[0]);
59 pcl::console::print_info (" where options are:\n");
60 pcl::console::print_info (" -radius X = Radius of the spere to filter (default: ");
61 pcl::console::print_value ("%s", default_radius); pcl::console::print_info (")\n");
62 pcl::console::print_info (" -inside X = keep the points inside the [min, max] interval or not (default: ");
63 pcl::console::print_value ("%d", default_inside); pcl::console::print_info (")\n");
64 pcl::console::print_info (" -keep 0/1 = keep the points organized (1) or not (default: ");
65 pcl::console::print_value ("%d", default_keep_organized); pcl::console::print_info (")\n");
66 }
67
68 bool
loadCloud(const std::string & filename,Cloud::Ptr cloud)69 loadCloud (const std::string &filename, Cloud::Ptr cloud)
70 {
71 pcl::console::TicToc tt;
72 pcl::console::print_highlight ("Loading "); pcl::console::print_value ("%s ", filename.c_str ());
73
74 tt.tic ();
75 if (pcl::io::loadPCDFile (filename, *cloud) < 0)
76 return (false);
77 pcl::console::print_info ("[done, "); pcl::console::print_value ("%g", tt.toc ()); pcl::console::print_info (" ms : "); pcl::console::print_value ("%d", cloud->size ()); pcl::console::print_info (" points]\n");
78
79 return (true);
80 }
81
82 void
compute(const Cloud::Ptr & input,Cloud::Ptr & output,float radius,bool inside,bool keep_organized)83 compute (const Cloud::Ptr &input, Cloud::Ptr &output,
84 float radius, bool inside, bool keep_organized)
85 {
86 // Estimate
87 pcl::console::TicToc tt;
88 tt.tic ();
89
90 pcl::console::print_highlight (stderr, "Computing ");
91
92 pcl::ConditionOr<PointType>::Ptr cond (new pcl::ConditionOr<PointType> ());
93 cond->addComparison (pcl::TfQuadraticXYZComparison<PointType>::ConstPtr (new pcl::TfQuadraticXYZComparison<PointType> (inside ? pcl::ComparisonOps::LT : pcl::ComparisonOps::GT, Eigen::Matrix3f::Identity (),
94 Eigen::Vector3f::Zero (), - radius * radius)));
95
96 pcl::ConditionalRemoval<PointType> condrem;
97 condrem.setCondition (cond);
98 condrem.setInputCloud (input);
99 condrem.setKeepOrganized (keep_organized);
100 condrem.filter (*output);
101
102 pcl::console::print_info ("[done, "); pcl::console::print_value ("%g", tt.toc ()); pcl::console::print_info (" ms : "); pcl::console::print_value ("%d", output->size ()); pcl::console::print_info (" points]\n");
103 }
104
105 void
saveCloud(const std::string & filename,const Cloud::Ptr & output)106 saveCloud (const std::string &filename, const Cloud::Ptr &output)
107 {
108 pcl::console::TicToc tt;
109 tt.tic ();
110
111 pcl::console::print_highlight ("Saving "); pcl::console::print_value ("%s ", filename.c_str ());
112
113 pcl::io::savePCDFileBinaryCompressed (filename, *output);
114
115 pcl::console::print_info ("[done, "); pcl::console::print_value ("%g", tt.toc ()); pcl::console::print_info (" ms : "); pcl::console::print_value ("%d", output->size ()); pcl::console::print_info (" points]\n");
116 }
117
118 int
batchProcess(const std::vector<std::string> & pcd_files,std::string & output_dir,float radius,bool inside,bool keep_organized)119 batchProcess (const std::vector<std::string> &pcd_files, std::string &output_dir,
120 float radius, bool inside, bool keep_organized)
121 {
122 std::vector<std::string> st;
123 for (const auto &pcd_file : pcd_files)
124 {
125 // Load the first file
126 Cloud::Ptr cloud (new Cloud);
127 if (!loadCloud (pcd_file, cloud))
128 return (-1);
129
130 // Perform the feature estimation
131 Cloud::Ptr output (new Cloud);
132 compute (cloud, output, radius, inside, keep_organized);
133
134 // Prepare output file name
135 std::string filename = boost::filesystem::path(pcd_file).filename().string();
136
137 // Save into the second file
138 const std::string filepath = output_dir + '/' + filename;
139 saveCloud (filepath, output);
140 }
141 return (0);
142 }
143
144
145 /* ---[ */
146 int
main(int argc,char ** argv)147 main (int argc, char** argv)
148 {
149 pcl::console::print_info ("Filter a point cloud using the pcl::TfQuadraticXYZComparison. For more information, use: %s -h\n", argv[0]);
150
151 if (argc < 3)
152 {
153 printHelp (argc, argv);
154 return (-1);
155 }
156
157 bool batch_mode = false;
158
159 // Command line parsing
160 float radius = default_radius;
161 bool inside = default_inside;
162 bool keep_organized = default_keep_organized;
163 pcl::console::parse_argument (argc, argv, "-radius", radius);
164 pcl::console::parse_argument (argc, argv, "-inside", inside);
165 pcl::console::parse_argument (argc, argv, "-keep", keep_organized);
166 std::string input_dir, output_dir;
167 if (pcl::console::parse_argument (argc, argv, "-input_dir", input_dir) != -1)
168 {
169 PCL_INFO ("Input directory given as %s. Batch process mode on.\n", input_dir.c_str ());
170 if (pcl::console::parse_argument (argc, argv, "-output_dir", output_dir) == -1)
171 {
172 PCL_ERROR ("Need an output directory! Please use -output_dir to continue.\n");
173 return (-1);
174 }
175
176 // Both input dir and output dir given, switch into batch processing mode
177 batch_mode = true;
178 }
179
180 if (!batch_mode)
181 {
182 // Parse the command line arguments for .pcd files
183 std::vector<int> p_file_indices;
184 p_file_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
185 if (p_file_indices.size () != 2)
186 {
187 pcl::console::print_error ("Need one input PCD file and one output PCD file to continue.\n");
188 return (-1);
189 }
190
191 // Load the first file
192 Cloud::Ptr cloud (new Cloud);
193 if (!loadCloud (argv[p_file_indices[0]], cloud))
194 return (-1);
195
196 // Perform the feature estimation
197 Cloud::Ptr output (new Cloud);
198 compute (cloud, output, radius, inside, keep_organized);
199
200 // Save into the second file
201 saveCloud (argv[p_file_indices[1]], output);
202 }
203 else
204 {
205 if (!input_dir.empty() && boost::filesystem::exists (input_dir))
206 {
207 std::vector<std::string> pcd_files;
208 boost::filesystem::directory_iterator end_itr;
209 for (boost::filesystem::directory_iterator itr (input_dir); itr != end_itr; ++itr)
210 {
211 // Only add PCD files
212 if (!is_directory (itr->status ()) && boost::algorithm::to_upper_copy (boost::filesystem::extension (itr->path ())) == ".PCD" )
213 {
214 pcd_files.push_back (itr->path ().string ());
215 PCL_INFO ("[Batch processing mode] Added %s for processing.\n", itr->path ().string ().c_str ());
216 }
217 }
218 batchProcess (pcd_files, output_dir, radius, inside, keep_organized);
219 }
220 else
221 {
222 PCL_ERROR ("Batch processing mode enabled, but invalid input directory (%s) given!\n", input_dir.c_str ());
223 return (-1);
224 }
225 }
226 }
227