Point Cloud Library (PCL)
1.7.0
|
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_