Point Cloud Library (PCL)  1.9.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
ply_io.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  *
38  */
39 
40 #ifndef PCL_IO_PLY_IO_H_
41 #define PCL_IO_PLY_IO_H_
42 
43 #include <pcl/io/boost.h>
44 #include <pcl/io/file_io.h>
45 #include <pcl/io/ply/ply_parser.h>
46 #include <pcl/PolygonMesh.h>
47 #include <sstream>
48 
49 namespace pcl
50 {
51  /** \brief Point Cloud Data (PLY) file format reader.
52  *
53  * The PLY data format is organized in the following way:
54  * lines beginning with "comment" are treated as comments
55  * - ply
56  * - format [ascii|binary_little_endian|binary_big_endian] 1.0
57  * - element vertex COUNT
58  * - property float x
59  * - property float y
60  * - [property float z]
61  * - [property float normal_x]
62  * - [property float normal_y]
63  * - [property float normal_z]
64  * - [property uchar red]
65  * - [property uchar green]
66  * - [property uchar blue] ...
67  * - ascii/binary point coordinates
68  * - [element camera 1]
69  * - [property float view_px] ...
70  * - [element range_grid COUNT]
71  * - [property list uchar int vertex_indices]
72  * - end header
73  *
74  * \author Nizar Sallem
75  * \ingroup io
76  */
77  class PCL_EXPORTS PLYReader : public FileReader
78  {
79  public:
80  enum
81  {
82  PLY_V0 = 0,
83  PLY_V1 = 1
84  };
85 
87  : FileReader ()
88  , parser_ ()
89  , origin_ (Eigen::Vector4f::Zero ())
90  , orientation_ (Eigen::Matrix3f::Zero ())
91  , cloud_ ()
92  , vertex_count_ (0)
93  , vertex_offset_before_ (0)
94  , range_grid_ (0)
95  , rgb_offset_before_ (0)
96  , do_resize_ (false)
97  , polygons_ (0)
98  , r_(0), g_(0), b_(0)
99  , a_(0), rgba_(0)
100  {}
101 
102  PLYReader (const PLYReader &p)
103  : FileReader ()
104  , parser_ ()
105  , origin_ (Eigen::Vector4f::Zero ())
106  , orientation_ (Eigen::Matrix3f::Zero ())
107  , cloud_ ()
108  , vertex_count_ (0)
109  , vertex_offset_before_ (0)
110  , range_grid_ (0)
111  , rgb_offset_before_ (0)
112  , do_resize_ (false)
113  , polygons_ (0)
114  , r_(0), g_(0), b_(0)
115  , a_(0), rgba_(0)
116  {
117  *this = p;
118  }
119 
120  PLYReader&
121  operator = (const PLYReader &p)
122  {
123  origin_ = p.origin_;
124  orientation_ = p.orientation_;
125  range_grid_ = p.range_grid_;
126  polygons_ = p.polygons_;
127  return (*this);
128  }
129 
130  ~PLYReader () { delete range_grid_; }
131  /** \brief Read a point cloud data header from a PLY file.
132  *
133  * Load only the meta information (number of points, their types, etc),
134  * and not the points themselves, from a given PLY file. Useful for fast
135  * evaluation of the underlying data structure.
136  *
137  * Returns:
138  * * < 0 (-1) on error
139  * * > 0 on success
140  * \param[in] file_name the name of the file to load
141  * \param[out] cloud the resultant point cloud dataset (only the header will be filled)
142  * \param[in] origin the sensor data acquisition origin (translation)
143  * \param[in] orientation the sensor data acquisition origin (rotation)
144  * \param[out] ply_version the PLY version read from the file
145  * \param[out] data_type the type of PLY data stored in the file
146  * \param[out] data_idx the data index
147  * \param[in] offset the offset in the file where to expect the true header to begin.
148  * One usage example for setting the offset parameter is for reading
149  * data from a TAR "archive containing multiple files: TAR files always
150  * add a 512 byte header in front of the actual file, so set the offset
151  * to the next byte after the header (e.g., 513).
152  */
153  int
154  readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
155  Eigen::Vector4f &origin, Eigen::Quaternionf &orientation,
156  int &ply_version, int &data_type, unsigned int &data_idx, const int offset = 0);
157 
158  /** \brief Read a point cloud data from a PLY file and store it into a pcl/PCLPointCloud2.
159  * \param[in] file_name the name of the file containing the actual PointCloud data
160  * \param[out] cloud the resultant PointCloud message read from disk
161  * \param[in] origin the sensor data acquisition origin (translation)
162  * \param[in] orientation the sensor data acquisition origin (rotation)
163  * \param[out] ply_version the PLY version read from the file
164  * \param[in] offset the offset in the file where to expect the true header to begin.
165  * One usage example for setting the offset parameter is for reading
166  * data from a TAR "archive containing multiple files: TAR files always
167  * add a 512 byte header in front of the actual file, so set the offset
168  * to the next byte after the header (e.g., 513).
169  */
170  int
171  read (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
172  Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int& ply_version, const int offset = 0);
173 
174  /** \brief Read a point cloud data from a PLY file and store it into a pcl/PCLPointCloud2.
175  * \note This function is provided for backwards compatibility only
176  * \param[in] file_name the name of the file containing the actual PointCloud data
177  * \param[out] cloud the resultant PointCloud message read from disk
178  * \param[in] offset the offset in the file where to expect the true header to begin.
179  * One usage example for setting the offset parameter is for reading
180  * data from a TAR "archive containing multiple files: TAR files always
181  * add a 512 byte header in front of the actual file, so set the offset
182  * to the next byte after the header (e.g., 513).
183  */
184  inline int
185  read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, const int offset = 0)
186  {
187  Eigen::Vector4f origin;
188  Eigen::Quaternionf orientation;
189  int ply_version;
190  return read (file_name, cloud, origin, orientation, ply_version, offset);
191  }
192 
193  /** \brief Read a point cloud data from any PLY file, and convert it to the given template format.
194  * \param[in] file_name the name of the file containing the actual PointCloud data
195  * \param[out] cloud the resultant PointCloud message read from disk
196  * \param[in] offset the offset in the file where to expect the true header to begin.
197  * One usage example for setting the offset parameter is for reading
198  * data from a TAR "archive containing multiple files: TAR files always
199  * add a 512 byte header in front of the actual file, so set the offset
200  * to the next byte after the header (e.g., 513).
201  */
202  template<typename PointT> inline int
203  read (const std::string &file_name, pcl::PointCloud<PointT> &cloud, const int offset = 0)
204  {
205  pcl::PCLPointCloud2 blob;
206  int ply_version;
207  int res = read (file_name, blob, cloud.sensor_origin_, cloud.sensor_orientation_,
208  ply_version, offset);
209 
210  // Exit in case of error
211  if (res < 0)
212  return (res);
213  pcl::fromPCLPointCloud2 (blob, cloud);
214  return (0);
215  }
216 
217  /** \brief Read a point cloud data from a PLY file and store it into a pcl/PolygonMesh.
218  *
219  * \param[in] file_name the name of the file containing the actual PointCloud data
220  * \param[out] mesh the resultant PolygonMesh message read from disk
221  * \param[in] origin the sensor data acquisition origin (translation)
222  * \param[in] orientation the sensor data acquisition origin (rotation)
223  * \param[out] ply_version the PLY version read from the file
224  * \param[in] offset the offset in the file where to expect the true header to begin.
225  * One usage example for setting the offset parameter is for reading
226  * data from a TAR "archive containing multiple files: TAR files always
227  * add a 512 byte header in front of the actual file, so set the offset
228  * to the next byte after the header (e.g., 513).
229  */
230  int
231  read (const std::string &file_name, pcl::PolygonMesh &mesh,
232  Eigen::Vector4f &origin, Eigen::Quaternionf &orientation,
233  int& ply_version, const int offset = 0);
234 
235  /** \brief Read a point cloud data from a PLY file and store it into a pcl/PolygonMesh.
236  *
237  * \param[in] file_name the name of the file containing the actual PointCloud data
238  * \param[out] mesh the resultant PolygonMesh message read from disk
239  * \param[in] offset the offset in the file where to expect the true header to begin.
240  * One usage example for setting the offset parameter is for reading
241  * data from a TAR "archive containing multiple files: TAR files always
242  * add a 512 byte header in front of the actual file, so set the offset
243  * to the next byte after the header (e.g., 513).
244  */
245  int
246  read (const std::string &file_name, pcl::PolygonMesh &mesh, const int offset = 0);
247 
248  private:
250 
251  bool
252  parse (const std::string& istream_filename);
253 
254  /** \brief Info callback function
255  * \param[in] filename PLY file read
256  * \param[in] line_number line triggering the callback
257  * \param[in] message information message
258  */
259  void
260  infoCallback (const std::string& filename, std::size_t line_number, const std::string& message)
261  {
262  PCL_DEBUG ("[pcl::PLYReader] %s:%lu: %s\n", filename.c_str (), line_number, message.c_str ());
263  }
264 
265  /** \brief Warning callback function
266  * \param[in] filename PLY file read
267  * \param[in] line_number line triggering the callback
268  * \param[in] message warning message
269  */
270  void
271  warningCallback (const std::string& filename, std::size_t line_number, const std::string& message)
272  {
273  PCL_WARN ("[pcl::PLYReader] %s:%lu: %s\n", filename.c_str (), line_number, message.c_str ());
274  }
275 
276  /** \brief Error callback function
277  * \param[in] filename PLY file read
278  * \param[in] line_number line triggering the callback
279  * \param[in] message error message
280  */
281  void
282  errorCallback (const std::string& filename, std::size_t line_number, const std::string& message)
283  {
284  PCL_ERROR ("[pcl::PLYReader] %s:%lu: %s\n", filename.c_str (), line_number, message.c_str ());
285  }
286 
287  /** \brief function called when the keyword element is parsed
288  * \param[in] element_name element name
289  * \param[in] count number of instances
290  */
291  boost::tuple<boost::function<void ()>, boost::function<void ()> >
292  elementDefinitionCallback (const std::string& element_name, std::size_t count);
293 
294  bool
295  endHeaderCallback ();
296 
297  /** \brief function called when a scalar property is parsed
298  * \param[in] element_name element name to which the property belongs
299  * \param[in] property_name property name
300  */
301  template <typename ScalarType> boost::function<void (ScalarType)>
302  scalarPropertyDefinitionCallback (const std::string& element_name, const std::string& property_name);
303 
304  /** \brief function called when a list property is parsed
305  * \param[in] element_name element name to which the property belongs
306  * \param[in] property_name list property name
307  */
308  template <typename SizeType, typename ScalarType>
309  boost::tuple<boost::function<void (SizeType)>, boost::function<void (ScalarType)>, boost::function<void ()> >
310  listPropertyDefinitionCallback (const std::string& element_name, const std::string& property_name);
311 
312  /** \brief function called at the beginning of a list property parsing.
313  * \param[in] size number of elements in the list
314  */
315  template <typename SizeType> void
316  vertexListPropertyBeginCallback (const std::string& property_name, SizeType size);
317 
318  /** \brief function called when a list element is parsed.
319  * \param[in] value the list's element value
320  */
321  template <typename ContentType> void
322  vertexListPropertyContentCallback (ContentType value);
323 
324  /** \brief function called at the end of a list property parsing */
325  inline void
326  vertexListPropertyEndCallback ();
327 
328  /** Callback function for an anonymous vertex scalar property.
329  * Writes down a double value in cloud data.
330  * param[in] value double value parsed
331  */
332  template<typename Scalar> void
333  vertexScalarPropertyCallback (Scalar value);
334 
335  /** Callback function for vertex RGB color.
336  * This callback is in charge of packing red green and blue in a single int
337  * before writing it down in cloud data.
338  * param[in] color_name color name in {red, green, blue}
339  * param[in] color value of {red, green, blue} property
340  */
341  inline void
342  vertexColorCallback (const std::string& color_name, pcl::io::ply::uint8 color);
343 
344  /** Callback function for vertex intensity.
345  * converts intensity from int to float before writing it down in cloud data.
346  * param[in] intensity
347  */
348  inline void
349  vertexIntensityCallback (pcl::io::ply::uint8 intensity);
350 
351  /** Callback function for vertex alpha.
352  * extracts RGB value, append alpha and put it back
353  * param[in] alpha
354  */
355  inline void
356  vertexAlphaCallback (pcl::io::ply::uint8 alpha);
357 
358  /** Callback function for origin x component.
359  * param[in] value origin x value
360  */
361  inline void
362  originXCallback (const float& value) { origin_[0] = value; }
363 
364  /** Callback function for origin y component.
365  * param[in] value origin y value
366  */
367  inline void
368  originYCallback (const float& value) { origin_[1] = value; }
369 
370  /** Callback function for origin z component.
371  * param[in] value origin z value
372  */
373  inline void
374  originZCallback (const float& value) { origin_[2] = value; }
375 
376  /** Callback function for orientation x axis x component.
377  * param[in] value orientation x axis x value
378  */
379  inline void
380  orientationXaxisXCallback (const float& value) { orientation_ (0,0) = value; }
381 
382  /** Callback function for orientation x axis y component.
383  * param[in] value orientation x axis y value
384  */
385  inline void
386  orientationXaxisYCallback (const float& value) { orientation_ (0,1) = value; }
387 
388  /** Callback function for orientation x axis z component.
389  * param[in] value orientation x axis z value
390  */
391  inline void
392  orientationXaxisZCallback (const float& value) { orientation_ (0,2) = value; }
393 
394  /** Callback function for orientation y axis x component.
395  * param[in] value orientation y axis x value
396  */
397  inline void
398  orientationYaxisXCallback (const float& value) { orientation_ (1,0) = value; }
399 
400  /** Callback function for orientation y axis y component.
401  * param[in] value orientation y axis y value
402  */
403  inline void
404  orientationYaxisYCallback (const float& value) { orientation_ (1,1) = value; }
405 
406  /** Callback function for orientation y axis z component.
407  * param[in] value orientation y axis z value
408  */
409  inline void
410  orientationYaxisZCallback (const float& value) { orientation_ (1,2) = value; }
411 
412  /** Callback function for orientation z axis x component.
413  * param[in] value orientation z axis x value
414  */
415  inline void
416  orientationZaxisXCallback (const float& value) { orientation_ (2,0) = value; }
417 
418  /** Callback function for orientation z axis y component.
419  * param[in] value orientation z axis y value
420  */
421  inline void
422  orientationZaxisYCallback (const float& value) { orientation_ (2,1) = value; }
423 
424  /** Callback function for orientation z axis z component.
425  * param[in] value orientation z axis z value
426  */
427  inline void
428  orientationZaxisZCallback (const float& value) { orientation_ (2,2) = value; }
429 
430  /** Callback function to set the cloud height
431  * param[in] height cloud height
432  */
433  inline void
434  cloudHeightCallback (const int &height) { cloud_->height = height; }
435 
436  /** Callback function to set the cloud width
437  * param[in] width cloud width
438  */
439  inline void
440  cloudWidthCallback (const int &width) { cloud_->width = width; }
441 
442  /** Append a scalar property to the cloud fields.
443  * param[in] name property name
444  * param[in] count property count: 1 for scalar properties and higher for a
445  * list property.
446  */
447  template<typename Scalar> void
448  appendScalarProperty (const std::string& name, const size_t& count = 1);
449 
450  /** Amend property from cloud fields identified by \a old_name renaming
451  * it \a new_name.
452  * param[in] old_name property old name
453  * param[in] new_name property new name
454  */
455  void
456  amendProperty (const std::string& old_name, const std::string& new_name, uint8_t datatype = 0);
457 
458  /** Callback function for the begin of vertex line */
459  void
460  vertexBeginCallback ();
461 
462  /** Callback function for the end of vertex line */
463  void
464  vertexEndCallback ();
465 
466  /** Callback function for the begin of range_grid line */
467  void
468  rangeGridBeginCallback ();
469 
470  /** Callback function for the begin of range_grid vertex_indices property
471  * param[in] size vertex_indices list size
472  */
473  void
474  rangeGridVertexIndicesBeginCallback (pcl::io::ply::uint8 size);
475 
476  /** Callback function for each range_grid vertex_indices element
477  * param[in] vertex_index index of the vertex in vertex_indices
478  */
479  void
480  rangeGridVertexIndicesElementCallback (pcl::io::ply::int32 vertex_index);
481 
482  /** Callback function for the end of a range_grid vertex_indices property */
483  void
484  rangeGridVertexIndicesEndCallback ();
485 
486  /** Callback function for the end of a range_grid element end */
487  void
488  rangeGridEndCallback ();
489 
490  /** Callback function for obj_info */
491  void
492  objInfoCallback (const std::string& line);
493 
494  /** Callback function for the begin of face line */
495  void
496  faceBeginCallback ();
497 
498  /** Callback function for the begin of face vertex_indices property
499  * param[in] size vertex_indices list size
500  */
501  void
502  faceVertexIndicesBeginCallback (pcl::io::ply::uint8 size);
503 
504  /** Callback function for each face vertex_indices element
505  * param[in] vertex_index index of the vertex in vertex_indices
506  */
507  void
508  faceVertexIndicesElementCallback (pcl::io::ply::int32 vertex_index);
509 
510  /** Callback function for the end of a face vertex_indices property */
511  void
512  faceVertexIndicesEndCallback ();
513 
514  /** Callback function for the end of a face element end */
515  void
516  faceEndCallback ();
517 
518  /// origin
519  Eigen::Vector4f origin_;
520 
521  /// orientation
522  Eigen::Matrix3f orientation_;
523 
524  //vertex element artifacts
525  pcl::PCLPointCloud2 *cloud_;
526  size_t vertex_count_;
527  int vertex_offset_before_;
528  //range element artifacts
529  std::vector<std::vector <int> > *range_grid_;
530  size_t rgb_offset_before_;
531  bool do_resize_;
532  //face element artifact
533  std::vector<pcl::Vertices> *polygons_;
534  public:
535  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
536 
537  private:
538  // RGB values stored by vertexColorCallback()
539  int32_t r_, g_, b_;
540  // Color values stored by vertexAlphaCallback()
541  uint32_t a_, rgba_;
542  };
543 
544  /** \brief Point Cloud Data (PLY) file format writer.
545  * \author Nizar Sallem
546  * \ingroup io
547  */
548  class PCL_EXPORTS PLYWriter : public FileWriter
549  {
550  public:
551  ///Constructor
552  PLYWriter () : FileWriter () {};
553 
554  ///Destructor
555  ~PLYWriter () {};
556 
557  /** \brief Generate the header of a PLY v.7 file format
558  * \param[in] cloud the point cloud data message
559  * \param[in] origin the sensor data acquisition origin (translation)
560  * \param[in] orientation the sensor data acquisition origin (rotation)
561  * \param[in] valid_points number of valid points (finite ones for range_grid and
562  * all of them for camer)
563  * \param[in] use_camera if set to true then PLY file will use element camera else
564  * element range_grid will be used.
565  */
566  inline std::string
568  const Eigen::Vector4f &origin,
569  const Eigen::Quaternionf &orientation,
570  int valid_points,
571  bool use_camera = true)
572  {
573  return (generateHeader (cloud, origin, orientation, true, use_camera, valid_points));
574  }
575 
576  /** \brief Generate the header of a PLY v.7 file format
577  * \param[in] cloud the point cloud data message
578  * \param[in] origin the sensor data acquisition origin (translation)
579  * \param[in] orientation the sensor data acquisition origin (rotation)
580  * \param[in] valid_points number of valid points (finite ones for range_grid and
581  * all of them for camer)
582  * \param[in] use_camera if set to true then PLY file will use element camera else
583  * element range_grid will be used.
584  */
585  inline std::string
587  const Eigen::Vector4f &origin,
588  const Eigen::Quaternionf &orientation,
589  int valid_points,
590  bool use_camera = true)
591  {
592  return (generateHeader (cloud, origin, orientation, false, use_camera, valid_points));
593  }
594 
595  /** \brief Save point cloud data to a PLY file containing n-D points, in ASCII format
596  * \param[in] file_name the output file name
597  * \param[in] cloud the point cloud data message
598  * \param[in] origin the sensor data acquisition origin (translation)
599  * \param[in] orientation the sensor data acquisition origin (rotation)
600  * \param[in] precision the specified output numeric stream precision (default: 8)
601  * \param[in] use_camera if set to true then PLY file will use element camera else
602  * element range_grid will be used.
603  */
604  int
605  writeASCII (const std::string &file_name, const pcl::PCLPointCloud2 &cloud,
606  const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
607  const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
608  int precision = 8,
609  bool use_camera = true);
610 
611  /** \brief Save point cloud data to a PLY file containing n-D points, in BINARY format
612  * \param[in] file_name the output file name
613  * \param[in] cloud the point cloud data message
614  * \param[in] origin the sensor data acquisition origin (translation)
615  * \param[in] orientation the sensor data acquisition origin (rotation)
616  * \param[in] use_camera if set to true then PLY file will use element camera else
617  * element range_grid will be used
618  */
619  int
620  writeBinary (const std::string &file_name, const pcl::PCLPointCloud2 &cloud,
621  const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
622  const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
623  bool use_camera = true);
624 
625  /** \brief Save point cloud data to a PLY file containing n-D points
626  * \param[in] file_name the output file name
627  * \param[in] cloud the point cloud data message
628  * \param[in] origin the sensor acquisition origin
629  * \param[in] orientation the sensor acquisition orientation
630  * \param[in] binary set to true if the file is to be written in a binary
631  * PLY format, false (default) for ASCII
632  */
633  inline int
634  write (const std::string &file_name, const pcl::PCLPointCloud2 &cloud,
635  const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
636  const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
637  const bool binary = false)
638  {
639  if (binary)
640  return (this->writeBinary (file_name, cloud, origin, orientation, true));
641  else
642  return (this->writeASCII (file_name, cloud, origin, orientation, 8, true));
643  }
644 
645  /** \brief Save point cloud data to a PLY file containing n-D points
646  * \param[in] file_name the output file name
647  * \param[in] cloud the point cloud data message
648  * \param[in] origin the sensor acquisition origin
649  * \param[in] orientation the sensor acquisition orientation
650  * \param[in] binary set to true if the file is to be written in a binary
651  * PLY format, false (default) for ASCII
652  * \param[in] use_camera set to true to use camera element and false to
653  * use range_grid element
654  */
655  inline int
656  write (const std::string &file_name, const pcl::PCLPointCloud2 &cloud,
657  const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
658  const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
659  bool binary = false,
660  bool use_camera = true)
661  {
662  if (binary)
663  return (this->writeBinary (file_name, cloud, origin, orientation, use_camera));
664  else
665  return (this->writeASCII (file_name, cloud, origin, orientation, 8, use_camera));
666  }
667 
668  /** \brief Save point cloud data to a PLY file containing n-D points
669  * \param[in] file_name the output file name
670  * \param[in] cloud the point cloud data message (boost shared pointer)
671  * \param[in] origin the sensor acquisition origin
672  * \param[in] orientation the sensor acquisition orientation
673  * \param[in] binary set to true if the file is to be written in a binary
674  * PLY format, false (default) for ASCII
675  * \param[in] use_camera set to true to use camera element and false to
676  * use range_grid element
677  */
678  inline int
679  write (const std::string &file_name, const pcl::PCLPointCloud2::ConstPtr &cloud,
680  const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
681  const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
682  bool binary = false,
683  bool use_camera = true)
684  {
685  return (write (file_name, *cloud, origin, orientation, binary, use_camera));
686  }
687 
688  /** \brief Save point cloud data to a PLY file containing n-D points
689  * \param[in] file_name the output file name
690  * \param[in] cloud the pcl::PointCloud data
691  * \param[in] binary set to true if the file is to be written in a binary
692  * PLY format, false (default) for ASCII
693  * \param[in] use_camera set to true to use camera element and false to
694  * use range_grid element
695  */
696  template<typename PointT> inline int
697  write (const std::string &file_name,
698  const pcl::PointCloud<PointT> &cloud,
699  bool binary = false,
700  bool use_camera = true)
701  {
702  Eigen::Vector4f origin = cloud.sensor_origin_;
703  Eigen::Quaternionf orientation = cloud.sensor_orientation_;
704 
705  pcl::PCLPointCloud2 blob;
706  pcl::toPCLPointCloud2 (cloud, blob);
707 
708  // Save the data
709  return (this->write (file_name, blob, origin, orientation, binary, use_camera));
710  }
711 
712  private:
713  /** \brief Generate a PLY header.
714  * \param[in] cloud the input point cloud
715  * \param[in] binary whether the PLY file should be saved as binary data (true) or ascii (false)
716  */
717  std::string
718  generateHeader (const pcl::PCLPointCloud2 &cloud,
719  const Eigen::Vector4f &origin,
720  const Eigen::Quaternionf &orientation,
721  bool binary,
722  bool use_camera,
723  int valid_points);
724 
725  void
726  writeContentWithCameraASCII (int nr_points,
727  int point_size,
728  const pcl::PCLPointCloud2 &cloud,
729  const Eigen::Vector4f &origin,
730  const Eigen::Quaternionf &orientation,
731  std::ofstream& fs);
732 
733  void
734  writeContentWithRangeGridASCII (int nr_points,
735  int point_size,
736  const pcl::PCLPointCloud2 &cloud,
737  std::ostringstream& fs,
738  int& nb_valid_points);
739  };
740 
741  namespace io
742  {
743  /** \brief Load a PLY v.6 file into a templated PointCloud type.
744  *
745  * Any PLY files containing sensor data will generate a warning as a
746  * pcl/PCLPointCloud2 message cannot hold the sensor origin.
747  *
748  * \param[in] file_name the name of the file to load
749  * \param[in] cloud the resultant templated point cloud
750  * \ingroup io
751  */
752  inline int
753  loadPLYFile (const std::string &file_name, pcl::PCLPointCloud2 &cloud)
754  {
755  pcl::PLYReader p;
756  return (p.read (file_name, cloud));
757  }
758 
759  /** \brief Load any PLY file into a templated PointCloud type.
760  * \param[in] file_name the name of the file to load
761  * \param[in] cloud the resultant templated point cloud
762  * \param[in] origin the sensor acquisition origin (only for > PLY_V7 - null if not present)
763  * \param[in] orientation the sensor acquisition orientation if available,
764  * identity if not present
765  * \ingroup io
766  */
767  inline int
768  loadPLYFile (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
769  Eigen::Vector4f &origin, Eigen::Quaternionf &orientation)
770  {
771  pcl::PLYReader p;
772  int ply_version;
773  return (p.read (file_name, cloud, origin, orientation, ply_version));
774  }
775 
776  /** \brief Load any PLY file into a templated PointCloud type
777  * \param[in] file_name the name of the file to load
778  * \param[in] cloud the resultant templated point cloud
779  * \ingroup io
780  */
781  template<typename PointT> inline int
782  loadPLYFile (const std::string &file_name, pcl::PointCloud<PointT> &cloud)
783  {
784  pcl::PLYReader p;
785  return (p.read (file_name, cloud));
786  }
787 
788  /** \brief Load a PLY file into a PolygonMesh type.
789  *
790  * Any PLY files containing sensor data will generate a warning as a
791  * pcl/PolygonMesh message cannot hold the sensor origin.
792  *
793  * \param[in] file_name the name of the file to load
794  * \param[in] mesh the resultant polygon mesh
795  * \ingroup io
796  */
797  inline int
798  loadPLYFile (const std::string &file_name, pcl::PolygonMesh &mesh)
799  {
800  pcl::PLYReader p;
801  return (p.read (file_name, mesh));
802  }
803 
804  /** \brief Save point cloud data to a PLY file containing n-D points
805  * \param[in] file_name the output file name
806  * \param[in] cloud the point cloud data message
807  * \param[in] origin the sensor data acquisition origin (translation)
808  * \param[in] orientation the sensor data acquisition origin (rotation)
809  * \param[in] binary_mode true for binary mode, false (default) for ASCII
810  * \param[in] use_camera
811  * \ingroup io
812  */
813  inline int
814  savePLYFile (const std::string &file_name, const pcl::PCLPointCloud2 &cloud,
815  const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
816  const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
817  bool binary_mode = false, bool use_camera = true)
818  {
819  PLYWriter w;
820  return (w.write (file_name, cloud, origin, orientation, binary_mode, use_camera));
821  }
822 
823  /** \brief Templated version for saving point cloud data to a PLY file
824  * containing a specific given cloud format
825  * \param[in] file_name the output file name
826  * \param[in] cloud the point cloud data message
827  * \param[in] binary_mode true for binary mode, false (default) for ASCII
828  * \ingroup io
829  */
830  template<typename PointT> inline int
831  savePLYFile (const std::string &file_name, const pcl::PointCloud<PointT> &cloud, bool binary_mode = false)
832  {
833  PLYWriter w;
834  return (w.write<PointT> (file_name, cloud, binary_mode));
835  }
836 
837  /** \brief Templated version for saving point cloud data to a PLY file
838  * containing a specific given cloud format.
839  * \param[in] file_name the output file name
840  * \param[in] cloud the point cloud data message
841  * \ingroup io
842  */
843  template<typename PointT> inline int
844  savePLYFileASCII (const std::string &file_name, const pcl::PointCloud<PointT> &cloud)
845  {
846  PLYWriter w;
847  return (w.write<PointT> (file_name, cloud, false));
848  }
849 
850  /** \brief Templated version for saving point cloud data to a PLY file containing a specific given cloud format.
851  * \param[in] file_name the output file name
852  * \param[in] cloud the point cloud data message
853  * \ingroup io
854  */
855  template<typename PointT> inline int
856  savePLYFileBinary (const std::string &file_name, const pcl::PointCloud<PointT> &cloud)
857  {
858  PLYWriter w;
859  return (w.write<PointT> (file_name, cloud, true));
860  }
861 
862  /** \brief Templated version for saving point cloud data to a PLY file containing a specific given cloud format
863  * \param[in] file_name the output file name
864  * \param[in] cloud the point cloud data message
865  * \param[in] indices the set of indices to save
866  * \param[in] binary_mode true for binary mode, false (default) for ASCII
867  * \ingroup io
868  */
869  template<typename PointT> int
870  savePLYFile (const std::string &file_name, const pcl::PointCloud<PointT> &cloud,
871  const std::vector<int> &indices, bool binary_mode = false)
872  {
873  // Copy indices to a new point cloud
874  pcl::PointCloud<PointT> cloud_out;
875  copyPointCloud (cloud, indices, cloud_out);
876  // Save the data
877  PLYWriter w;
878  return (w.write<PointT> (file_name, cloud_out, binary_mode));
879  }
880 
881  /** \brief Saves a PolygonMesh in ascii PLY format.
882  * \param[in] file_name the name of the file to write to disk
883  * \param[in] mesh the polygonal mesh to save
884  * \param[in] precision the output ASCII precision default 5
885  * \ingroup io
886  */
887  PCL_EXPORTS int
888  savePLYFile (const std::string &file_name, const pcl::PolygonMesh &mesh, unsigned precision = 5);
889 
890  /** \brief Saves a PolygonMesh in binary PLY format.
891  * \param[in] file_name the name of the file to write to disk
892  * \param[in] mesh the polygonal mesh to save
893  * \ingroup io
894  */
895  PCL_EXPORTS int
896  savePLYFileBinary (const std::string &file_name, const pcl::PolygonMesh &mesh);
897  }
898 }
899 
900 #endif //#ifndef PCL_IO_PLY_IO_H_
void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud object using a field_map...
Definition: conversions.h:169
PLYReader(const PLYReader &p)
Definition: ply_io.h:102
int write(const std::string &file_name, const pcl::PCLPointCloud2 &cloud, const Eigen::Vector4f &origin=Eigen::Vector4f::Zero(), const Eigen::Quaternionf &orientation=Eigen::Quaternionf::Identity(), const bool binary=false)
Save point cloud data to a PLY file containing n-D points.
Definition: ply_io.h:634
std::string generateHeaderASCII(const pcl::PCLPointCloud2 &cloud, const Eigen::Vector4f &origin, const Eigen::Quaternionf &orientation, int valid_points, bool use_camera=true)
Generate the header of a PLY v.7 file format.
Definition: ply_io.h:586
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
int savePLYFile(const std::string &file_name, const pcl::PCLPointCloud2 &cloud, const Eigen::Vector4f &origin=Eigen::Vector4f::Zero(), const Eigen::Quaternionf &orientation=Eigen::Quaternionf::Identity(), bool binary_mode=false, bool use_camera=true)
Save point cloud data to a PLY file containing n-D points.
Definition: ply_io.h:814
~PLYWriter()
Destructor.
Definition: ply_io.h:555
void read(std::istream &stream, Type &value)
Function for reading data from a stream.
Definition: region_xy.h:47
Point Cloud Data (FILE) file format writer.
Definition: file_io.h:162
PCL_EXPORTS void copyPointCloud(const pcl::PCLPointCloud2 &cloud_in, const std::vector< int > &indices, pcl::PCLPointCloud2 &cloud_out)
Extract the indices of a given point cloud as a new point cloud.
int write(const std::string &file_name, const pcl::PCLPointCloud2::ConstPtr &cloud, const Eigen::Vector4f &origin=Eigen::Vector4f::Zero(), const Eigen::Quaternionf &orientation=Eigen::Quaternionf::Identity(), bool binary=false, bool use_camera=true)
Save point cloud data to a PLY file containing n-D points.
Definition: ply_io.h:679
Definition: bfgs.h:10
int read(const std::string &file_name, pcl::PointCloud< PointT > &cloud, const int offset=0)
Read a point cloud data from any PLY file, and convert it to the given template format.
Definition: ply_io.h:203
std::string generateHeaderBinary(const pcl::PCLPointCloud2 &cloud, const Eigen::Vector4f &origin, const Eigen::Quaternionf &orientation, int valid_points, bool use_camera=true)
Generate the header of a PLY v.7 file format.
Definition: ply_io.h:567
int savePLYFileBinary(const std::string &file_name, const pcl::PointCloud< PointT > &cloud)
Templated version for saving point cloud data to a PLY file containing a specific given cloud format...
Definition: ply_io.h:856
Point Cloud Data (PLY) file format writer.
Definition: ply_io.h:548
int savePLYFileASCII(const std::string &file_name, const pcl::PointCloud< PointT > &cloud)
Templated version for saving point cloud data to a PLY file containing a specific given cloud format...
Definition: ply_io.h:844
Class ply_parser parses a PLY file and generates appropriate atomic parsers for the body...
Definition: ply_parser.h:82
Point Cloud Data (FILE) file format reader interface.
Definition: file_io.h:56
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:423
int read(const std::string &file_name, pcl::PCLPointCloud2 &cloud, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &ply_version, const int offset=0)
Read a point cloud data from a PLY file and store it into a pcl/PCLPointCloud2.
int write(const std::string &file_name, const pcl::PCLPointCloud2 &cloud, const Eigen::Vector4f &origin=Eigen::Vector4f::Zero(), const Eigen::Quaternionf &orientation=Eigen::Quaternionf::Identity(), bool binary=false, bool use_camera=true)
Save point cloud data to a PLY file containing n-D points.
Definition: ply_io.h:656
boost::int32_t int32
Definition: ply.h:61
boost::shared_ptr< ::pcl::PCLPointCloud2 const > ConstPtr
int loadPLYFile(const std::string &file_name, pcl::PCLPointCloud2 &cloud)
Load a PLY v.6 file into a templated PointCloud type.
Definition: ply_io.h:753
PointCloud represents the base class in PCL for storing collections of 3D points. ...
int write(const std::string &file_name, const pcl::PointCloud< PointT > &cloud, bool binary=false, bool use_camera=true)
Save point cloud data to a PLY file containing n-D points.
Definition: ply_io.h:697
void toPCLPointCloud2(const pcl::PointCloud< PointT > &cloud, pcl::PCLPointCloud2 &msg)
Convert a pcl::PointCloud object to a PCLPointCloud2 binary data blob.
Definition: conversions.h:242
int read(const std::string &file_name, pcl::PCLPointCloud2 &cloud, const int offset=0)
Read a point cloud data from a PLY file and store it into a pcl/PCLPointCloud2.
Definition: ply_io.h:185
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:421
Point Cloud Data (PLY) file format reader.
Definition: ply_io.h:77
void write(std::ostream &stream, Type value)
Function for writing data to a stream.
Definition: region_xy.h:64
boost::uint8_t uint8
Definition: ply.h:62
A point structure representing Euclidean xyz coordinates, and the RGB color.
PLYWriter()
Constructor.
Definition: ply_io.h:552