io.h

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.h 2413 2011-09-07 07:01:00Z rusu $
00037  *
00038  */
00039 
00040 #ifndef PCL_COMMON_IO_H_
00041 #define PCL_COMMON_IO_H_
00042 
00043 #include <string>
00044 #include <boost/fusion/sequence/intrinsic/at_key.hpp>
00045 #include "pcl/pcl_base.h"
00046 #include "pcl/PointIndices.h"
00047 #include "pcl/ros/conversions.h"
00048 #include <locale>
00049 
00050 namespace pcl
00051 {
00057   inline int
00058   getFieldIndex (const sensor_msgs::PointCloud2 &cloud, const std::string &field_name)
00059   {
00060     // Get the index we need
00061     for (size_t d = 0; d < cloud.fields.size (); ++d)
00062       if (cloud.fields[d].name == field_name)
00063         return ((int) d);
00064     return (-1);
00065   }
00066 
00073   template <typename PointT> inline int 
00074   getFieldIndex (const pcl::PointCloud<PointT> &cloud, const std::string &field_name, 
00075                  std::vector<sensor_msgs::PointField> &fields);
00076 
00082   template <typename PointT> inline void 
00083   getFields (const pcl::PointCloud<PointT> &cloud, std::vector<sensor_msgs::PointField> &fields);
00084 
00089   template <typename PointT> inline std::string 
00090   getFieldsList (const pcl::PointCloud<PointT> &cloud);
00091 
00096   inline std::string
00097   getFieldsList (const sensor_msgs::PointCloud2 &cloud)
00098   {
00099     std::string result;
00100     for (size_t i = 0; i < cloud.fields.size () - 1; ++i)
00101       result += cloud.fields[i].name + " ";
00102     result += cloud.fields[cloud.fields.size () - 1].name;
00103     return (result);
00104   }
00105 
00110   inline int
00111   getFieldSize (const int datatype)
00112   {
00113     switch (datatype)
00114     {
00115       case sensor_msgs::PointField::INT8:
00116       case sensor_msgs::PointField::UINT8:
00117         return (1);
00118 
00119       case sensor_msgs::PointField::INT16:
00120       case sensor_msgs::PointField::UINT16:
00121         return (2);
00122 
00123       case sensor_msgs::PointField::INT32:
00124       case sensor_msgs::PointField::UINT32:
00125       case sensor_msgs::PointField::FLOAT32:
00126         return (4);
00127 
00128       case sensor_msgs::PointField::FLOAT64:
00129         return (8);
00130 
00131       default:
00132         return (0);
00133     }
00134   }
00135 
00140   PCL_EXPORTS void
00141   getFieldsSizes (const std::vector<sensor_msgs::PointField> &fields, 
00142                   std::vector<int> &field_sizes);
00143 
00149   inline int
00150   getFieldType (const int size, char type)
00151   {
00152     type = std::toupper (type, std::locale::classic ());
00153     switch (size)
00154     {
00155       case 1:
00156         if (type == 'I')
00157           return (sensor_msgs::PointField::INT8);
00158         if (type == 'U')
00159           return (sensor_msgs::PointField::UINT8);
00160 
00161       case 2:
00162         if (type == 'I')
00163           return (sensor_msgs::PointField::INT16);
00164         if (type == 'U')
00165           return (sensor_msgs::PointField::UINT16);
00166 
00167       case 4:
00168         if (type == 'I')
00169           return (sensor_msgs::PointField::INT32);
00170         if (type == 'U')
00171           return (sensor_msgs::PointField::UINT32);
00172         if (type == 'F')
00173           return (sensor_msgs::PointField::FLOAT32);
00174 
00175       case 8:
00176         return (sensor_msgs::PointField::FLOAT64);
00177 
00178       default:
00179         return (-1);
00180     }
00181   }
00182 
00187   inline char
00188   getFieldType (const int type)
00189   {
00190     switch (type)
00191     {
00192       case sensor_msgs::PointField::INT8:
00193       case sensor_msgs::PointField::INT16:
00194       case sensor_msgs::PointField::INT32:
00195         return ('I');
00196 
00197       case sensor_msgs::PointField::UINT8:
00198       case sensor_msgs::PointField::UINT16:
00199       case sensor_msgs::PointField::UINT32:
00200         return ('U');
00201 
00202       case sensor_msgs::PointField::FLOAT32:
00203       case sensor_msgs::PointField::FLOAT64:
00204         return ('F');
00205       default:
00206         return ('?');
00207     }
00208   }
00209 
00215   template <typename PointInT, typename PointOutT> void 
00216   copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in, 
00217                   pcl::PointCloud<PointOutT> &cloud_out);
00218 
00226   PCL_EXPORTS bool 
00227   concatenatePointCloud (const sensor_msgs::PointCloud2 &cloud1, 
00228                          const sensor_msgs::PointCloud2 &cloud2, 
00229                          sensor_msgs::PointCloud2 &cloud_out);
00230 
00237   PCL_EXPORTS void 
00238   copyPointCloud (const sensor_msgs::PointCloud2 &cloud_in, 
00239                   const std::vector<int> &indices, 
00240                   sensor_msgs::PointCloud2 &cloud_out);
00241 
00248   template <typename PointT> void 
00249   copyPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00250                   const std::vector<int> &indices, 
00251                   pcl::PointCloud<PointT> &cloud_out);
00258   template <typename PointT> void 
00259   copyPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00260                   const std::vector<int, Eigen::aligned_allocator<int> > &indices, 
00261                   pcl::PointCloud<PointT> &cloud_out);
00262 
00269   template <typename PointInT, typename PointOutT> void 
00270   copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in, 
00271                   const std::vector<int> &indices, 
00272                   pcl::PointCloud<PointOutT> &cloud_out);
00273 
00280   template <typename PointInT, typename PointOutT> void 
00281   copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in, 
00282                   const std::vector<int, Eigen::aligned_allocator<int> > &indices, 
00283                   pcl::PointCloud<PointOutT> &cloud_out);
00284 
00291   template <typename PointT> void 
00292   copyPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00293                   const PointIndices &indices, 
00294                   pcl::PointCloud<PointT> &cloud_out);
00295 
00302   template <typename PointInT, typename PointOutT> void 
00303   copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in, 
00304                   const PointIndices &indices, 
00305                   pcl::PointCloud<PointOutT> &cloud_out);
00306 
00313   template <typename PointT> void 
00314   copyPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00315                   const std::vector<pcl::PointIndices> &indices, 
00316                   pcl::PointCloud<PointT> &cloud_out);
00317 
00324   template <typename PointInT, typename PointOutT> void 
00325   copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in, 
00326                   const std::vector<pcl::PointIndices> &indices, 
00327                   pcl::PointCloud<PointOutT> &cloud_out);
00328 
00340   template <typename PointIn1T, typename PointIn2T, typename PointOutT> void 
00341   concatenateFields (const pcl::PointCloud<PointIn1T> &cloud1_in, 
00342                      const pcl::PointCloud<PointIn2T> &cloud2_in, 
00343                      pcl::PointCloud<PointOutT> &cloud_out);
00344 
00356   PCL_EXPORTS bool
00357   concatenateFields (const sensor_msgs::PointCloud2 &cloud1_in, 
00358                      const sensor_msgs::PointCloud2 &cloud2_in, 
00359                      sensor_msgs::PointCloud2 &cloud_out);
00360 
00366   PCL_EXPORTS bool 
00367   getPointCloudAsEigen (const sensor_msgs::PointCloud2 &in, Eigen::MatrixXf &out);
00368 
00375   PCL_EXPORTS bool 
00376   getEigenAsPointCloud (Eigen::MatrixXf &in, sensor_msgs::PointCloud2 &out);
00377   
00378   namespace io 
00379   {
00384     template <std::size_t N> void 
00385     swapByte (char* bytes);
00386 
00390     template <> inline void 
00391     swapByte<1> (char* bytes) {}
00392 
00393   
00397     template <> inline void 
00398     swapByte<2> (char* bytes) { std::swap (bytes[0], bytes[1]); }
00399   
00403     template <> inline void 
00404     swapByte<4> (char* bytes)
00405     {
00406       std::swap (bytes[0], bytes[3]);
00407       std::swap (bytes[1], bytes[2]);
00408     }
00409   
00413     template <> inline void 
00414     swapByte<8> (char* bytes)
00415     {
00416       std::swap (bytes[0], bytes[7]);
00417       std::swap (bytes[1], bytes[6]);
00418       std::swap (bytes[2], bytes[5]);
00419       std::swap (bytes[3], bytes[4]);
00420     }
00421   
00425     template <typename T> void 
00426     swapByte (T& value)
00427     {
00428       pcl::io::swapByte<sizeof(T)> (reinterpret_cast<char*> (&value));
00429     }
00430   }
00431 }
00432 
00433 #include "pcl/common/impl/io.hpp"
00434 
00435 #endif  //#ifndef PCL_COMMON_IO_H_
00436