file_io.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, 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: file_io.h 4932 2012-03-07 07:10:07Z rusu $
00035  *
00036  */
00037 
00038 #ifndef PCL_IO_FILE_IO_H_
00039 #define PCL_IO_FILE_IO_H_
00040 
00041 #include <pcl/pcl_macros.h>
00042 #include <pcl/common/io.h>
00043 #include <boost/numeric/conversion/cast.hpp>
00044 #include <cmath>
00045 #include <sstream>
00046 
00047 namespace pcl
00048 {
00054   class PCL_EXPORTS FileReader
00055   {
00056     public:
00058       FileReader() {}
00060       virtual ~FileReader() {}
00083       virtual int 
00084       readHeader (const std::string &file_name, sensor_msgs::PointCloud2 &cloud, 
00085                   Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, 
00086                   int &file_version, int &data_type, unsigned int &data_idx, const int offset = 0) = 0;
00087 
00100       virtual int 
00101       read (const std::string &file_name, sensor_msgs::PointCloud2 &cloud, 
00102             Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &file_version, 
00103             const int offset = 0) = 0;
00104 
00121       int 
00122       read (const std::string &file_name, sensor_msgs::PointCloud2 &cloud, const int offset = 0)
00123       {
00124         Eigen::Vector4f origin;
00125         Eigen::Quaternionf orientation;
00126         int file_version;
00127         return (read (file_name, cloud, origin, orientation, file_version, offset));
00128       }
00129 
00139       template<typename PointT> inline int
00140       read (const std::string &file_name, pcl::PointCloud<PointT> &cloud, const int offset  =0)
00141       {
00142         sensor_msgs::PointCloud2 blob;
00143         int file_version;
00144         int res = read (file_name, blob, cloud.sensor_origin_, cloud.sensor_orientation_, 
00145                         file_version, offset);
00146 
00147         // Exit in case of error
00148         if (res < 0)
00149           return res;
00150         pcl::fromROSMsg (blob, cloud);
00151         return (0);
00152       }
00153   };
00154 
00160   class PCL_EXPORTS FileWriter
00161   {
00162     public:
00164       FileWriter () {}
00165 
00167       virtual ~FileWriter () {}
00168 
00177       virtual int
00178       write (const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, 
00179              const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 
00180              const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
00181              const bool binary = false) = 0;
00182 
00191       inline int
00192       write (const std::string &file_name, const sensor_msgs::PointCloud2::ConstPtr &cloud, 
00193              const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 
00194              const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
00195              const bool binary = false)
00196       {
00197         return (write (file_name, *cloud, origin, orientation, binary));
00198       }
00199 
00206       template<typename PointT> inline int
00207       write (const std::string &file_name, 
00208              const pcl::PointCloud<PointT> &cloud, 
00209              const bool binary = false)
00210       {
00211         Eigen::Vector4f origin = cloud.sensor_origin_;
00212         Eigen::Quaternionf orientation = cloud.sensor_orientation_;
00213 
00214         sensor_msgs::PointCloud2 blob;
00215         pcl::toROSMsg (cloud, blob);
00216 
00217         // Save the data
00218         return (write (file_name, blob, origin, orientation, binary));
00219       }
00220   };
00221 
00233   template <typename Type> inline void
00234   copyValueString (const sensor_msgs::PointCloud2 &cloud, 
00235                    const unsigned int point_index, 
00236                    const int point_size, 
00237                    const unsigned int field_idx, 
00238                    const unsigned int fields_count, 
00239                    std::ostream &stream)
00240   {
00241     Type value;
00242     memcpy (&value, &cloud.data[point_index * point_size + cloud.fields[field_idx].offset + fields_count * sizeof (Type)], sizeof (Type));
00243     if (pcl_isnan (value))
00244       stream << "nan";
00245     else
00246       stream << boost::numeric_cast<Type>(value);
00247   }
00248   template <> inline void
00249   copyValueString<int8_t> (const sensor_msgs::PointCloud2 &cloud, 
00250                            const unsigned int point_index, 
00251                            const int point_size, 
00252                            const unsigned int field_idx, 
00253                            const unsigned int fields_count, 
00254                            std::ostream &stream)
00255   {
00256     int8_t value;
00257     memcpy (&value, &cloud.data[point_index * point_size + cloud.fields[field_idx].offset + fields_count * sizeof (int8_t)], sizeof (int8_t));
00258     if (pcl_isnan (value))
00259       stream << "nan";
00260     else
00261       // Numeric cast doesn't give us what we want for int8_t
00262       stream << boost::numeric_cast<int>(value);
00263   }
00264   template <> inline void
00265   copyValueString<uint8_t> (const sensor_msgs::PointCloud2 &cloud, 
00266                             const unsigned int point_index, 
00267                             const int point_size, 
00268                             const unsigned int field_idx, 
00269                             const unsigned int fields_count, 
00270                             std::ostream &stream)
00271   {
00272     uint8_t value;
00273     memcpy (&value, &cloud.data[point_index * point_size + cloud.fields[field_idx].offset + fields_count * sizeof (uint8_t)], sizeof (uint8_t));
00274     if (pcl_isnan (value))
00275       stream << "nan";
00276     else
00277       // Numeric cast doesn't give us what we want for uint8_t
00278       stream << boost::numeric_cast<int>(value);
00279   }
00280 
00291   template <typename Type> inline bool
00292   isValueFinite (const sensor_msgs::PointCloud2 &cloud, 
00293                  const unsigned int point_index, 
00294                  const int point_size, 
00295                  const unsigned int field_idx, 
00296                  const unsigned int fields_count)
00297   {
00298     Type value;
00299     memcpy (&value, &cloud.data[point_index * point_size + cloud.fields[field_idx].offset + fields_count * sizeof (Type)], sizeof (Type));
00300     if (!pcl_isfinite (value))
00301       return (false);
00302     return (true);
00303   }
00304 
00316   template <typename Type> inline void
00317   copyStringValue (const std::string &st, sensor_msgs::PointCloud2 &cloud,
00318                    unsigned int point_index, unsigned int field_idx, unsigned int fields_count)
00319   {
00320     Type value;
00321     if (st == "nan")
00322     {
00323       value = std::numeric_limits<Type>::quiet_NaN ();
00324       cloud.is_dense = false;
00325     }
00326     else
00327     {
00328       std::istringstream is (st);
00329       is.imbue (std::locale::classic ());
00330       is >> value;
00331     }
00332 
00333     memcpy (&cloud.data[point_index * cloud.point_step + 
00334                         cloud.fields[field_idx].offset + 
00335                         fields_count * sizeof (Type)], reinterpret_cast<char*> (&value), sizeof (Type));
00336   }
00337 
00338   template <> inline void
00339   copyStringValue<int8_t> (const std::string &st, sensor_msgs::PointCloud2 &cloud,
00340                            unsigned int point_index, unsigned int field_idx, unsigned int fields_count)
00341   {
00342     int8_t value;
00343     if (st == "nan")
00344     {
00345       value = static_cast<int8_t> (std::numeric_limits<int>::quiet_NaN ());
00346       cloud.is_dense = false;
00347     }
00348     else
00349     {
00350       int val;
00351       std::istringstream is (st);
00352       is.imbue (std::locale::classic ());
00353       is >> val;
00354       value = static_cast<int8_t> (val);
00355     }
00356 
00357     memcpy (&cloud.data[point_index * cloud.point_step + 
00358                         cloud.fields[field_idx].offset + 
00359                         fields_count * sizeof (int8_t)], reinterpret_cast<char*> (&value), sizeof (int8_t));
00360   }
00361 
00362   template <> inline void
00363   copyStringValue<uint8_t> (const std::string &st, sensor_msgs::PointCloud2 &cloud,
00364                            unsigned int point_index, unsigned int field_idx, unsigned int fields_count)
00365   {
00366     uint8_t value;
00367     if (st == "nan")
00368     {
00369       value = static_cast<uint8_t> (std::numeric_limits<int>::quiet_NaN ());
00370       cloud.is_dense = false;
00371     }
00372     else
00373     {
00374       int val;
00375       std::istringstream is (st);
00376       is.imbue (std::locale::classic ());
00377       is >> val;
00378       value = static_cast<uint8_t> (val);
00379     }
00380 
00381     memcpy (&cloud.data[point_index * cloud.point_step + 
00382                         cloud.fields[field_idx].offset + 
00383                         fields_count * sizeof (uint8_t)], reinterpret_cast<char*> (&value), sizeof (uint8_t));
00384   }
00385 }
00386 
00387 #endif  //#ifndef PCL_IO_FILE_IO_H_


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