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: ply_io.h 3008 2011-10-31 23:50:29Z rusu $ 00035 * 00036 */ 00037 00038 #ifndef PCL_IO_PLY_IO_H_ 00039 #define PCL_IO_PLY_IO_H_ 00040 00041 #include "pcl/io/file_io.h" 00042 #include "pcl/io/ply.h" 00043 #include <pcl/PolygonMesh.h> 00044 #include <sstream> 00045 00046 namespace pcl 00047 { 00052 class PCL_EXPORTS PLYReader : public FileReader 00053 { 00054 public: 00055 enum 00056 { 00057 PLY_V0 = 0, 00058 PLY_V1 = 1 00059 }; 00083 int readHeader (const std::string &file_name, sensor_msgs::PointCloud2 &cloud, 00084 Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, 00085 int &ply_version, int &data_type, int &data_idx); 00086 00091 int read (const std::string &file_name, sensor_msgs::PointCloud2 &cloud, 00092 Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int& ply_version); 00093 00104 inline int read (const std::string &file_name, sensor_msgs::PointCloud2 &cloud) 00105 { 00106 Eigen::Vector4f origin; 00107 Eigen::Quaternionf orientation; 00108 int ply_version; 00109 return read (file_name, cloud, origin, orientation, ply_version); 00110 } 00111 00116 template<typename PointT> inline int 00117 read (const std::string &file_name, pcl::PointCloud<PointT> &cloud) 00118 { 00119 sensor_msgs::PointCloud2 blob; 00120 int ply_version; 00121 int res = read (file_name, blob, cloud.sensor_origin_, cloud.sensor_orientation_, 00122 ply_version); 00123 00124 // Exit in case of error 00125 if (res < 0) 00126 return res; 00127 pcl::fromROSMsg (blob, cloud); 00128 return 0; 00129 } 00130 00131 private: 00132 pcl::io::ply::parser parser_; 00133 bool swap_bytes_; 00145 template <typename Type> inline void 00146 copyStringValue (const std::string &string_value, void* data, size_t offset = 0) 00147 { 00148 //char value = (char)atoi (st.at (d + c).c_str ()); 00149 static char* char_ptr; 00150 char_ptr = (char*) data; 00151 Type value; 00152 std::istringstream is(string_value); 00153 is >> value; 00154 memcpy (char_ptr+offset, &value, sizeof (Type)); 00155 } 00156 }; 00157 00162 class PCL_EXPORTS PLYWriter : public FileWriter 00163 { 00164 public: 00165 PLYWriter() : mask_(0) {}; 00166 ~PLYWriter() {}; 00170 inline std::string 00171 generateHeaderBinary (const sensor_msgs::PointCloud2 &cloud) 00172 { 00173 return generateHeader (cloud, true); 00174 } 00178 inline std::string 00179 generateHeaderASCII (const sensor_msgs::PointCloud2 &cloud) 00180 00181 { 00182 return generateHeader (cloud, false); 00183 } 00189 int writeASCII (const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, 00190 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 00191 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), 00192 int precision = 8); 00193 00198 int writeBinary (const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, 00199 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 00200 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity ()); 00201 00210 inline int 00211 write (const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, 00212 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 00213 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), 00214 bool binary = false) 00215 { 00216 if (binary) 00217 return (this->writeBinary (file_name, cloud, origin, orientation)); 00218 else 00219 return (this->writeASCII (file_name, cloud, origin, orientation, 8)); 00220 } 00221 00228 inline int 00229 write (const std::string &file_name, const sensor_msgs::PointCloud2::ConstPtr &cloud, 00230 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 00231 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), 00232 bool binary = false) 00233 { 00234 return (write (file_name, *cloud, origin, orientation, binary)); 00235 } 00236 00243 template<typename PointT> inline int 00244 write (const std::string &file_name, 00245 const pcl::PointCloud<PointT> &cloud, 00246 bool binary = false) 00247 { 00248 Eigen::Vector4f origin = cloud.sensor_origin_; 00249 Eigen::Quaternionf orientation = cloud.sensor_orientation_; 00250 00251 sensor_msgs::PointCloud2 blob; 00252 pcl::toROSMsg (cloud, blob); 00253 00254 // Save the data 00255 return (this->write (file_name, blob, origin, orientation, binary)); 00256 } 00257 00258 private: 00259 std::string 00260 generateHeader (const sensor_msgs::PointCloud2 &cloud, bool binary); 00261 void 00262 setMaskFromFieldsList(const std::string& fields_list); 00263 int mask_; 00264 }; 00265 00266 namespace io 00267 { 00277 inline int 00278 loadPLYFile (const std::string &file_name, sensor_msgs::PointCloud2 &cloud) 00279 { 00280 pcl::PLYReader p; 00281 return (p.read (file_name, cloud)); 00282 } 00283 00292 inline int 00293 loadPLYFile (const std::string &file_name, sensor_msgs::PointCloud2 &cloud, 00294 Eigen::Vector4f &origin, Eigen::Quaternionf &orientation) 00295 { 00296 pcl::PLYReader p; 00297 int ply_version; 00298 return (p.read (file_name, cloud, origin, orientation, ply_version)); 00299 } 00300 00306 template<typename PointT> inline int 00307 loadPLYFile (const std::string &file_name, pcl::PointCloud<PointT> &cloud) 00308 { 00309 pcl::PLYReader p; 00310 return (p.read (file_name, cloud)); 00311 } 00312 00319 inline int 00320 savePLYFile (const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, 00321 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 00322 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), 00323 bool binary_mode = false) 00324 { 00325 PLYWriter w; 00326 return (w.write (file_name, cloud, origin, orientation, binary_mode)); 00327 } 00328 00336 template<typename PointT> inline int 00337 savePLYFile (const std::string &file_name, const pcl::PointCloud<PointT> &cloud, bool binary_mode = false) 00338 { 00339 PLYWriter w; 00340 return (w.write<PointT> (file_name, cloud, binary_mode)); 00341 } 00342 00350 template<typename PointT> inline int 00351 savePLYFileASCII (const std::string &file_name, const pcl::PointCloud<PointT> &cloud) 00352 { 00353 PLYWriter w; 00354 return (w.write<PointT> (file_name, cloud, false)); 00355 } 00356 00362 template<typename PointT> inline int 00363 savePLYFileBinary (const std::string &file_name, const pcl::PointCloud<PointT> &cloud) 00364 { 00365 PLYWriter w; 00366 return (w.write<PointT> (file_name, cloud, true)); 00367 } 00368 00369 00380 template<typename PointT> int 00381 savePLYFile (const std::string &file_name, const pcl::PointCloud<PointT> &cloud, 00382 const std::vector<int> &indices, bool binary_mode = false) 00383 { 00384 // Copy indices to a new point cloud 00385 pcl::PointCloud<PointT> cloud_out; 00386 copyPointCloud (cloud, indices, cloud_out); 00387 // Save the data 00388 PLYWriter w; 00389 return (w.write<PointT> (file_name, cloud_out, binary_mode)); 00390 } 00391 00398 PCL_EXPORTS int 00399 savePLYFile (const std::string &file_name, const pcl::PolygonMesh &mesh, unsigned precision = 5); 00400 00401 }; 00402 } 00403 00404 #endif //#ifndef PCL_IO_PLY_IO_H_