|
Point Cloud Library (PCL)
1.4.0
|
|
Point Cloud Library (PCL)
1.4.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2011, Willow Garage, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of Willow Garage, Inc. nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * $Id: ply_io.h 3771 2012-01-01 06:58:14Z rusu $ 00037 * 00038 */ 00039 00040 #ifndef PCL_IO_PLY_IO_H_ 00041 #define PCL_IO_PLY_IO_H_ 00042 00043 #include "pcl/io/file_io.h" 00044 #include "pcl/io/ply.h" 00045 #include <pcl/PolygonMesh.h> 00046 #include <sstream> 00047 00048 namespace pcl 00049 { 00062 class PCL_EXPORTS PLYReader : public FileReader 00063 { 00064 public: 00065 enum 00066 { 00067 PLY_V0 = 0, 00068 PLY_V1 = 1 00069 }; 00070 00088 int 00089 readHeader (const std::string &file_name, sensor_msgs::PointCloud2 &cloud, 00090 Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, 00091 int &ply_version, int &data_type, int &data_idx); 00092 00100 int 00101 read (const std::string &file_name, sensor_msgs::PointCloud2 &cloud, 00102 Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int& ply_version); 00103 00114 inline int 00115 read (const std::string &file_name, sensor_msgs::PointCloud2 &cloud) 00116 { 00117 Eigen::Vector4f origin; 00118 Eigen::Quaternionf orientation; 00119 int ply_version; 00120 return read (file_name, cloud, origin, orientation, ply_version); 00121 } 00122 00127 template<typename PointT> inline int 00128 read (const std::string &file_name, pcl::PointCloud<PointT> &cloud) 00129 { 00130 sensor_msgs::PointCloud2 blob; 00131 int ply_version; 00132 int res = read (file_name, blob, cloud.sensor_origin_, cloud.sensor_orientation_, 00133 ply_version); 00134 00135 // Exit in case of error 00136 if (res < 0) 00137 return (res); 00138 pcl::fromROSMsg (blob, cloud); 00139 return (0); 00140 } 00141 00142 private: 00143 pcl::io::ply::parser parser_; 00144 bool swap_bytes_; 00145 00155 template <typename Type> inline void 00156 copyStringValue (const std::string &string_value, void* data, size_t offset = 0) 00157 { 00158 //char value = (char)atoi (st.at (d + c).c_str ()); 00159 static char* char_ptr; 00160 char_ptr = (char*) data; 00161 Type value; 00162 std::istringstream is(string_value); 00163 is >> value; 00164 memcpy (char_ptr+offset, &value, sizeof (Type)); 00165 } 00166 }; 00167 00172 class PCL_EXPORTS PLYWriter : public FileWriter 00173 { 00174 public: 00175 PLYWriter () : mask_ (0) {}; 00176 ~PLYWriter () {}; 00177 00181 inline std::string 00182 generateHeaderBinary (const sensor_msgs::PointCloud2 &cloud, 00183 const Eigen::Vector4f &origin, 00184 const Eigen::Quaternionf &orientation, 00185 int valid_points, 00186 bool use_camera = true) 00187 { 00188 return (generateHeader (cloud, origin, orientation, true, use_camera, valid_points)); 00189 } 00190 00194 inline std::string 00195 generateHeaderASCII (const sensor_msgs::PointCloud2 &cloud, 00196 const Eigen::Vector4f &origin, 00197 const Eigen::Quaternionf &orientation, 00198 int valid_points, 00199 bool use_camera = true) 00200 { 00201 return (generateHeader (cloud, origin, orientation, false, use_camera, valid_points)); 00202 } 00203 00211 int 00212 writeASCII (const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, 00213 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 00214 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), 00215 int precision = 8, 00216 bool use_camera = true); 00217 00224 int 00225 writeBinary (const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, 00226 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 00227 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity ()); 00228 00237 inline int 00238 write (const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, 00239 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 00240 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), 00241 bool binary = false) 00242 { 00243 if (binary) 00244 return (this->writeBinary (file_name, cloud, origin, orientation)); 00245 else 00246 return (this->writeASCII (file_name, cloud, origin, orientation, 8, true)); 00247 } 00248 00259 inline int 00260 write (const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, 00261 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 00262 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), 00263 bool binary = false, 00264 bool use_camera = true) 00265 { 00266 if (binary) 00267 return (this->writeBinary (file_name, cloud, origin, orientation)); 00268 else 00269 return (this->writeASCII (file_name, cloud, origin, orientation, 8, use_camera)); 00270 } 00271 00280 inline int 00281 write (const std::string &file_name, const sensor_msgs::PointCloud2::ConstPtr &cloud, 00282 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 00283 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), 00284 bool binary = false, 00285 bool use_camera = true) 00286 { 00287 return (write (file_name, *cloud, origin, orientation, binary, use_camera)); 00288 } 00289 00296 template<typename PointT> inline int 00297 write (const std::string &file_name, 00298 const pcl::PointCloud<PointT> &cloud, 00299 bool binary = false, 00300 bool use_camera = true) 00301 { 00302 Eigen::Vector4f origin = cloud.sensor_origin_; 00303 Eigen::Quaternionf orientation = cloud.sensor_orientation_; 00304 00305 sensor_msgs::PointCloud2 blob; 00306 pcl::toROSMsg (cloud, blob); 00307 00308 // Save the data 00309 return (this->write (file_name, blob, origin, orientation, binary, use_camera)); 00310 } 00311 00312 private: 00317 std::string 00318 generateHeader (const sensor_msgs::PointCloud2 &cloud, 00319 const Eigen::Vector4f &origin, 00320 const Eigen::Quaternionf &orientation, 00321 bool binary, 00322 bool use_camera, 00323 int valid_points); 00324 00325 void 00326 writeContentWithCameraASCII (int nr_points, 00327 int point_size, 00328 const sensor_msgs::PointCloud2 &cloud, 00329 const Eigen::Vector4f &origin, 00330 const Eigen::Quaternionf &orientation, 00331 std::ofstream& fs); 00332 00333 void 00334 writeContentWithRangeGridASCII (int nr_points, 00335 int point_size, 00336 const sensor_msgs::PointCloud2 &cloud, 00337 std::ostringstream& fs, 00338 int& nb_valid_points); 00339 00343 void 00344 setMaskFromFieldsList (const std::string& fields_list); 00345 00347 int mask_; 00348 }; 00349 00350 namespace io 00351 { 00361 inline int 00362 loadPLYFile (const std::string &file_name, sensor_msgs::PointCloud2 &cloud) 00363 { 00364 pcl::PLYReader p; 00365 return (p.read (file_name, cloud)); 00366 } 00367 00376 inline int 00377 loadPLYFile (const std::string &file_name, sensor_msgs::PointCloud2 &cloud, 00378 Eigen::Vector4f &origin, Eigen::Quaternionf &orientation) 00379 { 00380 pcl::PLYReader p; 00381 int ply_version; 00382 return (p.read (file_name, cloud, origin, orientation, ply_version)); 00383 } 00384 00390 template<typename PointT> inline int 00391 loadPLYFile (const std::string &file_name, pcl::PointCloud<PointT> &cloud) 00392 { 00393 pcl::PLYReader p; 00394 return (p.read (file_name, cloud)); 00395 } 00396 00405 inline int 00406 savePLYFile (const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, 00407 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 00408 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), 00409 bool binary_mode = false, bool use_camera = true) 00410 { 00411 PLYWriter w; 00412 return (w.write (file_name, cloud, origin, orientation, binary_mode, use_camera)); 00413 } 00414 00422 template<typename PointT> inline int 00423 savePLYFile (const std::string &file_name, const pcl::PointCloud<PointT> &cloud, bool binary_mode = false) 00424 { 00425 PLYWriter w; 00426 return (w.write<PointT> (file_name, cloud, binary_mode)); 00427 } 00428 00435 template<typename PointT> inline int 00436 savePLYFileASCII (const std::string &file_name, const pcl::PointCloud<PointT> &cloud) 00437 { 00438 PLYWriter w; 00439 return (w.write<PointT> (file_name, cloud, false)); 00440 } 00441 00447 template<typename PointT> inline int 00448 savePLYFileBinary (const std::string &file_name, const pcl::PointCloud<PointT> &cloud) 00449 { 00450 PLYWriter w; 00451 return (w.write<PointT> (file_name, cloud, true)); 00452 } 00453 00461 template<typename PointT> int 00462 savePLYFile (const std::string &file_name, const pcl::PointCloud<PointT> &cloud, 00463 const std::vector<int> &indices, bool binary_mode = false) 00464 { 00465 // Copy indices to a new point cloud 00466 pcl::PointCloud<PointT> cloud_out; 00467 copyPointCloud (cloud, indices, cloud_out); 00468 // Save the data 00469 PLYWriter w; 00470 return (w.write<PointT> (file_name, cloud_out, binary_mode)); 00471 } 00472 00479 PCL_EXPORTS int 00480 savePLYFile (const std::string &file_name, const pcl::PolygonMesh &mesh, unsigned precision = 5); 00481 00482 }; 00483 } 00484 00485 #endif //#ifndef PCL_IO_PLY_IO_H_
1.7.6.1
