io.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: io.cpp 6126 2012-07-03 20:19:58Z aichim $
00037  *
00038  */
00039 
00040 #include <pcl/point_types.h>
00041 #include <pcl/common/io.h>
00042 
00044 void
00045 getFieldsSizes (const std::vector<sensor_msgs::PointField> &fields,
00046                 std::vector<int> &fields_sizes)
00047 {
00048   int valid = 0;
00049   fields_sizes.resize (fields.size ());
00050   for (size_t i = 0; i < fields.size (); ++i)
00051   {
00052     if (fields[i].name == "_")
00053       continue;
00054 
00055     int fs = fields[i].count * pcl::getFieldSize (fields[i].datatype);
00056     fields_sizes[i] = fs;
00057     valid++;
00058   }
00059   fields_sizes.resize (valid);
00060 }
00061 
00062 bool fieldComp (const sensor_msgs::PointField* i, const sensor_msgs::PointField* j)
00063 {
00064   return i->offset < j->offset;
00065 }
00066 
00068 bool
00069 pcl::concatenateFields (const sensor_msgs::PointCloud2 &cloud1, 
00070                         const sensor_msgs::PointCloud2 &cloud2, 
00071                         sensor_msgs::PointCloud2 &cloud_out)
00072 {
00073   // If the cloud's sizes differ (points wise), then exit with error
00074   if (cloud1.width != cloud2.width || cloud1.height != cloud2.height)
00075   {
00076     PCL_ERROR ("[pcl::concatenateFields] Dimensions of input clouds do not match: cloud1 (w, %d, h, %d), cloud2 (w, %d, h, %d)\n", cloud1.width, cloud1.height, cloud2.width, cloud2.height );
00077     return (false);
00078   }
00079   
00080 
00081   if (cloud1.is_bigendian != cloud2.is_bigendian)
00082   {
00083     PCL_ERROR ("[pcl::concatenateFields] Endianness of clouds does not match\n");
00084     return (false);
00085   }
00086   
00087   // Else, copy the second cloud (width, height, header stay the same)
00088   // we do this since fields from the second cloud are supposed to overwrite
00089   // those of the first
00090   cloud_out.header = cloud2.header;
00091   cloud_out.fields = cloud2.fields;
00092   cloud_out.width = cloud2.width;
00093   cloud_out.height = cloud2.height;
00094   cloud_out.is_bigendian = cloud2.is_bigendian;
00095 
00096   //We need to find how many fields overlap between the two clouds
00097   size_t total_fields = cloud2.fields.size ();
00098 
00099   //for the non-matching fields in cloud1, we need to store the offset
00100   //from the beginning of the point
00101   std::vector<const sensor_msgs::PointField*> cloud1_unique_fields;
00102   std::vector<int> field_sizes;
00103 
00104   //We need to make sure that the fields for cloud 1 are sorted
00105   //by offset so that we can compute sizes correctly. There is no
00106   //guarantee that the fields are in the correct order when they come in
00107   std::vector<const sensor_msgs::PointField*> cloud1_fields_sorted;
00108   for (size_t i = 0; i < cloud1.fields.size (); ++i)
00109     cloud1_fields_sorted.push_back (&(cloud1.fields[i]));
00110 
00111   std::sort (cloud1_fields_sorted.begin (), cloud1_fields_sorted.end (), fieldComp);
00112 
00113   for (size_t i = 0; i < cloud1_fields_sorted.size (); ++i)
00114   {
00115     bool match = false;
00116     for (size_t j = 0; j < cloud2.fields.size (); ++j)
00117     {
00118       if (cloud1_fields_sorted[i]->name == cloud2.fields[j].name)
00119         match = true;
00120     }
00121 
00122     //if the field is new, we'll increment out total fields
00123     if (!match && cloud1_fields_sorted[i]->name != "_")
00124     {
00125       cloud1_unique_fields.push_back (cloud1_fields_sorted[i]);
00126 
00127       int size = 0;
00128       size_t next_valid_field = i + 1;
00129 
00130       while (next_valid_field < cloud1_fields_sorted.size())
00131       {
00132         if (cloud1_fields_sorted[next_valid_field]->name != "_")
00133           break;
00134         next_valid_field++;
00135       }
00136 
00137       if (next_valid_field < cloud1_fields_sorted.size ())
00138         //compute the true size of the field, including padding
00139         size = cloud1_fields_sorted[next_valid_field]->offset - cloud1_fields_sorted[i]->offset;
00140       else
00141         //for the last point, we'll just use the point step to compute the size
00142         size = cloud1.point_step - cloud1_fields_sorted[i]->offset;
00143 
00144       field_sizes.push_back (size);
00145 
00146       total_fields++;
00147     }
00148   }
00149 
00150   //we need to compute the size of the additional data added from cloud 1
00151   uint32_t cloud1_unique_point_step = 0;
00152   for (size_t i = 0; i < cloud1_unique_fields.size (); ++i)
00153     cloud1_unique_point_step += field_sizes[i];
00154 
00155   //the total size of extra data should be the size of data per point
00156   //multiplied by the total number of points in the cloud
00157   uint32_t cloud1_unique_data_size = cloud1_unique_point_step * cloud1.width * cloud1.height; 
00158 
00159   // Point step must increase with the length of each matching field
00160   cloud_out.point_step = cloud2.point_step + cloud1_unique_point_step;
00161   // Recalculate row_step
00162   cloud_out.row_step = cloud_out.point_step * cloud_out.width;
00163 
00164   // Resize data to hold all clouds
00165   cloud_out.data.resize (cloud2.data.size () + cloud1_unique_data_size);
00166 
00167   // Concatenate fields
00168   cloud_out.fields.resize (cloud2.fields.size () + cloud1_unique_fields.size ());
00169   int offset = cloud2.point_step;
00170 
00171   for (size_t d = 0; d < cloud1_unique_fields.size (); ++d)
00172   {
00173     const sensor_msgs::PointField& f = *cloud1_unique_fields[d];
00174     cloud_out.fields[cloud2.fields.size () + d].name = f.name;
00175     cloud_out.fields[cloud2.fields.size () + d].datatype = f.datatype;
00176     cloud_out.fields[cloud2.fields.size () + d].count = f.count;
00177     // Adjust the offset
00178     cloud_out.fields[cloud2.fields.size () + d].offset = offset;
00179     offset += field_sizes[d];
00180   }
00181  
00182   // Iterate over each point and perform the appropriate memcpys
00183   int point_offset = 0;
00184   for (size_t cp = 0; cp < cloud_out.width * cloud_out.height; ++cp)
00185   {
00186     memcpy (&cloud_out.data[point_offset], &cloud2.data[cp * cloud2.point_step], cloud2.point_step);
00187     int field_offset = cloud2.point_step;
00188 
00189     // Copy each individual point, we have to do this on a per-field basis
00190     // since some fields are not unique
00191     for (size_t i = 0; i < cloud1_unique_fields.size (); ++i)
00192     {
00193       const sensor_msgs::PointField& f = *cloud1_unique_fields[i];
00194       int local_data_size = f.count * pcl::getFieldSize (f.datatype);
00195       int padding_size = field_sizes[i] - local_data_size;
00196       
00197       memcpy (&cloud_out.data[point_offset + field_offset], &cloud1.data[cp * cloud1.point_step + f.offset], local_data_size);
00198       field_offset +=  local_data_size;
00199 
00200       //make sure that we add padding when its needed
00201       if (padding_size > 0)
00202         memset (&cloud_out.data[point_offset + field_offset], 0, padding_size);
00203       field_offset += padding_size;
00204     }
00205     point_offset += field_offset;
00206   }
00207 
00208   if (!cloud1.is_dense || !cloud2.is_dense)
00209     cloud_out.is_dense = false;
00210   else
00211     cloud_out.is_dense = true;
00212 
00213   return (true);
00214 }
00215 
00217 bool
00218 pcl::concatenatePointCloud (const sensor_msgs::PointCloud2 &cloud1, 
00219                             const sensor_msgs::PointCloud2 &cloud2, 
00220                             sensor_msgs::PointCloud2 &cloud_out)
00221 {
00222   if (cloud1.fields.size () != cloud2.fields.size ())
00223   {
00224     PCL_ERROR ("[pcl::concatenatePointCloud] Number of fields in cloud1 (%u) != Number of fields in cloud2 (%u)\n", cloud1.fields.size (), cloud2.fields.size ());
00225     return (false);
00226   }
00227   
00228   for (size_t i = 0; i < cloud1.fields.size (); ++i)
00229     if (cloud1.fields[i].name != cloud2.fields[i].name)
00230     {
00231       PCL_ERROR ("[pcl::concatenatePointCloud] Name of field %d in cloud1, %s, does not match name in cloud2, %s\n", i, cloud1.fields[i].name.c_str (), cloud2.fields[i].name.c_str () );      
00232       return (false);
00233     }
00234   
00235   // Copy cloud1 into cloud_out
00236   cloud_out = cloud1;
00237   size_t nrpts = cloud_out.data.size ();
00238   cloud_out.data.resize (nrpts + cloud2.data.size ());
00239   memcpy (&cloud_out.data[nrpts], &cloud2.data[0], cloud2.data.size ());
00240 
00241   // Height = 1 => no more organized
00242   cloud_out.width    = cloud1.width * cloud1.height + cloud2.width * cloud2.height;
00243   cloud_out.height   = 1;
00244   if (!cloud1.is_dense || !cloud2.is_dense)
00245     cloud_out.is_dense = false;
00246   else
00247     cloud_out.is_dense = true;
00248 
00249   return (true);
00250 }
00251 
00253 bool
00254 pcl::getPointCloudAsEigen (const sensor_msgs::PointCloud2 &in, Eigen::MatrixXf &out)
00255 {
00256   // Get X-Y-Z indices
00257   int x_idx = getFieldIndex (in, "x");
00258   int y_idx = getFieldIndex (in, "y");
00259   int z_idx = getFieldIndex (in, "z");
00260 
00261   if (x_idx == -1 || y_idx == -1 || z_idx == -1)
00262   {
00263     PCL_ERROR ("Input dataset has no X-Y-Z coordinates! Cannot convert to Eigen format.\n");
00264     return (false);
00265   }
00266 
00267   if (in.fields[x_idx].datatype != sensor_msgs::PointField::FLOAT32 || 
00268       in.fields[y_idx].datatype != sensor_msgs::PointField::FLOAT32 || 
00269       in.fields[z_idx].datatype != sensor_msgs::PointField::FLOAT32)
00270   {
00271     PCL_ERROR ("X-Y-Z coordinates not floats. Currently only floats are supported.\n");
00272     return (false);
00273   }
00274 
00275   size_t npts = in.width * in.height;
00276   out = Eigen::MatrixXf::Ones (4, npts);
00277 
00278   Eigen::Array4i xyz_offset (in.fields[x_idx].offset, in.fields[y_idx].offset, in.fields[z_idx].offset, 0);
00279 
00280   // Copy the input dataset into Eigen format
00281   for (size_t i = 0; i < npts; ++i)
00282   {
00283      // Unoptimized memcpys: assume fields x, y, z are in random order
00284      memcpy (&out (0, i), &in.data[xyz_offset[0]], sizeof (float));
00285      memcpy (&out (1, i), &in.data[xyz_offset[1]], sizeof (float));
00286      memcpy (&out (2, i), &in.data[xyz_offset[2]], sizeof (float));
00287 
00288      xyz_offset += in.point_step;
00289   }
00290 
00291   return (true);
00292 }
00293 
00295 bool 
00296 pcl::getEigenAsPointCloud (Eigen::MatrixXf &in, sensor_msgs::PointCloud2 &out)
00297 {
00298   // Get X-Y-Z indices
00299   int x_idx = getFieldIndex (out, "x");
00300   int y_idx = getFieldIndex (out, "y");
00301   int z_idx = getFieldIndex (out, "z");
00302 
00303   if (x_idx == -1 || y_idx == -1 || z_idx == -1)
00304   {
00305     PCL_ERROR ("Output dataset has no X-Y-Z coordinates set up as fields! Cannot convert from Eigen format.\n");
00306     return (false);
00307   }
00308 
00309   if (out.fields[x_idx].datatype != sensor_msgs::PointField::FLOAT32 || 
00310       out.fields[y_idx].datatype != sensor_msgs::PointField::FLOAT32 || 
00311       out.fields[z_idx].datatype != sensor_msgs::PointField::FLOAT32)
00312   {
00313     PCL_ERROR ("X-Y-Z coordinates not floats. Currently only floats are supported.\n");
00314     return (false);
00315   }
00316 
00317   if (in.cols () != static_cast<int>(out.width * out.height))
00318   {
00319     PCL_ERROR ("Number of points in the point cloud differs from the Eigen matrix. Cannot continue.\n");
00320     return (false);
00321   }
00322 
00323   size_t npts = in.cols ();
00324 
00325   Eigen::Array4i xyz_offset (out.fields[x_idx].offset, out.fields[y_idx].offset, out.fields[z_idx].offset, 0);
00326 
00327   // Copy the input dataset into Eigen format
00328   for (size_t i = 0; i < npts; ++i)
00329   {
00330      // Unoptimized memcpys: assume fields x, y, z are in random order
00331      memcpy (&out.data[xyz_offset[0]], &in (0, i), sizeof (float));
00332      memcpy (&out.data[xyz_offset[1]], &in (1, i), sizeof (float));
00333      memcpy (&out.data[xyz_offset[2]], &in (2, i), sizeof (float));
00334 
00335      xyz_offset += out.point_step;
00336   }
00337 
00338   return (true);
00339 }
00340 
00342 void 
00343 pcl::copyPointCloud (
00344     const sensor_msgs::PointCloud2 &cloud_in, const std::vector<int> &indices, 
00345     sensor_msgs::PointCloud2 &cloud_out)
00346 {
00347   cloud_out.header       = cloud_in.header;
00348   cloud_out.height       = 1;
00349   cloud_out.width        = static_cast<uint32_t> (indices.size ()); 
00350   cloud_out.fields       = cloud_in.fields;
00351   cloud_out.is_bigendian = cloud_in.is_bigendian;
00352   cloud_out.point_step   = cloud_in.point_step;
00353   cloud_out.row_step     = cloud_in.row_step;
00354   if (cloud_in.is_dense)
00355     cloud_out.is_dense = true;
00356   else
00357     // It's not necessarily true that is_dense is false if cloud_in.is_dense is false
00358     // To verify this, we would need to iterate over all points and check for NaNs
00359     cloud_out.is_dense = false;
00360 
00361   cloud_out.data.resize (cloud_out.width * cloud_out.height * cloud_out.point_step);
00362 
00363   // Iterate over each point
00364   for (size_t i = 0; i < indices.size (); ++i)
00365   {
00366     memcpy (&cloud_out.data[i * cloud_out.point_step], &cloud_in.data[indices[i] * cloud_in.point_step], cloud_in.point_step);
00367     // Check for NaNs, set is_dense to true/false based on this
00368     // ...
00369   }
00370 }
00371 
00373 void 
00374 pcl::copyPointCloud (
00375     const sensor_msgs::PointCloud2 &cloud_in, 
00376     sensor_msgs::PointCloud2 &cloud_out)
00377 {
00378   cloud_out.header       = cloud_in.header;
00379   cloud_out.height       = cloud_in.height;
00380   cloud_out.width        = cloud_in.width;
00381   cloud_out.fields       = cloud_in.fields;
00382   cloud_out.is_bigendian = cloud_in.is_bigendian;
00383   cloud_out.point_step   = cloud_in.point_step;
00384   cloud_out.row_step     = cloud_in.row_step;
00385 
00386   if (cloud_in.is_dense)
00387     cloud_out.is_dense = true;
00388   else
00389     cloud_out.is_dense = false;
00390 
00391   cloud_out.data.resize (cloud_out.width * cloud_out.height * cloud_out.point_step);
00392 
00393   //copy the data directly
00394   memcpy ( &cloud_out.data[0], &cloud_in.data[0], cloud_in.point_step * cloud_in.width * cloud_in.height );
00395 }


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:15:30