|
Point Cloud Library (PCL)
1.5.1
|
|
Point Cloud Library (PCL)
1.5.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: ply_io.h 4702 2012-02-23 09:39:33Z gedikli $ 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 <pcl/PolygonMesh.h> 00046 #include <sstream> 00047 00048 namespace pcl 00049 { 00076 class PCL_EXPORTS PLYReader : public FileReader 00077 { 00078 public: 00079 enum 00080 { 00081 PLY_V0 = 0, 00082 PLY_V1 = 1 00083 }; 00084 00085 PLYReader () 00086 : FileReader () 00087 , origin_ (Eigen::Vector4f::Zero ()) 00088 , orientation_ (Eigen::Matrix3f::Zero ()) 00089 , range_grid_ (0) 00090 {} 00091 00092 ~PLYReader () { delete range_grid_; } 00110 int 00111 readHeader (const std::string &file_name, sensor_msgs::PointCloud2 &cloud, 00112 Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, 00113 int &ply_version, int &data_type, int &data_idx); 00114 00122 int 00123 read (const std::string &file_name, sensor_msgs::PointCloud2 &cloud, 00124 Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int& ply_version); 00125 00136 inline int 00137 read (const std::string &file_name, sensor_msgs::PointCloud2 &cloud) 00138 { 00139 Eigen::Vector4f origin; 00140 Eigen::Quaternionf orientation; 00141 int ply_version; 00142 return read (file_name, cloud, origin, orientation, ply_version); 00143 } 00144 00149 template<typename PointT> inline int 00150 read (const std::string &file_name, pcl::PointCloud<PointT> &cloud) 00151 { 00152 sensor_msgs::PointCloud2 blob; 00153 int ply_version; 00154 int res = read (file_name, blob, cloud.sensor_origin_, cloud.sensor_orientation_, 00155 ply_version); 00156 00157 // Exit in case of error 00158 if (res < 0) 00159 return (res); 00160 pcl::fromROSMsg (blob, cloud); 00161 return (0); 00162 } 00163 00164 private: 00165 ::pcl::io::ply::ply_parser parser_; 00166 00167 bool 00168 parse (const std::string& istream_filename); 00169 00175 void 00176 infoCallback (const std::string& filename, std::size_t line_number, const std::string& message) 00177 { 00178 PCL_DEBUG ("[pcl::PLYReader] %s:%lu: %s\n", filename.c_str (), line_number, message.c_str ()); 00179 } 00180 00186 void 00187 warningCallback (const std::string& filename, std::size_t line_number, const std::string& message) 00188 { 00189 PCL_WARN ("[pcl::PLYReader] %s:%lu: %s\n", filename.c_str (), line_number, message.c_str ()); 00190 } 00191 00197 void 00198 errorCallback (const std::string& filename, std::size_t line_number, const std::string& message) 00199 { 00200 PCL_ERROR ("[pcl::PLYReader] %s:%lu: %s\n", filename.c_str (), line_number, message.c_str ()); 00201 } 00202 00207 std::tr1::tuple<std::tr1::function<void ()>, std::tr1::function<void ()> > 00208 elementDefinitionCallback (const std::string& element_name, std::size_t count); 00209 00210 bool 00211 endHeaderCallback (); 00212 00217 template <typename ScalarType> std::tr1::function<void (ScalarType)> 00218 scalarPropertyDefinitionCallback (const std::string& element_name, const std::string& property_name); 00219 00224 template <typename SizeType, typename ScalarType> 00225 std::tr1::tuple<std::tr1::function<void (SizeType)>, std::tr1::function<void (ScalarType)>, std::tr1::function<void ()> > 00226 listPropertyDefinitionCallback (const std::string& element_name, const std::string& property_name); 00227 00232 inline void 00233 vertexFloatPropertyCallback (pcl::io::ply::float32 value); 00234 00241 inline void 00242 vertexColorCallback (const std::string& color_name, pcl::io::ply::uint8 color); 00243 00248 inline void 00249 vertexIntensityCallback (pcl::io::ply::uint8 intensity); 00250 00254 inline void 00255 originXCallback (const float& value) { origin_[0] = value; } 00256 00260 inline void 00261 originYCallback (const float& value) { origin_[1] = value; } 00262 00266 inline void 00267 originZCallback (const float& value) { origin_[2] = value; } 00268 00272 inline void 00273 orientationXaxisXCallback (const float& value) { orientation_ (0,0) = value; } 00274 00278 inline void 00279 orientationXaxisYCallback (const float& value) { orientation_ (0,1) = value; } 00280 00284 inline void 00285 orientationXaxisZCallback (const float& value) { orientation_ (0,2) = value; } 00286 00290 inline void 00291 orientationYaxisXCallback (const float& value) { orientation_ (1,0) = value; } 00292 00296 inline void 00297 orientationYaxisYCallback (const float& value) { orientation_ (1,1) = value; } 00298 00302 inline void 00303 orientationYaxisZCallback (const float& value) { orientation_ (1,2) = value; } 00304 00308 inline void 00309 orientationZaxisXCallback (const float& value) { orientation_ (2,0) = value; } 00310 00314 inline void 00315 orientationZaxisYCallback (const float& value) { orientation_ (2,1) = value; } 00316 00320 inline void 00321 orientationZaxisZCallback (const float& value) { orientation_ (2,2) = value; } 00322 00326 inline void 00327 cloudHeightCallback (const int &height) { cloud_->height = height; } 00328 00332 inline void 00333 cloudWidthCallback (const int &width) { cloud_->width = width; } 00334 00340 void 00341 appendFloatProperty (const std::string& name, const size_t& count = 1); 00342 00344 void 00345 vertexBeginCallback (); 00346 00348 void 00349 vertexEndCallback (); 00350 00352 void 00353 rangeGridBeginCallback (); 00354 00358 void 00359 rangeGridVertexIndicesBeginCallback (pcl::io::ply::uint8 size); 00360 00364 void 00365 rangeGridVertexIndicesElementCallback (pcl::io::ply::int32 vertex_index); 00366 00368 void 00369 rangeGridVertexIndicesEndCallback (); 00370 00372 void 00373 rangeGridEndCallback (); 00374 00376 void 00377 objInfoCallback (const std::string& line); 00378 00380 Eigen::Vector4f origin_; 00381 00383 Eigen::Matrix3f orientation_; 00384 00385 //vertex element artifacts 00386 sensor_msgs::PointCloud2 *cloud_; 00387 size_t vertex_count_, vertex_properties_counter_; 00388 int vertex_offset_before_; 00389 //range element artifacts 00390 std::vector<std::vector <int> > *range_grid_; 00391 size_t range_count_, range_grid_vertex_indices_element_index_; 00392 size_t rgb_offset_before_; 00393 00394 public: 00395 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00396 }; 00397 00402 class PCL_EXPORTS PLYWriter : public FileWriter 00403 { 00404 public: 00406 PLYWriter () : FileWriter () {}; 00407 00409 ~PLYWriter () {}; 00410 00420 inline std::string 00421 generateHeaderBinary (const sensor_msgs::PointCloud2 &cloud, 00422 const Eigen::Vector4f &origin, 00423 const Eigen::Quaternionf &orientation, 00424 int valid_points, 00425 bool use_camera = true) 00426 { 00427 return (generateHeader (cloud, origin, orientation, true, use_camera, valid_points)); 00428 } 00429 00439 inline std::string 00440 generateHeaderASCII (const sensor_msgs::PointCloud2 &cloud, 00441 const Eigen::Vector4f &origin, 00442 const Eigen::Quaternionf &orientation, 00443 int valid_points, 00444 bool use_camera = true) 00445 { 00446 return (generateHeader (cloud, origin, orientation, false, use_camera, valid_points)); 00447 } 00448 00458 int 00459 writeASCII (const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, 00460 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 00461 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), 00462 int precision = 8, 00463 bool use_camera = true); 00464 00471 int 00472 writeBinary (const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, 00473 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 00474 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity ()); 00475 00484 inline int 00485 write (const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, 00486 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 00487 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), 00488 bool binary = false) 00489 { 00490 if (binary) 00491 return (this->writeBinary (file_name, cloud, origin, orientation)); 00492 else 00493 return (this->writeASCII (file_name, cloud, origin, orientation, 8, true)); 00494 } 00495 00506 inline int 00507 write (const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, 00508 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 00509 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), 00510 bool binary = false, 00511 bool use_camera = true) 00512 { 00513 if (binary) 00514 return (this->writeBinary (file_name, cloud, origin, orientation)); 00515 else 00516 return (this->writeASCII (file_name, cloud, origin, orientation, 8, use_camera)); 00517 } 00518 00529 inline int 00530 write (const std::string &file_name, const sensor_msgs::PointCloud2::ConstPtr &cloud, 00531 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 00532 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), 00533 bool binary = false, 00534 bool use_camera = true) 00535 { 00536 return (write (file_name, *cloud, origin, orientation, binary, use_camera)); 00537 } 00538 00547 template<typename PointT> inline int 00548 write (const std::string &file_name, 00549 const pcl::PointCloud<PointT> &cloud, 00550 bool binary = false, 00551 bool use_camera = true) 00552 { 00553 Eigen::Vector4f origin = cloud.sensor_origin_; 00554 Eigen::Quaternionf orientation = cloud.sensor_orientation_; 00555 00556 sensor_msgs::PointCloud2 blob; 00557 pcl::toROSMsg (cloud, blob); 00558 00559 // Save the data 00560 return (this->write (file_name, blob, origin, orientation, binary, use_camera)); 00561 } 00562 00563 private: 00568 std::string 00569 generateHeader (const sensor_msgs::PointCloud2 &cloud, 00570 const Eigen::Vector4f &origin, 00571 const Eigen::Quaternionf &orientation, 00572 bool binary, 00573 bool use_camera, 00574 int valid_points); 00575 00576 void 00577 writeContentWithCameraASCII (int nr_points, 00578 int point_size, 00579 const sensor_msgs::PointCloud2 &cloud, 00580 const Eigen::Vector4f &origin, 00581 const Eigen::Quaternionf &orientation, 00582 std::ofstream& fs); 00583 00584 void 00585 writeContentWithRangeGridASCII (int nr_points, 00586 int point_size, 00587 const sensor_msgs::PointCloud2 &cloud, 00588 std::ostringstream& fs, 00589 int& nb_valid_points); 00590 }; 00591 00592 namespace io 00593 { 00603 inline int 00604 loadPLYFile (const std::string &file_name, sensor_msgs::PointCloud2 &cloud) 00605 { 00606 pcl::PLYReader p; 00607 return (p.read (file_name, cloud)); 00608 } 00609 00618 inline int 00619 loadPLYFile (const std::string &file_name, sensor_msgs::PointCloud2 &cloud, 00620 Eigen::Vector4f &origin, Eigen::Quaternionf &orientation) 00621 { 00622 pcl::PLYReader p; 00623 int ply_version; 00624 return (p.read (file_name, cloud, origin, orientation, ply_version)); 00625 } 00626 00632 template<typename PointT> inline int 00633 loadPLYFile (const std::string &file_name, pcl::PointCloud<PointT> &cloud) 00634 { 00635 pcl::PLYReader p; 00636 return (p.read (file_name, cloud)); 00637 } 00638 00647 inline int 00648 savePLYFile (const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, 00649 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 00650 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), 00651 bool binary_mode = false, bool use_camera = true) 00652 { 00653 PLYWriter w; 00654 return (w.write (file_name, cloud, origin, orientation, binary_mode, use_camera)); 00655 } 00656 00664 template<typename PointT> inline int 00665 savePLYFile (const std::string &file_name, const pcl::PointCloud<PointT> &cloud, bool binary_mode = false) 00666 { 00667 PLYWriter w; 00668 return (w.write<PointT> (file_name, cloud, binary_mode)); 00669 } 00670 00677 template<typename PointT> inline int 00678 savePLYFileASCII (const std::string &file_name, const pcl::PointCloud<PointT> &cloud) 00679 { 00680 PLYWriter w; 00681 return (w.write<PointT> (file_name, cloud, false)); 00682 } 00683 00689 template<typename PointT> inline int 00690 savePLYFileBinary (const std::string &file_name, const pcl::PointCloud<PointT> &cloud) 00691 { 00692 PLYWriter w; 00693 return (w.write<PointT> (file_name, cloud, true)); 00694 } 00695 00703 template<typename PointT> int 00704 savePLYFile (const std::string &file_name, const pcl::PointCloud<PointT> &cloud, 00705 const std::vector<int> &indices, bool binary_mode = false) 00706 { 00707 // Copy indices to a new point cloud 00708 pcl::PointCloud<PointT> cloud_out; 00709 copyPointCloud (cloud, indices, cloud_out); 00710 // Save the data 00711 PLYWriter w; 00712 return (w.write<PointT> (file_name, cloud_out, binary_mode)); 00713 } 00714 00721 PCL_EXPORTS int 00722 savePLYFile (const std::string &file_name, const pcl::PolygonMesh &mesh, unsigned precision = 5); 00723 00724 }; 00725 } 00726 00727 #endif //#ifndef PCL_IO_PLY_IO_H_
1.8.0
