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
00040 #include <pcl/PCLPointCloud2.h>
00041 #include <pcl/io/pcd_io.h>
00042 #include <pcl/features/normal_3d.h>
00043 #include <pcl/console/print.h>
00044 #include <pcl/console/parse.h>
00045 #include <pcl/console/time.h>
00046 #include <pcl/sample_consensus/sac_model_plane.h>
00047
00048 using namespace pcl;
00049 using namespace pcl::io;
00050 using namespace pcl::console;
00051
00052 Eigen::Vector4f translation;
00053 Eigen::Quaternionf orientation;
00054
00055 void
00056 printHelp (int, char **argv)
00057 {
00058 print_error ("Syntax is: %s input.pcd output.pcd A B C D\n", argv[0]);
00059 print_info (" where the plane is represented by the following equation:\n");
00060 print_info (" Ax + By + Cz + D = 0\n");
00061 }
00062
00063 bool
00064 loadCloud (const std::string &filename, pcl::PCLPointCloud2 &cloud)
00065 {
00066 TicToc tt;
00067 print_highlight ("Loading "); print_value ("%s ", filename.c_str ());
00068
00069 tt.tic ();
00070 if (loadPCDFile (filename, cloud, translation, orientation) < 0)
00071 return (false);
00072 print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]\n");
00073 print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (cloud).c_str ());
00074
00075 return (true);
00076 }
00077
00078 void
00079 project (const pcl::PCLPointCloud2::ConstPtr &input, pcl::PCLPointCloud2 &output, float a, float b, float c, float d)
00080 {
00081 Eigen::Vector4f coeffs;
00082 coeffs << a, b, c, d;
00083
00084
00085 PointCloud<PointXYZ>::Ptr xyz (new PointCloud<PointXYZ>);
00086 fromPCLPointCloud2 (*input, *xyz);
00087
00088
00089 TicToc tt;
00090 tt.tic ();
00091
00092
00093 print_highlight (stderr, "Projecting ");
00094
00095 PointCloud<PointXYZ>::Ptr projected_cloud_pcl (new PointCloud<PointXYZ>);
00096 projected_cloud_pcl->width = xyz->width;
00097 projected_cloud_pcl->height = xyz->height;
00098 projected_cloud_pcl->is_dense = xyz->is_dense;
00099 projected_cloud_pcl->sensor_origin_ = xyz->sensor_origin_;
00100 projected_cloud_pcl->sensor_orientation_ = xyz->sensor_orientation_;
00101
00102 for(size_t i = 0; i < xyz->points.size(); ++i)
00103 {
00104 pcl::PointXYZ projection;
00105 pcl::projectPoint<PointXYZ> (xyz->points[i], coeffs, projection);
00106 projected_cloud_pcl->points.push_back(projection);
00107 }
00108
00109
00110 print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : ");
00111 pcl::io::savePCDFile ("foo.pcd", *projected_cloud_pcl);
00112
00113
00114 pcl::PCLPointCloud2 projected_cloud;
00115 toPCLPointCloud2 (*projected_cloud_pcl, projected_cloud);
00116
00117
00118
00119
00120 concatenateFields (*input, projected_cloud, output);
00121 }
00122
00123 void
00124 saveCloud (const std::string &filename, const pcl::PCLPointCloud2 &output)
00125 {
00126 TicToc tt;
00127 tt.tic ();
00128
00129 print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
00130
00131 pcl::io::savePCDFile (filename, output, translation, orientation, false);
00132
00133 print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n");
00134 }
00135
00136
00137 int
00138 main (int argc, char** argv)
00139 {
00140 print_info ("Estimate surface normals using pcl::NormalEstimation. For more information, use: %s -h\n", argv[0]);
00141
00142 if (argc < 3)
00143 {
00144 printHelp (argc, argv);
00145 return (-1);
00146 }
00147
00148
00149 std::vector<int> p_file_indices;
00150 p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
00151 if (p_file_indices.size () != 2)
00152 {
00153 print_error ("Need one input PCD file and one output PCD file to continue.\n");
00154 return (-1);
00155 }
00156
00157 if(argc != 7)
00158 {
00159 print_error("This function takes: input_file output_file A B C D");
00160 return(-1);
00161 }
00162
00163
00164 float a = static_cast<float> (atof (argv[3]));
00165 float b = static_cast<float> (atof (argv[4]));
00166 float c = static_cast<float> (atof (argv[5]));
00167 float d = static_cast<float> (atof (argv[6]));
00168
00169
00170 pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2);
00171 if (!loadCloud (argv[p_file_indices[0]], *cloud))
00172 return (-1);
00173
00174
00175 pcl::PCLPointCloud2 output;
00176 project (cloud, output, a, b, c, d);
00177
00178
00179 saveCloud (argv[p_file_indices[1]], output);
00180 }
00181