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