|
Point Cloud Library (PCL)
1.6.0
|
|
Point Cloud Library (PCL)
1.6.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 5354 2012-03-27 22:13:21Z nizar $ 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/ply_parser.h> 00045 #include <boost/bind.hpp> 00046 #include <pcl/PolygonMesh.h> 00047 #include <sstream> 00048 00049 namespace pcl 00050 { 00077 class PCL_EXPORTS PLYReader : public FileReader 00078 { 00079 public: 00080 enum 00081 { 00082 PLY_V0 = 0, 00083 PLY_V1 = 1 00084 }; 00085 00086 PLYReader () 00087 : FileReader () 00088 , parser_ () 00089 , origin_ (Eigen::Vector4f::Zero ()) 00090 , orientation_ (Eigen::Matrix3f::Zero ()) 00091 , cloud_ () 00092 , vertex_count_ (0) 00093 , vertex_properties_counter_ (0) 00094 , vertex_offset_before_ (0) 00095 , range_grid_ (0) 00096 , range_count_ (0) 00097 , range_grid_vertex_indices_element_index_ (0) 00098 , rgb_offset_before_ (0) 00099 {} 00100 00101 PLYReader (const PLYReader &p) 00102 : parser_ () 00103 , origin_ (Eigen::Vector4f::Zero ()) 00104 , orientation_ (Eigen::Matrix3f::Zero ()) 00105 , cloud_ () 00106 , vertex_count_ (0) 00107 , vertex_properties_counter_ (0) 00108 , vertex_offset_before_ (0) 00109 , range_grid_ (0) 00110 , range_count_ (0) 00111 , range_grid_vertex_indices_element_index_ (0) 00112 , rgb_offset_before_ (0) 00113 { 00114 *this = p; 00115 } 00116 00117 PLYReader& 00118 operator = (const PLYReader &p) 00119 { 00120 origin_ = p.origin_; 00121 orientation_ = p.orientation_; 00122 range_grid_ = p.range_grid_; 00123 return (*this); 00124 } 00125 00126 ~PLYReader () { delete range_grid_; } 00149 int 00150 readHeader (const std::string &file_name, sensor_msgs::PointCloud2 &cloud, 00151 Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, 00152 int &ply_version, int &data_type, unsigned int &data_idx, const int offset = 0); 00153 00166 int 00167 read (const std::string &file_name, sensor_msgs::PointCloud2 &cloud, 00168 Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int& ply_version, const int offset = 0); 00169 00185 inline int 00186 read (const std::string &file_name, sensor_msgs::PointCloud2 &cloud, const int offset = 0) 00187 { 00188 Eigen::Vector4f origin; 00189 Eigen::Quaternionf orientation; 00190 int ply_version; 00191 return read (file_name, cloud, origin, orientation, ply_version, offset); 00192 } 00193 00203 template<typename PointT> inline int 00204 read (const std::string &file_name, pcl::PointCloud<PointT> &cloud, const int offset = 0) 00205 { 00206 sensor_msgs::PointCloud2 blob; 00207 int ply_version; 00208 int res = read (file_name, blob, cloud.sensor_origin_, cloud.sensor_orientation_, 00209 ply_version, offset); 00210 00211 // Exit in case of error 00212 if (res < 0) 00213 return (res); 00214 pcl::fromROSMsg (blob, cloud); 00215 return (0); 00216 } 00217 00218 private: 00219 ::pcl::io::ply::ply_parser parser_; 00220 00221 bool 00222 parse (const std::string& istream_filename); 00223 00229 void 00230 infoCallback (const std::string& filename, std::size_t line_number, const std::string& message) 00231 { 00232 PCL_DEBUG ("[pcl::PLYReader] %s:%lu: %s\n", filename.c_str (), line_number, message.c_str ()); 00233 } 00234 00240 void 00241 warningCallback (const std::string& filename, std::size_t line_number, const std::string& message) 00242 { 00243 PCL_WARN ("[pcl::PLYReader] %s:%lu: %s\n", filename.c_str (), line_number, message.c_str ()); 00244 } 00245 00251 void 00252 errorCallback (const std::string& filename, std::size_t line_number, const std::string& message) 00253 { 00254 PCL_ERROR ("[pcl::PLYReader] %s:%lu: %s\n", filename.c_str (), line_number, message.c_str ()); 00255 } 00256 00261 boost::tuple<boost::function<void ()>, boost::function<void ()> > 00262 elementDefinitionCallback (const std::string& element_name, std::size_t count); 00263 00264 bool 00265 endHeaderCallback (); 00266 00271 template <typename ScalarType> boost::function<void (ScalarType)> 00272 scalarPropertyDefinitionCallback (const std::string& element_name, const std::string& property_name); 00273 00278 template <typename SizeType, typename ScalarType> 00279 boost::tuple<boost::function<void (SizeType)>, boost::function<void (ScalarType)>, boost::function<void ()> > 00280 listPropertyDefinitionCallback (const std::string& element_name, const std::string& property_name); 00281 00286 inline void 00287 vertexFloatPropertyCallback (pcl::io::ply::float32 value); 00288 00295 inline void 00296 vertexColorCallback (const std::string& color_name, pcl::io::ply::uint8 color); 00297 00302 inline void 00303 vertexIntensityCallback (pcl::io::ply::uint8 intensity); 00304 00308 inline void 00309 originXCallback (const float& value) { origin_[0] = value; } 00310 00314 inline void 00315 originYCallback (const float& value) { origin_[1] = value; } 00316 00320 inline void 00321 originZCallback (const float& value) { origin_[2] = value; } 00322 00326 inline void 00327 orientationXaxisXCallback (const float& value) { orientation_ (0,0) = value; } 00328 00332 inline void 00333 orientationXaxisYCallback (const float& value) { orientation_ (0,1) = value; } 00334 00338 inline void 00339 orientationXaxisZCallback (const float& value) { orientation_ (0,2) = value; } 00340 00344 inline void 00345 orientationYaxisXCallback (const float& value) { orientation_ (1,0) = value; } 00346 00350 inline void 00351 orientationYaxisYCallback (const float& value) { orientation_ (1,1) = value; } 00352 00356 inline void 00357 orientationYaxisZCallback (const float& value) { orientation_ (1,2) = value; } 00358 00362 inline void 00363 orientationZaxisXCallback (const float& value) { orientation_ (2,0) = value; } 00364 00368 inline void 00369 orientationZaxisYCallback (const float& value) { orientation_ (2,1) = value; } 00370 00374 inline void 00375 orientationZaxisZCallback (const float& value) { orientation_ (2,2) = value; } 00376 00380 inline void 00381 cloudHeightCallback (const int &height) { cloud_->height = height; } 00382 00386 inline void 00387 cloudWidthCallback (const int &width) { cloud_->width = width; } 00388 00394 void 00395 appendFloatProperty (const std::string& name, const size_t& count = 1); 00396 00398 void 00399 vertexBeginCallback (); 00400 00402 void 00403 vertexEndCallback (); 00404 00406 void 00407 rangeGridBeginCallback (); 00408 00412 void 00413 rangeGridVertexIndicesBeginCallback (pcl::io::ply::uint8 size); 00414 00418 void 00419 rangeGridVertexIndicesElementCallback (pcl::io::ply::int32 vertex_index); 00420 00422 void 00423 rangeGridVertexIndicesEndCallback (); 00424 00426 void 00427 rangeGridEndCallback (); 00428 00430 void 00431 objInfoCallback (const std::string& line); 00432 00434 Eigen::Vector4f origin_; 00435 00437 Eigen::Matrix3f orientation_; 00438 00439 //vertex element artifacts 00440 sensor_msgs::PointCloud2 *cloud_; 00441 size_t vertex_count_, vertex_properties_counter_; 00442 int vertex_offset_before_; 00443 //range element artifacts 00444 std::vector<std::vector <int> > *range_grid_; 00445 size_t range_count_, range_grid_vertex_indices_element_index_; 00446 size_t rgb_offset_before_; 00447 00448 public: 00449 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00450 }; 00451 00456 class PCL_EXPORTS PLYWriter : public FileWriter 00457 { 00458 public: 00460 PLYWriter () : FileWriter () {}; 00461 00463 ~PLYWriter () {}; 00464 00474 inline std::string 00475 generateHeaderBinary (const sensor_msgs::PointCloud2 &cloud, 00476 const Eigen::Vector4f &origin, 00477 const Eigen::Quaternionf &orientation, 00478 int valid_points, 00479 bool use_camera = true) 00480 { 00481 return (generateHeader (cloud, origin, orientation, true, use_camera, valid_points)); 00482 } 00483 00493 inline std::string 00494 generateHeaderASCII (const sensor_msgs::PointCloud2 &cloud, 00495 const Eigen::Vector4f &origin, 00496 const Eigen::Quaternionf &orientation, 00497 int valid_points, 00498 bool use_camera = true) 00499 { 00500 return (generateHeader (cloud, origin, orientation, false, use_camera, valid_points)); 00501 } 00502 00512 int 00513 writeASCII (const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, 00514 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 00515 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), 00516 int precision = 8, 00517 bool use_camera = true); 00518 00525 int 00526 writeBinary (const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, 00527 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 00528 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity ()); 00529 00538 inline int 00539 write (const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, 00540 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 00541 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), 00542 const bool binary = false) 00543 { 00544 if (binary) 00545 return (this->writeBinary (file_name, cloud, origin, orientation)); 00546 else 00547 return (this->writeASCII (file_name, cloud, origin, orientation, 8, true)); 00548 } 00549 00560 inline int 00561 write (const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, 00562 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 00563 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), 00564 bool binary = false, 00565 bool use_camera = true) 00566 { 00567 if (binary) 00568 return (this->writeBinary (file_name, cloud, origin, orientation)); 00569 else 00570 return (this->writeASCII (file_name, cloud, origin, orientation, 8, use_camera)); 00571 } 00572 00583 inline int 00584 write (const std::string &file_name, const sensor_msgs::PointCloud2::ConstPtr &cloud, 00585 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 00586 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), 00587 bool binary = false, 00588 bool use_camera = true) 00589 { 00590 return (write (file_name, *cloud, origin, orientation, binary, use_camera)); 00591 } 00592 00601 template<typename PointT> inline int 00602 write (const std::string &file_name, 00603 const pcl::PointCloud<PointT> &cloud, 00604 bool binary = false, 00605 bool use_camera = true) 00606 { 00607 Eigen::Vector4f origin = cloud.sensor_origin_; 00608 Eigen::Quaternionf orientation = cloud.sensor_orientation_; 00609 00610 sensor_msgs::PointCloud2 blob; 00611 pcl::toROSMsg (cloud, blob); 00612 00613 // Save the data 00614 return (this->write (file_name, blob, origin, orientation, binary, use_camera)); 00615 } 00616 00617 private: 00622 std::string 00623 generateHeader (const sensor_msgs::PointCloud2 &cloud, 00624 const Eigen::Vector4f &origin, 00625 const Eigen::Quaternionf &orientation, 00626 bool binary, 00627 bool use_camera, 00628 int valid_points); 00629 00630 void 00631 writeContentWithCameraASCII (int nr_points, 00632 int point_size, 00633 const sensor_msgs::PointCloud2 &cloud, 00634 const Eigen::Vector4f &origin, 00635 const Eigen::Quaternionf &orientation, 00636 std::ofstream& fs); 00637 00638 void 00639 writeContentWithRangeGridASCII (int nr_points, 00640 int point_size, 00641 const sensor_msgs::PointCloud2 &cloud, 00642 std::ostringstream& fs, 00643 int& nb_valid_points); 00644 }; 00645 00646 namespace io 00647 { 00657 inline int 00658 loadPLYFile (const std::string &file_name, sensor_msgs::PointCloud2 &cloud) 00659 { 00660 pcl::PLYReader p; 00661 return (p.read (file_name, cloud)); 00662 } 00663 00672 inline int 00673 loadPLYFile (const std::string &file_name, sensor_msgs::PointCloud2 &cloud, 00674 Eigen::Vector4f &origin, Eigen::Quaternionf &orientation) 00675 { 00676 pcl::PLYReader p; 00677 int ply_version; 00678 return (p.read (file_name, cloud, origin, orientation, ply_version)); 00679 } 00680 00686 template<typename PointT> inline int 00687 loadPLYFile (const std::string &file_name, pcl::PointCloud<PointT> &cloud) 00688 { 00689 pcl::PLYReader p; 00690 return (p.read (file_name, cloud)); 00691 } 00692 00701 inline int 00702 savePLYFile (const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, 00703 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 00704 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), 00705 bool binary_mode = false, bool use_camera = true) 00706 { 00707 PLYWriter w; 00708 return (w.write (file_name, cloud, origin, orientation, binary_mode, use_camera)); 00709 } 00710 00718 template<typename PointT> inline int 00719 savePLYFile (const std::string &file_name, const pcl::PointCloud<PointT> &cloud, bool binary_mode = false) 00720 { 00721 PLYWriter w; 00722 return (w.write<PointT> (file_name, cloud, binary_mode)); 00723 } 00724 00731 template<typename PointT> inline int 00732 savePLYFileASCII (const std::string &file_name, const pcl::PointCloud<PointT> &cloud) 00733 { 00734 PLYWriter w; 00735 return (w.write<PointT> (file_name, cloud, false)); 00736 } 00737 00743 template<typename PointT> inline int 00744 savePLYFileBinary (const std::string &file_name, const pcl::PointCloud<PointT> &cloud) 00745 { 00746 PLYWriter w; 00747 return (w.write<PointT> (file_name, cloud, true)); 00748 } 00749 00757 template<typename PointT> int 00758 savePLYFile (const std::string &file_name, const pcl::PointCloud<PointT> &cloud, 00759 const std::vector<int> &indices, bool binary_mode = false) 00760 { 00761 // Copy indices to a new point cloud 00762 pcl::PointCloud<PointT> cloud_out; 00763 copyPointCloud (cloud, indices, cloud_out); 00764 // Save the data 00765 PLYWriter w; 00766 return (w.write<PointT> (file_name, cloud_out, binary_mode)); 00767 } 00768 00775 PCL_EXPORTS int 00776 savePLYFile (const std::string &file_name, const pcl::PolygonMesh &mesh, unsigned precision = 5); 00777 } 00778 } 00779 00780 #endif //#ifndef PCL_IO_PLY_IO_H_
1.7.6.1
