Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/io/include/pcl/io/file_io.h
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 the copyright holder(s) 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 827 2011-05-04 02:00:04Z nizar $
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 <pcl/io/boost.h>
00044 #include <cmath>
00045 #include <sstream>
00046 
00047 namespace pcl
00048 {
00049   /** \brief Point Cloud Data (FILE) file format reader interface.
00050     * Any (FILE) format file reader should implement its virtual methodes.
00051     * \author Nizar Sallem
00052     * \ingroup io
00053     */
00054   class PCL_EXPORTS FileReader
00055   {
00056     public:
00057       /** \brief empty constructor */ 
00058       FileReader() {}
00059       /** \brief empty destructor */ 
00060       virtual ~FileReader() {}
00061       /** \brief Read a point cloud data header from a FILE file. 
00062         *
00063         * Load only the meta information (number of points, their types, etc),
00064         * and not the points themselves, from a given FILE file. Useful for fast
00065         * evaluation of the underlying data structure.
00066         *
00067         * Returns:
00068         *  * < 0 (-1) on error
00069         *  * > 0 on success
00070         * \param[in] file_name the name of the file to load
00071         * \param[out] cloud the resultant point cloud dataset (only the header will be filled)
00072         * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present)
00073         * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present)
00074         * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7)
00075         * \param[out] data_type the type of data (binary data=1, ascii=0, etc)
00076         * \param[out] data_idx the offset of cloud data within the file
00077         * \param[in] offset the offset in the file where to expect the true header to begin.
00078         * One usage example for setting the offset parameter is for reading
00079         * data from a TAR "archive containing multiple files: TAR files always
00080         * add a 512 byte header in front of the actual file, so set the offset
00081         * to the next byte after the header (e.g., 513).
00082         */
00083       virtual int 
00084       readHeader (const std::string &file_name, pcl::PCLPointCloud2 &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 
00088       /** \brief Read a point cloud data from a FILE file and store it into a pcl/PCLPointCloud2.
00089         * \param[in] file_name the name of the file containing the actual PointCloud data
00090         * \param[out] cloud the resultant PointCloud message read from disk
00091         * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present)
00092         * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present)
00093         * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7)
00094         * \param[in] offset the offset in the file where to expect the true header to begin.
00095         * One usage example for setting the offset parameter is for reading
00096         * data from a TAR "archive containing multiple files: TAR files always
00097         * add a 512 byte header in front of the actual file, so set the offset
00098         * to the next byte after the header (e.g., 513).
00099         */
00100       virtual int 
00101       read (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
00102             Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &file_version, 
00103             const int offset = 0) = 0;
00104 
00105       /** \brief Read a point cloud data from a FILE file (FILE_V6 only!) and store it into a pcl/PCLPointCloud2.
00106         * 
00107         * \note This function is provided for backwards compatibility only and
00108         * it can only read FILE_V6 files correctly, as pcl::PCLPointCloud2
00109         * does not contain a sensor origin/orientation. Reading any file 
00110         * > FILE_V6 will generate a warning. 
00111         *
00112         * \param[in] file_name the name of the file containing the actual PointCloud data
00113         * \param[out] cloud the resultant PointCloud message read from disk
00114         *
00115         * \param[in] offset the offset in the file where to expect the true header to begin.
00116         * One usage example for setting the offset parameter is for reading
00117         * data from a TAR "archive containing multiple files: TAR files always
00118         * add a 512 byte header in front of the actual file, so set the offset
00119         * to the next byte after the header (e.g., 513).
00120         */
00121       int 
00122       read (const std::string &file_name, pcl::PCLPointCloud2 &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 
00130       /** \brief Read a point cloud data from any FILE file, and convert it to the given template format.
00131         * \param[in] file_name the name of the file containing the actual PointCloud data
00132         * \param[out] cloud the resultant PointCloud message read from disk
00133         * \param[in] offset the offset in the file where to expect the true header to begin.
00134         * One usage example for setting the offset parameter is for reading
00135         * data from a TAR "archive containing multiple files: TAR files always
00136         * add a 512 byte header in front of the actual file, so set the offset
00137         * to the next byte after the header (e.g., 513).
00138         */
00139       template<typename PointT> inline int
00140       read (const std::string &file_name, pcl::PointCloud<PointT> &cloud, const int offset  =0)
00141       {
00142         pcl::PCLPointCloud2 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::fromPCLPointCloud2 (blob, cloud);
00151         return (0);
00152       }
00153   };
00154 
00155   /** \brief Point Cloud Data (FILE) file format writer.
00156     * Any (FILE) format file reader should implement its virtual methodes
00157     * \author Nizar Sallem
00158     * \ingroup io
00159     */
00160   class PCL_EXPORTS FileWriter
00161   {
00162     public:
00163       /** \brief Empty constructor */ 
00164       FileWriter () {}
00165 
00166       /** \brief Empty destructor */ 
00167       virtual ~FileWriter () {}
00168 
00169       /** \brief Save point cloud data to a FILE file containing n-D points
00170         * \param[in] file_name the output file name
00171         * \param[in] cloud the point cloud data message
00172         * \param[in] origin the sensor acquisition origin
00173         * \param[in] orientation the sensor acquisition orientation
00174         * \param[in] binary set to true if the file is to be written in a binary
00175         * FILE format, false (default) for ASCII
00176         */
00177       virtual int
00178       write (const std::string &file_name, const pcl::PCLPointCloud2 &cloud,
00179              const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 
00180              const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
00181              const bool binary = false) = 0;
00182 
00183       /** \brief Save point cloud data to a FILE file containing n-D points
00184         * \param[in] file_name the output file name
00185         * \param[in] cloud the point cloud data message (boost shared pointer)
00186         * \param[in] binary set to true if the file is to be written in a binary
00187         * FILE format, false (default) for ASCII
00188         * \param[in] origin the sensor acquisition origin
00189         * \param[in] orientation the sensor acquisition orientation
00190         */
00191       inline int
00192       write (const std::string &file_name, const pcl::PCLPointCloud2::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 
00200       /** \brief Save point cloud data to a FILE file containing n-D points
00201         * \param[in] file_name the output file name
00202         * \param[in] cloud the pcl::PointCloud data
00203         * \param[in] binary set to true if the file is to be written in a binary
00204         * FILE format, false (default) for ASCII
00205         */
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         pcl::PCLPointCloud2 blob;
00215         pcl::toPCLPointCloud2 (cloud, blob);
00216 
00217         // Save the data
00218         return (write (file_name, blob, origin, orientation, binary));
00219       }
00220   };
00221 
00222   /** \brief insers a value of type Type (uchar, char, uint, int, float, double, ...) into a stringstream.
00223     *
00224     * If the value is NaN, it inserst "nan".
00225     *
00226     * \param[in] cloud the cloud to copy from
00227     * \param[in] point_index the index of the point
00228     * \param[in] point_size the size of the point in the cloud
00229     * \param[in] field_idx the index of the dimension/field
00230     * \param[in] fields_count the current fields count
00231     * \param[out] stream the ostringstream to copy into
00232     */
00233   template <typename Type> inline void
00234   copyValueString (const pcl::PCLPointCloud2 &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 pcl::PCLPointCloud2 &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 pcl::PCLPointCloud2 &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 
00281   /** \brief Check whether a given value of type Type (uchar, char, uint, int, float, double, ...) is finite or not
00282     *
00283     * \param[in] cloud the cloud that contains the data
00284     * \param[in] point_index the index of the point
00285     * \param[in] point_size the size of the point in the cloud
00286     * \param[in] field_idx the index of the dimension/field
00287     * \param[in] fields_count the current fields count
00288     *
00289     * \return true if the value is finite, false otherwise
00290     */
00291   template <typename Type> inline bool
00292   isValueFinite (const pcl::PCLPointCloud2 &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 
00305   /** \brief Copy one single value of type T (uchar, char, uint, int, float, double, ...) from a string
00306     * 
00307     * Uses aoti/atof to do the conversion.
00308     * Checks if the st is "nan" and converts it accordingly.
00309     *
00310     * \param[in] st the string containing the value to convert and copy
00311     * \param[out] cloud the cloud to copy it to
00312     * \param[in] point_index the index of the point
00313     * \param[in] field_idx the index of the dimension/field
00314     * \param[in] fields_count the current fields count
00315     */
00316   template <typename Type> inline void
00317   copyStringValue (const std::string &st, pcl::PCLPointCloud2 &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       if (!(is >> value))
00331         value = static_cast<Type> (atof (st.c_str ()));
00332     }
00333 
00334     memcpy (&cloud.data[point_index * cloud.point_step + 
00335                         cloud.fields[field_idx].offset + 
00336                         fields_count * sizeof (Type)], reinterpret_cast<char*> (&value), sizeof (Type));
00337   }
00338 
00339   template <> inline void
00340   copyStringValue<int8_t> (const std::string &st, pcl::PCLPointCloud2 &cloud,
00341                            unsigned int point_index, unsigned int field_idx, unsigned int fields_count)
00342   {
00343     int8_t value;
00344     if (st == "nan")
00345     {
00346       value = static_cast<int8_t> (std::numeric_limits<int>::quiet_NaN ());
00347       cloud.is_dense = false;
00348     }
00349     else
00350     {
00351       int val;
00352       std::istringstream is (st);
00353       is.imbue (std::locale::classic ());
00354       //is >> val;  -- unfortunately this fails on older GCC versions and CLANG on MacOS
00355       if (!(is >> val))
00356         val = static_cast<int> (atof (st.c_str ()));
00357       value = static_cast<int8_t> (val);
00358     }
00359 
00360     memcpy (&cloud.data[point_index * cloud.point_step + 
00361                         cloud.fields[field_idx].offset + 
00362                         fields_count * sizeof (int8_t)], reinterpret_cast<char*> (&value), sizeof (int8_t));
00363   }
00364 
00365   template <> inline void
00366   copyStringValue<uint8_t> (const std::string &st, pcl::PCLPointCloud2 &cloud,
00367                            unsigned int point_index, unsigned int field_idx, unsigned int fields_count)
00368   {
00369     uint8_t value;
00370     if (st == "nan")
00371     {
00372       value = static_cast<uint8_t> (std::numeric_limits<int>::quiet_NaN ());
00373       cloud.is_dense = false;
00374     }
00375     else
00376     {
00377       int val;
00378       std::istringstream is (st);
00379       is.imbue (std::locale::classic ());
00380       //is >> val;  -- unfortunately this fails on older GCC versions and CLANG on MacOS
00381       if (!(is >> val))
00382         val = static_cast<int> (atof (st.c_str ()));
00383       value = static_cast<uint8_t> (val);
00384     }
00385 
00386     memcpy (&cloud.data[point_index * cloud.point_step + 
00387                         cloud.fields[field_idx].offset + 
00388                         fields_count * sizeof (uint8_t)], reinterpret_cast<char*> (&value), sizeof (uint8_t));
00389   }
00390 }
00391 
00392 #endif  //#ifndef PCL_IO_FILE_IO_H_