Point Cloud Library (PCL)
1.3.1
|
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: pcd_io.h 2697 2011-10-11 06:14:08Z rusu $ 00037 * 00038 */ 00039 00040 #ifndef PCL_IO_PCD_IO_H_ 00041 #define PCL_IO_PCD_IO_H_ 00042 00043 #include "pcl/io/file_io.h" 00044 00045 namespace pcl 00046 { 00051 class PCL_EXPORTS PCDReader : public FileReader 00052 { 00053 public: 00055 PCDReader() : FileReader() {} 00057 ~PCDReader() {} 00082 enum 00083 { 00084 PCD_V6 = 0, 00085 PCD_V7 = 1 00086 }; 00087 00105 int 00106 readHeader (const std::string &file_name, sensor_msgs::PointCloud2 &cloud, 00107 Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &pcd_version, 00108 int &data_type, int &data_idx); 00109 00117 int 00118 read (const std::string &file_name, sensor_msgs::PointCloud2 &cloud, 00119 Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &pcd_version); 00120 00131 int 00132 read (const std::string &file_name, sensor_msgs::PointCloud2 &cloud); 00133 00138 template<typename PointT> inline int 00139 read (const std::string &file_name, pcl::PointCloud<PointT> &cloud) 00140 { 00141 sensor_msgs::PointCloud2 blob; 00142 int pcd_version; 00143 int res = read (file_name, blob, cloud.sensor_origin_, cloud.sensor_orientation_, 00144 pcd_version); 00145 00146 // Exit in case of error 00147 if (res < 0) 00148 return res; 00149 pcl::fromROSMsg (blob, cloud); 00150 return 0; 00151 } 00152 }; 00153 00158 class PCL_EXPORTS PCDWriter : public FileWriter 00159 { 00160 public: 00161 PCDWriter() : FileWriter(), map_synchronization_(false) {} 00162 ~PCDWriter() {} 00163 00172 void 00173 setMapSynchronization (bool sync) 00174 { 00175 map_synchronization_ = sync; 00176 } 00177 00183 std::string 00184 generateHeaderBinary (const sensor_msgs::PointCloud2 &cloud, 00185 const Eigen::Vector4f &origin, 00186 const Eigen::Quaternionf &orientation); 00187 00193 std::string 00194 generateHeaderBinaryCompressed (const sensor_msgs::PointCloud2 &cloud, 00195 const Eigen::Vector4f &origin, 00196 const Eigen::Quaternionf &orientation); 00197 00203 std::string 00204 generateHeaderASCII (const sensor_msgs::PointCloud2 &cloud, 00205 const Eigen::Vector4f &origin, 00206 const Eigen::Quaternionf &orientation); 00207 00213 template <typename PointT> static std::string 00214 generateHeader (const pcl::PointCloud<PointT> &cloud, 00215 const int nr_points = std::numeric_limits<int>::max ()); 00216 00233 int 00234 writeASCII (const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, 00235 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 00236 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), 00237 const int precision = 8); 00238 00245 int 00246 writeBinary (const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, 00247 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 00248 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity ()); 00249 00256 int 00257 writeBinaryCompressed (const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, 00258 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 00259 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity ()); 00260 00278 inline int 00279 write (const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, 00280 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 00281 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), 00282 const bool binary = false) 00283 { 00284 if (binary) 00285 return (writeBinary (file_name, cloud, origin, orientation)); 00286 else 00287 return (writeASCII (file_name, cloud, origin, orientation, 8)); 00288 } 00289 00305 inline int 00306 write (const std::string &file_name, const sensor_msgs::PointCloud2::ConstPtr &cloud, 00307 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 00308 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), 00309 const bool binary = false) 00310 { 00311 return (write (file_name, *cloud, origin, orientation, binary)); 00312 } 00313 00318 template <typename PointT> int 00319 writeBinary (const std::string &file_name, 00320 const pcl::PointCloud<PointT> &cloud); 00321 00326 template <typename PointT> int 00327 writeBinaryCompressed (const std::string &file_name, 00328 const pcl::PointCloud<PointT> &cloud); 00329 00335 template <typename PointT> int 00336 writeBinary (const std::string &file_name, 00337 const pcl::PointCloud<PointT> &cloud, 00338 const std::vector<int> &indices); 00339 00345 template <typename PointT> int 00346 writeASCII (const std::string &file_name, 00347 const pcl::PointCloud<PointT> &cloud, 00348 const int precision = 8); 00349 00356 template <typename PointT> int 00357 writeASCII (const std::string &file_name, 00358 const pcl::PointCloud<PointT> &cloud, 00359 const std::vector<int> &indices, 00360 const int precision = 8); 00361 00375 template<typename PointT> inline int 00376 write (const std::string &file_name, 00377 const pcl::PointCloud<PointT> &cloud, 00378 const bool binary = false) 00379 { 00380 if (binary) 00381 return (writeBinary<PointT> (file_name, cloud)); 00382 else 00383 return (writeASCII<PointT> (file_name, cloud)); 00384 } 00385 00400 template<typename PointT> inline int 00401 write (const std::string &file_name, 00402 const pcl::PointCloud<PointT> &cloud, 00403 const std::vector<int> &indices, 00404 bool binary = false) 00405 { 00406 if (binary) 00407 return (writeBinary<PointT> (file_name, cloud, indices)); 00408 else 00409 return (writeASCII<PointT> (file_name, cloud, indices)); 00410 } 00411 00412 private: 00413 00415 bool map_synchronization_; 00416 }; 00417 00418 namespace io 00419 { 00429 inline int 00430 loadPCDFile (const std::string &file_name, sensor_msgs::PointCloud2 &cloud) 00431 { 00432 pcl::PCDReader p; 00433 return (p.read (file_name, cloud)); 00434 } 00435 00444 inline int 00445 loadPCDFile (const std::string &file_name, sensor_msgs::PointCloud2 &cloud, 00446 Eigen::Vector4f &origin, Eigen::Quaternionf &orientation) 00447 { 00448 pcl::PCDReader p; 00449 int pcd_version; 00450 return (p.read (file_name, cloud, origin, orientation, pcd_version)); 00451 } 00452 00458 template<typename PointT> inline int 00459 loadPCDFile (const std::string &file_name, pcl::PointCloud<PointT> &cloud) 00460 { 00461 pcl::PCDReader p; 00462 return (p.read (file_name, cloud)); 00463 } 00464 00480 inline int 00481 savePCDFile (const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, 00482 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 00483 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), 00484 const bool binary_mode = false) 00485 { 00486 PCDWriter w; 00487 return (w.write (file_name, cloud, origin, orientation, binary_mode)); 00488 } 00489 00504 template<typename PointT> inline int 00505 savePCDFile (const std::string &file_name, const pcl::PointCloud<PointT> &cloud, bool binary_mode = false) 00506 { 00507 PCDWriter w; 00508 return (w.write<PointT> (file_name, cloud, binary_mode)); 00509 } 00510 00527 template<typename PointT> inline int 00528 savePCDFileASCII (const std::string &file_name, const pcl::PointCloud<PointT> &cloud) 00529 { 00530 PCDWriter w; 00531 return (w.write<PointT> (file_name, cloud, false)); 00532 } 00533 00543 template<typename PointT> inline int 00544 savePCDFileBinary (const std::string &file_name, const pcl::PointCloud<PointT> &cloud) 00545 { 00546 PCDWriter w; 00547 return (w.write<PointT> (file_name, cloud, true)); 00548 } 00549 00567 template<typename PointT> int 00568 savePCDFile (const std::string &file_name, 00569 const pcl::PointCloud<PointT> &cloud, 00570 const std::vector<int> &indices, 00571 const bool binary_mode = false) 00572 { 00573 // Save the data 00574 PCDWriter w; 00575 return (w.write<PointT> (file_name, cloud, indices, binary_mode)); 00576 } 00577 }; 00578 } 00579 00580 #include "pcl/io/impl/pcd_io.hpp" 00581 00582 #endif //#ifndef PCL_IO_PCD_IO_H_