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