plane_projection.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2011, Willow Garage, Inc.
00006  *  
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id: plane_projection.cpp 1032 2011-05-18 22:43:27Z marton $
00037  *
00038  */
00039 
00040 #include <sensor_msgs/PointCloud2.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, sensor_msgs::PointCloud2 &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 sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output, float a, float b, float c, float d)
00080 {
00081   Eigen::Vector4f coeffs;
00082   coeffs << a, b, c, d;
00083 
00084   // Convert data to PointCloud<T>
00085   PointCloud<PointXYZ>::Ptr xyz (new PointCloud<PointXYZ>);
00086   fromROSMsg (*input, *xyz);
00087 
00088   // Estimate
00089   TicToc tt;
00090   tt.tic ();
00091 
00092   //First, we'll find a point on the plane
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   // Convert data back
00114   sensor_msgs::PointCloud2 projected_cloud;
00115   toROSMsg (*projected_cloud_pcl, projected_cloud);
00116 
00117   //we can actually use concatenate fields to inject our projection into the
00118   //output, the second argument overwrites the first's fields for those that
00119   //are shared
00120   concatenateFields (*input, projected_cloud, output);
00121 }
00122 
00123 void
00124 saveCloud (const std::string &filename, const sensor_msgs::PointCloud2 &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   // Parse the command line arguments for .pcd files
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   // Command line parsing
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   // Load the first file
00170   sensor_msgs::PointCloud2::Ptr cloud (new sensor_msgs::PointCloud2);
00171   if (!loadCloud (argv[p_file_indices[0]], *cloud)) 
00172     return (-1);
00173 
00174   // Perform the feature estimation
00175   sensor_msgs::PointCloud2 output;
00176   project (cloud, output, a, b, c, d);
00177 
00178   // Save into the second file
00179   saveCloud (argv[p_file_indices[1]], output);
00180 }
00181 


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:16:32