Point Cloud Library (PCL)  1.3.1
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 3008 2011-10-31 23:50:29Z rusu $
00035  *
00036  */
00037 
00038 #ifndef PCL_IO_FILE_IO_H_
00039 #define PCL_IO_FILE_IO_H_
00040 
00041 #include "pcl/win32_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() {}
00078       virtual int 
00079       readHeader (const std::string &file_name, sensor_msgs::PointCloud2 &cloud, 
00080                   Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &file_version,
00081                   int &data_type, int &data_idx) = 0;
00082 
00090       virtual int 
00091       read (const std::string &file_name, sensor_msgs::PointCloud2 &cloud, 
00092             Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &file_version) = 0;
00093 
00104       int 
00105       read (const std::string &file_name, sensor_msgs::PointCloud2 &cloud)
00106       {
00107         Eigen::Vector4f origin;
00108         Eigen::Quaternionf orientation;
00109         int file_version;
00110         return read(file_name, cloud, origin, orientation, file_version);
00111       }
00112 
00117       template<typename PointT> inline int
00118       read (const std::string &file_name, pcl::PointCloud<PointT> &cloud)
00119       {
00120         sensor_msgs::PointCloud2 blob;
00121         int file_version;
00122         int res = read (file_name, blob, cloud.sensor_origin_, cloud.sensor_orientation_, 
00123                         file_version);
00124 
00125         // Exit in case of error
00126         if (res < 0)
00127           return res;
00128         pcl::fromROSMsg (blob, cloud);
00129         return 0;
00130       }
00131 
00143       template <typename Type> inline void
00144       copyStringValue (const std::string &st, sensor_msgs::PointCloud2 &cloud,
00145                        unsigned int point_index, unsigned int field_idx, unsigned int fields_count)
00146       {
00147         Type value;
00148         if (st == "nan")
00149         {
00150           value = std::numeric_limits<Type>::quiet_NaN ();
00151           cloud.is_dense = false;
00152         }
00153         else
00154         {
00155           std::istringstream is(st);
00156           is.imbue (std::locale::classic ());
00157           is >> value;
00158         }
00159 
00160         memcpy (&cloud.data[point_index * cloud.point_step + 
00161                             cloud.fields[field_idx].offset + 
00162                             fields_count * sizeof (Type)], (char*)&value, sizeof (Type));
00163       }
00164 
00165   };
00166 
00172   class PCL_EXPORTS FileWriter
00173   {
00174     public:
00176       FileWriter() {}
00178       virtual ~FileWriter() {}
00187       virtual int
00188       write (const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, 
00189              const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 
00190              const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
00191              const bool binary = false) = 0;
00192 
00201       inline int
00202       write (const std::string &file_name, const sensor_msgs::PointCloud2::ConstPtr &cloud, 
00203              const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 
00204              const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
00205              const bool binary = false)
00206       {
00207         return (write (file_name, *cloud, origin, orientation, binary));
00208       }
00209 
00216       template<typename PointT> inline int
00217       write (const std::string &file_name, 
00218              const pcl::PointCloud<PointT> &cloud, 
00219              const bool binary = false)
00220       {
00221         Eigen::Vector4f origin = cloud.sensor_origin_;
00222         Eigen::Quaternionf orientation = cloud.sensor_orientation_;
00223 
00224         sensor_msgs::PointCloud2 blob;
00225         pcl::toROSMsg (cloud, blob);
00226 
00227         // Save the data
00228         return (write (file_name, blob, origin, orientation, binary));
00229       }
00230   };
00231 
00243   template <typename Type> inline void
00244   copyValueString (const sensor_msgs::PointCloud2 &cloud, 
00245                    const unsigned int point_index, 
00246                    const int point_size, 
00247                    const unsigned int field_idx, 
00248                    const unsigned int fields_count, 
00249                    std::ostream &stream)
00250   {
00251     Type value;
00252     memcpy (&value, &cloud.data[point_index * point_size + cloud.fields[field_idx].offset + fields_count * sizeof (Type)], sizeof (Type));
00253     if (pcl_isnan (value))
00254       stream << "nan";
00255     else
00256       stream << boost::numeric_cast<Type>(value);
00257   }
00258   template <> inline void
00259   copyValueString<int8_t> (const sensor_msgs::PointCloud2 &cloud, 
00260                            const unsigned int point_index, 
00261                            const int point_size, 
00262                            const unsigned int field_idx, 
00263                            const unsigned int fields_count, 
00264                            std::ostream &stream)
00265   {
00266     int8_t value;
00267     memcpy (&value, &cloud.data[point_index * point_size + cloud.fields[field_idx].offset + fields_count * sizeof (int8_t)], sizeof (int8_t));
00268     if (pcl_isnan (value))
00269       stream << "nan";
00270     else
00271       // Numeric cast doesn't give us what we want for int8_t
00272       stream << boost::numeric_cast<int>(value);
00273   }
00274   template <> inline void
00275   copyValueString<uint8_t> (const sensor_msgs::PointCloud2 &cloud, 
00276                             const unsigned int point_index, 
00277                             const int point_size, 
00278                             const unsigned int field_idx, 
00279                             const unsigned int fields_count, 
00280                             std::ostream &stream)
00281   {
00282     uint8_t value;
00283     memcpy (&value, &cloud.data[point_index * point_size + cloud.fields[field_idx].offset + fields_count * sizeof (uint8_t)], sizeof (uint8_t));
00284     if (pcl_isnan (value))
00285       stream << "nan";
00286     else
00287       // Numeric cast doesn't give us what we want for uint8_t
00288       stream << boost::numeric_cast<int>(value);
00289   }
00290 }
00291 
00292 #endif  //#ifndef PCL_IO_FILE_IO_H_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines