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
00039 #include <pcl/PCLPointCloud2.h>
00040 #include <pcl/point_types.h>
00041 #include <pcl/io/pcd_io.h>
00042 #include <pcl/console/print.h>
00043 #include <pcl/console/parse.h>
00044 #include <pcl/console/time.h>
00045 #include <pcl/filters/passthrough.h>
00046
00047
00048 using namespace std;
00049 using namespace pcl;
00050 using namespace pcl::io;
00051 using namespace pcl::console;
00052
00053 float default_min = 0.0f,
00054 default_max = 1.0f;
00055 bool default_inside = true;
00056 bool default_keep_organized = true;
00057 std::string default_field_name = "z";
00058
00059 void
00060 printHelp (int, char **argv)
00061 {
00062 print_error ("Syntax is: %s input.pcd output.pcd <options>\n", argv[0]);
00063 print_info (" where options are:\n");
00064 print_info (" -field X = the field of the point cloud we want to apply the filter to (default: ");
00065 print_value ("%s", default_field_name.c_str ()); print_info (")\n");
00066 print_info (" -min X = lower limit of the filter (default: ");
00067 print_value ("%f", default_min); print_info (")\n");
00068 print_info (" -max X = upper limit of the filter (default: ");
00069 print_value ("%f", default_max); print_info (")\n");
00070 print_info (" -inside X = keep the points inside the [min, max] interval or not (default: ");
00071 print_value ("%d", default_inside); print_info (")\n");
00072 print_info (" -keep 0/1 = keep the points organized (1) or not (default: ");
00073 print_value ("%d", default_keep_organized); print_info (")\n");
00074 }
00075
00076 bool
00077 loadCloud (const std::string &filename, pcl::PCLPointCloud2 &cloud)
00078 {
00079 TicToc tt;
00080 print_highlight ("Loading "); print_value ("%s ", filename.c_str ());
00081
00082 tt.tic ();
00083 if (loadPCDFile (filename, cloud) < 0)
00084 return (false);
00085 print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]\n");
00086 print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (cloud).c_str ());
00087
00088 return (true);
00089 }
00090
00091 void
00092 compute (const pcl::PCLPointCloud2::ConstPtr &input, pcl::PCLPointCloud2 &output,
00093 std::string field_name, float min, float max, bool inside, bool keep_organized)
00094 {
00095
00096 TicToc tt;
00097 tt.tic ();
00098
00099 print_highlight (stderr, "Computing ");
00100
00101 PassThrough<pcl::PCLPointCloud2> passthrough_filter;
00102 passthrough_filter.setInputCloud (input);
00103 passthrough_filter.setFilterFieldName (field_name);
00104 passthrough_filter.setFilterLimits (min, max);
00105 passthrough_filter.setFilterLimitsNegative (!inside);
00106 passthrough_filter.setKeepOrganized (keep_organized);
00107 passthrough_filter.filter (output);
00108
00109 print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n");
00110 }
00111
00112 void
00113 saveCloud (const std::string &filename, const pcl::PCLPointCloud2 &output)
00114 {
00115 TicToc tt;
00116 tt.tic ();
00117
00118 print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
00119
00120 PCDWriter w;
00121 w.writeBinaryCompressed (filename, output);
00122
00123 print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n");
00124 }
00125
00126 int
00127 batchProcess (const vector<string> &pcd_files, string &output_dir,
00128 std::string field_name, float min, float max, bool inside, bool keep_organized)
00129 {
00130 vector<string> st;
00131 for (size_t i = 0; i < pcd_files.size (); ++i)
00132 {
00133
00134 pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2);
00135 if (!loadCloud (pcd_files[i], *cloud))
00136 return (-1);
00137
00138
00139 pcl::PCLPointCloud2 output;
00140 compute (cloud, output, field_name, min, max, inside, keep_organized);
00141
00142
00143 string filename = pcd_files[i];
00144 boost::trim (filename);
00145 boost::split (st, filename, boost::is_any_of ("/\\"), boost::token_compress_on);
00146
00147
00148 stringstream ss;
00149 ss << output_dir << "/" << st.at (st.size () - 1);
00150 saveCloud (ss.str (), output);
00151 }
00152 return (0);
00153 }
00154
00155
00156
00157 int
00158 main (int argc, char** argv)
00159 {
00160 print_info ("Filter a point cloud using the pcl::PassThroughFilterEstimate. For more information, use: %s -h\n", argv[0]);
00161
00162 if (argc < 3)
00163 {
00164 printHelp (argc, argv);
00165 return (-1);
00166 }
00167
00168 bool batch_mode = false;
00169
00170
00171 float min = default_min, max = default_max;
00172 bool inside = default_inside;
00173 bool keep_organized = default_keep_organized;
00174 std::string field_name = default_field_name;
00175 parse_argument (argc, argv, "-min", min);
00176 parse_argument (argc, argv, "-max", max);
00177 parse_argument (argc, argv, "-inside", inside);
00178 parse_argument (argc, argv, "-field", field_name);
00179 parse_argument (argc, argv, "-keep", keep_organized);
00180 string input_dir, output_dir;
00181 if (parse_argument (argc, argv, "-input_dir", input_dir) != -1)
00182 {
00183 PCL_INFO ("Input directory given as %s. Batch process mode on.\n", input_dir.c_str ());
00184 if (parse_argument (argc, argv, "-output_dir", output_dir) == -1)
00185 {
00186 PCL_ERROR ("Need an output directory! Please use -output_dir to continue.\n");
00187 return (-1);
00188 }
00189
00190
00191 batch_mode = true;
00192 }
00193
00194 if (!batch_mode)
00195 {
00196
00197 std::vector<int> p_file_indices;
00198 p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
00199 if (p_file_indices.size () != 2)
00200 {
00201 print_error ("Need one input PCD file and one output PCD file to continue.\n");
00202 return (-1);
00203 }
00204
00205
00206 pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2);
00207 if (!loadCloud (argv[p_file_indices[0]], *cloud))
00208 return (-1);
00209
00210
00211 pcl::PCLPointCloud2 output;
00212 compute (cloud, output, field_name, min, max, inside, keep_organized);
00213
00214
00215 saveCloud (argv[p_file_indices[1]], output);
00216 }
00217 else
00218 {
00219 if (input_dir != "" && boost::filesystem::exists (input_dir))
00220 {
00221 vector<string> pcd_files;
00222 boost::filesystem::directory_iterator end_itr;
00223 for (boost::filesystem::directory_iterator itr (input_dir); itr != end_itr; ++itr)
00224 {
00225
00226 if (!is_directory (itr->status ()) && boost::algorithm::to_upper_copy (boost::filesystem::extension (itr->path ())) == ".PCD" )
00227 {
00228 pcd_files.push_back (itr->path ().string ());
00229 PCL_INFO ("[Batch processing mode] Added %s for processing.\n", itr->path ().string ().c_str ());
00230 }
00231 }
00232 batchProcess (pcd_files, output_dir, field_name, min, max, inside, keep_organized);
00233 }
00234 else
00235 {
00236 PCL_ERROR ("Batch processing mode enabled, but invalid input directory (%s) given!\n", input_dir.c_str ());
00237 return (-1);
00238 }
00239 }
00240 }