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 <sensor_msgs/PointCloud2.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 pcl;
00049 using namespace pcl::io;
00050 using namespace pcl::console;
00051
00052 float default_min = 0.0f,
00053 default_max = 1.0f;
00054 bool default_inside = true;
00055 bool default_keep_organized = true;
00056 std::string default_field_name = "z";
00057
00058 void
00059 printHelp (int, char **argv)
00060 {
00061 print_error ("Syntax is: %s input.pcd output.pcd <options>\n", argv[0]);
00062 print_info (" where options are:\n");
00063 print_info (" -field X = the field of the point cloud we want to apply the filter to (default: ");
00064 print_value ("%s", default_field_name.c_str ()); print_info (")\n");
00065 print_info (" -min X = lower limit of the filter (default: ");
00066 print_value ("%f", default_min); print_info (")\n");
00067 print_info (" -max X = upper limit of the filter (default: ");
00068 print_value ("%f", default_max); print_info (")\n");
00069 print_info (" -inside X = keep the points inside the [min, max] interval or not (default: ");
00070 print_value ("%d", default_inside); print_info (")\n");
00071 }
00072
00073 bool
00074 loadCloud (const std::string &filename, sensor_msgs::PointCloud2 &cloud)
00075 {
00076 TicToc tt;
00077 print_highlight ("Loading "); print_value ("%s ", filename.c_str ());
00078
00079 tt.tic ();
00080 if (loadPCDFile (filename, cloud) < 0)
00081 return (false);
00082 print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]\n");
00083 print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (cloud).c_str ());
00084
00085 return (true);
00086 }
00087
00088 void
00089 compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
00090 std::string field_name, float min, float max, bool inside, bool keep_organized)
00091 {
00092
00093 TicToc tt;
00094 tt.tic ();
00095
00096 print_highlight (stderr, "Computing ");
00097
00098 PassThrough<sensor_msgs::PointCloud2> passthrough_filter;
00099 passthrough_filter.setInputCloud (input);
00100 passthrough_filter.setFilterFieldName (field_name);
00101 passthrough_filter.setFilterLimits (min, max);
00102 passthrough_filter.setFilterLimitsNegative (!inside);
00103 passthrough_filter.setKeepOrganized (keep_organized);
00104 passthrough_filter.filter (output);
00105
00106 print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n");
00107 }
00108
00109 void
00110 saveCloud (const std::string &filename, const sensor_msgs::PointCloud2 &output)
00111 {
00112 TicToc tt;
00113 tt.tic ();
00114
00115 print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
00116
00117 PCDWriter w;
00118 w.writeBinaryCompressed (filename, output);
00119
00120 print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n");
00121 }
00122
00123
00124 int
00125 main (int argc, char** argv)
00126 {
00127 print_info ("Filter a point cloud using the pcl::PassThroughFilterEstimate. For more information, use: %s -h\n", argv[0]);
00128
00129 if (argc < 3)
00130 {
00131 printHelp (argc, argv);
00132 return (-1);
00133 }
00134
00135
00136 std::vector<int> p_file_indices;
00137 p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
00138 if (p_file_indices.size () != 2)
00139 {
00140 print_error ("Need one input PCD file and one output PCD file to continue.\n");
00141 return (-1);
00142 }
00143
00144
00145 float min = default_min, max = default_max;
00146 bool inside = default_inside;
00147 bool keep_organized = default_keep_organized;
00148 std::string field_name = default_field_name;
00149 parse_argument (argc, argv, "-min", min);
00150 parse_argument (argc, argv, "-max", max);
00151 parse_argument (argc, argv, "-inside", inside);
00152 parse_argument (argc, argv, "-field", field_name);
00153 parse_argument (argc, argv, "-keep", keep_organized);
00154
00155
00156 sensor_msgs::PointCloud2::Ptr cloud (new sensor_msgs::PointCloud2);
00157 if (!loadCloud (argv[p_file_indices[0]], *cloud))
00158 return (-1);
00159
00160
00161 sensor_msgs::PointCloud2 output;
00162 compute (cloud, output, field_name, min, max, inside, keep_organized);
00163
00164
00165 saveCloud (argv[p_file_indices[1]], output);
00166 }