transform_point_cloud.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: transform_point_cloud.cpp 1032 2011-05-18 22:43:27Z mdixon $
00035  *
00036  */
00037 
00038 #include <Eigen/Core>
00039 #include <sensor_msgs/PointCloud2.h>
00040 #include <pcl/ros/conversions.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/common/transforms.h>
00046 #include <cmath>
00047 
00048 using namespace pcl;
00049 using namespace pcl::io;
00050 using namespace pcl::console;
00051 
00052 void
00053 printHelp (int, char **argv)
00054 {
00055   print_error ("Syntax is: %s input.pcd output.pcd <options>\n", argv[0]);
00056   print_info ("  where options are:\n");
00057   print_info ("           -trans dx,dy,dz           = the translation (default: "); 
00058   print_value ("%0.1f, %0.1f, %0.1f", 0, 0, 0); print_info (")\n");
00059   print_info ("           -quat w,x,y,z             = rotation as quaternion\n"); 
00060   print_info ("           -axisangle ax,ay,az,theta = rotation in axis-angle form\n"); 
00061   print_info ("           -matrix v1,v2,...,v8,v9   = a 3x3 affine transform\n");
00062   print_info ("           -matrix v1,v2,...,v15,v16 = a 4x4 transformation matrix\n");
00063   print_info ("   Note: If a rotation is not specified, it will default to no rotation.\n");
00064   print_info ("         If redundant or conflicting transforms are specified, then:\n");
00065   print_info ("           -axisangle will override -quat\n");
00066   print_info ("           -matrix (3x3) will take override -axisangle and -quat\n");
00067   print_info ("           -matrix (4x4) will take override all other arguments\n");
00068 
00069 }
00070 
00071 void printElapsedTimeAndNumberOfPoints (double t, int w, int h=1)
00072 {
00073   print_info ("[done, "); print_value ("%g", t); print_info (" ms : "); 
00074   print_value ("%d", w*h); print_info (" points]\n");
00075 }
00076 
00077 bool
00078 loadCloud (const std::string &filename, sensor_msgs::PointCloud2 &cloud)
00079 {
00080   TicToc tt;
00081   print_highlight ("Loading "); print_value ("%s ", filename.c_str ());
00082 
00083   tt.tic ();
00084   if (loadPCDFile (filename, cloud) < 0)
00085     return (false);
00086 
00087   printElapsedTimeAndNumberOfPoints (tt.toc (), cloud.width, cloud.height);
00088 
00089   print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (cloud).c_str ());
00090 
00091   return (true);
00092 }
00093 
00094 template <typename PointT>
00095 void
00096 transformPointCloudHelper (PointCloud<PointT> & input, PointCloud<PointT> & output,
00097                            Eigen::Matrix4f &tform)
00098 {
00099   transformPointCloud (input, output, tform);
00100 }
00101 
00102 template <>
00103 void
00104 transformPointCloudHelper (PointCloud<PointNormal> & input, PointCloud<PointNormal> & output, 
00105                            Eigen::Matrix4f &tform)
00106 {
00107   transformPointCloudWithNormals (input, output, tform);
00108 }
00109 
00110 template <>
00111 void
00112 transformPointCloudHelper<PointXYZRGBNormal> (PointCloud<PointXYZRGBNormal> & input, 
00113                                               PointCloud<PointXYZRGBNormal> & output, 
00114                                               Eigen::Matrix4f &tform)
00115 {
00116   transformPointCloudWithNormals (input, output, tform);
00117 }
00118 
00119 
00120 template <typename PointT>
00121 void
00122 transformPointCloud2AsType (const sensor_msgs::PointCloud2 &input, sensor_msgs::PointCloud2 &output,
00123                             Eigen::Matrix4f &tform)
00124 {
00125   PointCloud<PointT> cloud;
00126   fromROSMsg (input, cloud);
00127   transformPointCloudHelper (cloud, cloud, tform);
00128   toROSMsg (cloud, output);
00129 }
00130 
00131 void
00132 transformPointCloud2 (const sensor_msgs::PointCloud2 &input, sensor_msgs::PointCloud2 &output,
00133                       Eigen::Matrix4f &tform)
00134 {
00135   // Check for 'rgb' and 'normals' fields
00136   bool has_rgb = false;
00137   bool has_normals = false;
00138   for (size_t i = 0; i < input.fields.size (); ++i)
00139   {
00140     if (input.fields[i].name == "rgb")
00141       has_rgb = true;
00142     if (input.fields[i].name == "normals")
00143       has_normals = true;
00144   }
00145 
00146   // Handle the following four point types differently: PointXYZ, PointXYZRGB, PointNormal, PointXYZRGBNormal
00147   if (!has_rgb && !has_normals)
00148     transformPointCloud2AsType<PointXYZ> (input, output, tform);
00149   else if (has_rgb && !has_normals)
00150     transformPointCloud2AsType<PointXYZRGB> (input, output, tform);
00151   else if (!has_rgb && has_normals)
00152     transformPointCloud2AsType<PointNormal> (input, output, tform);
00153   else // (has_rgb && has_normals)
00154     transformPointCloud2AsType<PointXYZRGBNormal> (input, output, tform);
00155 }
00156 
00157 void
00158 compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
00159                 Eigen::Matrix4f &tform)
00160 {
00161   TicToc tt;
00162   tt.tic ();
00163   
00164   print_highlight ("Transforming ");
00165 
00166   transformPointCloud2 (*input, output, tform);
00167 
00168   printElapsedTimeAndNumberOfPoints (tt.toc (), output.width, output.height);
00169 
00170 }
00171 
00172 void
00173 saveCloud (const std::string &filename, const sensor_msgs::PointCloud2 &output)
00174 {
00175   TicToc tt;
00176   tt.tic ();
00177 
00178   print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
00179   
00180   pcl::io::savePCDFile (filename, output);
00181   
00182   printElapsedTimeAndNumberOfPoints (tt.toc (), output.width, output.height);
00183 }
00184 
00185 /* ---[ */
00186 int
00187 main (int argc, char** argv)
00188 {
00189   print_info ("Transform a cloud. For more information, use: %s -h\n", argv[0]);
00190 
00191   if (argc < 3)
00192   {
00193     printHelp (argc, argv);
00194     return (-1);
00195   }
00196 
00197   // Parse the command line arguments for .pcd files
00198   std::vector<int> p_file_indices;
00199   p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
00200   if (p_file_indices.size () != 2)
00201   {
00202     print_error ("Need one input PCD file and one output PCD file to continue.\n");
00203     return (-1);
00204   }
00205 
00206   // Initialize the transformation matrix
00207   Eigen::Matrix4f tform; 
00208   tform.setIdentity ();
00209 
00210   // Command line parsing
00211   float dx, dy, dz;
00212   std::vector<float> values;
00213 
00214   if (parse_3x_arguments (argc, argv, "-trans", dx, dy, dz) > -1)
00215   {
00216     tform (0, 3) = dx;
00217     tform (1, 3) = dy;
00218     tform (2, 3) = dz;
00219   }
00220 
00221   if (parse_x_arguments (argc, argv, "-quat", values) > -1)
00222   {
00223     if (values.size () == 4)
00224     {
00225       const float& x = values[0];
00226       const float& y = values[1];
00227       const float& z = values[2];
00228       const float& w = values[3];
00229       tform.topLeftCorner (3, 3) = Eigen::Matrix3f (Eigen::Quaternionf (w, x, y, z));
00230     }
00231     else
00232     {
00233       print_error ("Wrong number of values given (%zu): ", values.size ());
00234       print_error ("The quaternion specified with -quat must contain 4 elements (w,x,y,z).\n");
00235     }
00236   }
00237 
00238   if (parse_x_arguments (argc, argv, "-axisangle", values) > -1)
00239   {
00240     if (values.size () == 4)
00241     {
00242       const float& ax = values[0];
00243       const float& ay = values[1];
00244       const float& az = values[2];
00245       const float& theta = values[3];
00246       tform.topLeftCorner (3, 3) = Eigen::Matrix3f (Eigen::AngleAxisf (theta, Eigen::Vector3f (ax, ay, az)));
00247     }
00248     else
00249     {
00250       print_error ("Wrong number of values given (%zu): ", values.size ());
00251       print_error ("The rotation specified with -axisangle must contain 4 elements (ax,ay,az,theta).\n");
00252     }
00253   }
00254 
00255   if (parse_x_arguments (argc, argv, "-matrix", values) > -1)
00256   {
00257     if (values.size () == 9 || values.size () == 16)
00258     {
00259       int n = values.size () == 9 ? 3 : 4;
00260       for (int r = 0; r < n; ++r)
00261         for (int c = 0; c < n; ++c)
00262           tform (r, c) = values[n*r+c];
00263     }
00264     else
00265     {
00266       print_error ("Wrong number of values given (%zu): ", values.size ());
00267       print_error ("The transformation specified with -matrix must be 3x3 (9) or 4x4 (16).\n");
00268     }
00269   }
00270 
00271   // Load the first file
00272   sensor_msgs::PointCloud2::Ptr cloud (new sensor_msgs::PointCloud2);
00273   if (!loadCloud (argv[p_file_indices[0]], *cloud)) 
00274     return (-1);
00275 
00276   // Apply the transform
00277   sensor_msgs::PointCloud2 output;
00278   compute (cloud, output, tform);
00279 
00280   // Save into the second file
00281   saveCloud (argv[p_file_indices[1]], output);
00282 }
00283 


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:18:50