Point Cloud Library (PCL)  1.9.1
pcd_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_PCD_IO_H_
41 #define PCL_IO_PCD_IO_H_
42 
43 #include <pcl/point_cloud.h>
44 #include <pcl/io/file_io.h>
45 
46 namespace pcl
47 {
48  /** \brief Point Cloud Data (PCD) file format reader.
49  * \author Radu B. Rusu
50  * \ingroup io
51  */
52  class PCL_EXPORTS PCDReader : public FileReader
53  {
54  public:
55  /** Empty constructor */
56  PCDReader () : FileReader () {}
57  /** Empty destructor */
58  ~PCDReader () {}
59 
60  /** \brief Various PCD file versions.
61  *
62  * PCD_V6 represents PCD files with version 0.6, which contain the following fields:
63  * - lines beginning with # are treated as comments
64  * - FIELDS ...
65  * - SIZE ...
66  * - TYPE ...
67  * - COUNT ...
68  * - WIDTH ...
69  * - HEIGHT ...
70  * - POINTS ...
71  * - DATA ascii/binary
72  *
73  * Everything that follows \b DATA is interpreted as data points and
74  * will be read accordingly.
75  *
76  * PCD_V7 represents PCD files with version 0.7 and has an important
77  * addon: it adds sensor origin/orientation (aka viewpoint) information
78  * to a dataset through the use of a new header field:
79  * - VIEWPOINT tx ty tz qw qx qy qz
80  */
81  enum
82  {
83  PCD_V6 = 0,
84  PCD_V7 = 1
85  };
86 
87  /** \brief Read a point cloud data header from a PCD-formatted, binary istream.
88  *
89  * Load only the meta information (number of points, their types, etc),
90  * and not the points themselves, from a given PCD stream. Useful for fast
91  * evaluation of the underlying data structure.
92  *
93  * \attention The PCD data is \b always stored in ROW major format! The
94  * read/write PCD methods will detect column major input and automatically convert it.
95  *
96  * \param[in] binary_istream a std::istream with openmode set to std::ios::binary.
97  * \param[out] cloud the resultant point cloud dataset (only these
98  * members will be filled: width, height, point_step,
99  * row_step, fields[]; data is resized but not written)
100  * \param[out] origin the sensor acquisition origin (only for > PCD_V7 - null if not present)
101  * \param[out] orientation the sensor acquisition orientation (only for > PCD_V7 - identity if not present)
102  * \param[out] pcd_version the PCD version of the file (i.e., PCD_V6, PCD_V7)
103  * \param[out] data_type the type of data (0 = ASCII, 1 = Binary, 2 = Binary compressed)
104  * \param[out] data_idx the offset of cloud data within the file
105  *
106  * \return
107  * * < 0 (-1) on error
108  * * == 0 on success
109  */
110  int
111  readHeader (std::istream &binary_istream, pcl::PCLPointCloud2 &cloud,
112  Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &pcd_version,
113  int &data_type, unsigned int &data_idx);
114 
115  /** \brief Read a point cloud data header from a PCD file.
116  *
117  * Load only the meta information (number of points, their types, etc),
118  * and not the points themselves, from a given PCD file. Useful for fast
119  * evaluation of the underlying data structure.
120  *
121  * \attention The PCD data is \b always stored in ROW major format! The
122  * read/write PCD methods will detect column major input and automatically convert it.
123  *
124  * \param[in] file_name the name of the file to load
125  * \param[out] cloud the resultant point cloud dataset (only these
126  * members will be filled: width, height, point_step,
127  * row_step, fields[]; data is resized but not written)
128  * \param[out] origin the sensor acquisition origin (only for > PCD_V7 - null if not present)
129  * \param[out] orientation the sensor acquisition orientation (only for > PCD_V7 - identity if not present)
130  * \param[out] pcd_version the PCD version of the file (i.e., PCD_V6, PCD_V7)
131  * \param[out] data_type the type of data (0 = ASCII, 1 = Binary, 2 = Binary compressed)
132  * \param[out] data_idx the offset of cloud data within the file
133  * \param[in] offset the offset of where to expect the PCD Header in the
134  * file (optional parameter). One usage example for setting the offset
135  * parameter is for reading data from a TAR "archive containing multiple
136  * PCD files: TAR files always add a 512 byte header in front of the
137  * actual file, so set the offset to the next byte after the header
138  * (e.g., 513).
139  *
140  * \return
141  * * < 0 (-1) on error
142  * * == 0 on success
143  */
144  int
145  readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
146  Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &pcd_version,
147  int &data_type, unsigned int &data_idx, const int offset = 0);
148 
149 
150  /** \brief Read a point cloud data header from a PCD file.
151  *
152  * Load only the meta information (number of points, their types, etc),
153  * and not the points themselves, from a given PCD file. Useful for fast
154  * evaluation of the underlying data structure.
155  *
156  * \attention The PCD data is \b always stored in ROW major format! The
157  * read/write PCD methods will detect column major input and automatically convert it.
158  *
159  * \param[in] file_name the name of the file to load
160  * \param[out] cloud the resultant point cloud dataset (only these
161  * members will be filled: width, height, point_step,
162  * row_step, fields[]; data is resized but not written)
163  * \param[in] offset the offset of where to expect the PCD Header in the
164  * file (optional parameter). One usage example for setting the offset
165  * parameter is for reading data from a TAR "archive containing multiple
166  * PCD files: TAR files always add a 512 byte header in front of the
167  * actual file, so set the offset to the next byte after the header
168  * (e.g., 513).
169  *
170  * \return
171  * * < 0 (-1) on error
172  * * == 0 on success
173  */
174  int
175  readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud, const int offset = 0);
176 
177  /** \brief Read the point cloud data (body) from a PCD stream.
178  *
179  * Reads the cloud points from a text-formatted stream. For use after
180  * readHeader(), when the resulting data_type == 0.
181  *
182  * \attention This assumes the stream has been seeked to the position
183  * indicated by the data_idx result of readHeader().
184  *
185  * \param[in] stream the stream from which to read the body.
186  * \param[out] cloud the resultant point cloud dataset to be filled.
187  * \param[in] pcd_version the PCD version of the stream (from readHeader()).
188  *
189  * \return
190  * * < 0 (-1) on error
191  * * == 0 on success
192  */
193  int
194  readBodyASCII (std::istream &stream, pcl::PCLPointCloud2 &cloud, int pcd_version);
195 
196  /** \brief Read the point cloud data (body) from a block of memory.
197  *
198  * Reads the cloud points from a binary-formatted memory block. For use
199  * after readHeader(), when the resulting data_type is nonzero.
200  *
201  * \param[in] data the memory location from which to read the body.
202  * \param[out] cloud the resultant point cloud dataset to be filled.
203  * \param[in] pcd_version the PCD version of the stream (from readHeader()).
204  * \param[in] compressed indicates whether the PCD block contains compressed
205  * data. This should be true if the data_type returne by readHeader() == 2.
206  * \param[in] data_idx the offset of the body, as reported by readHeader().
207  *
208  * \return
209  * * < 0 (-1) on error
210  * * == 0 on success
211  */
212  int
213  readBodyBinary (const unsigned char *data, pcl::PCLPointCloud2 &cloud,
214  int pcd_version, bool compressed, unsigned int data_idx);
215 
216  /** \brief Read a point cloud data from a PCD file and store it into a pcl/PCLPointCloud2.
217  * \param[in] file_name the name of the file containing the actual PointCloud data
218  * \param[out] cloud the resultant PointCloud message read from disk
219  * \param[out] origin the sensor acquisition origin (only for > PCD_V7 - null if not present)
220  * \param[out] orientation the sensor acquisition orientation (only for > PCD_V7 - identity if not present)
221  * \param[out] pcd_version the PCD version of the file (either PCD_V6 or PCD_V7)
222  * \param[in] offset the offset of where to expect the PCD Header in the
223  * file (optional parameter). One usage example for setting the offset
224  * parameter is for reading data from a TAR "archive containing multiple
225  * PCD files: TAR files always add a 512 byte header in front of the
226  * actual file, so set the offset to the next byte after the header
227  * (e.g., 513).
228  *
229  * \return
230  * * < 0 (-1) on error
231  * * == 0 on success
232  */
233  int
234  read (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
235  Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &pcd_version, const int offset = 0);
236 
237  /** \brief Read a point cloud data from a PCD (PCD_V6) and store it into a pcl/PCLPointCloud2.
238  *
239  * \note This function is provided for backwards compatibility only and
240  * it can only read PCD_V6 files correctly, as pcl::PCLPointCloud2
241  * does not contain a sensor origin/orientation. Reading any file
242  * > PCD_V6 will generate a warning.
243  *
244  * \param[in] file_name the name of the file containing the actual PointCloud data
245  * \param[out] cloud the resultant PointCloud message read from disk
246  * \param[in] offset the offset of where to expect the PCD Header in the
247  * file (optional parameter). One usage example for setting the offset
248  * parameter is for reading data from a TAR "archive containing multiple
249  * PCD files: TAR files always add a 512 byte header in front of the
250  * actual file, so set the offset to the next byte after the header
251  * (e.g., 513).
252  *
253  * \return
254  * * < 0 (-1) on error
255  * * == 0 on success
256  */
257  int
258  read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, const int offset = 0);
259 
260  /** \brief Read a point cloud data from any PCD file, and convert it to the given template format.
261  * \param[in] file_name the name of the file containing the actual PointCloud data
262  * \param[out] cloud the resultant PointCloud message read from disk
263  * \param[in] offset the offset of where to expect the PCD Header in the
264  * file (optional parameter). One usage example for setting the offset
265  * parameter is for reading data from a TAR "archive containing multiple
266  * PCD files: TAR files always add a 512 byte header in front of the
267  * actual file, so set the offset to the next byte after the header
268  * (e.g., 513).
269  *
270  * \return
271  * * < 0 (-1) on error
272  * * == 0 on success
273  */
274  template<typename PointT> int
275  read (const std::string &file_name, pcl::PointCloud<PointT> &cloud, const int offset = 0)
276  {
277  pcl::PCLPointCloud2 blob;
278  int pcd_version;
279  int res = read (file_name, blob, cloud.sensor_origin_, cloud.sensor_orientation_,
280  pcd_version, offset);
281 
282  // If no error, convert the data
283  if (res == 0)
284  pcl::fromPCLPointCloud2 (blob, cloud);
285  return (res);
286  }
287 
288  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
289  };
290 
291  /** \brief Point Cloud Data (PCD) file format writer.
292  * \author Radu Bogdan Rusu
293  * \ingroup io
294  */
295  class PCL_EXPORTS PCDWriter : public FileWriter
296  {
297  public:
298  PCDWriter() : FileWriter(), map_synchronization_(false) {}
300 
301  /** \brief Set whether mmap() synchornization via msync() is desired before munmap() calls.
302  * Setting this to true could prevent NFS data loss (see
303  * http://www.pcl-developers.org/PCD-IO-consistency-on-NFS-msync-needed-td4885942.html).
304  * Default: false
305  * \note This option should be used by advanced users only!
306  * \note Please note that using msync() on certain systems can reduce the I/O performance by up to 80%!
307  * \param[in] sync set to true if msync() should be called before munmap()
308  */
309  void
311  {
312  map_synchronization_ = sync;
313  }
314 
315  /** \brief Generate the header of a PCD file format
316  * \param[in] cloud the point cloud data message
317  * \param[in] origin the sensor acquisition origin
318  * \param[in] orientation the sensor acquisition orientation
319  */
320  std::string
321  generateHeaderBinary (const pcl::PCLPointCloud2 &cloud,
322  const Eigen::Vector4f &origin,
323  const Eigen::Quaternionf &orientation);
324 
325  /** \brief Generate the header of a BINARY_COMPRESSED PCD file format
326  * \param[out] os the stream into which to write the header
327  * \param[in] cloud the point cloud data message
328  * \param[in] origin the sensor acquisition origin
329  * \param[in] orientation the sensor acquisition orientation
330  *
331  * \return
332  * * < 0 (-1) on error
333  * * == 0 on success
334  */
335  int
336  generateHeaderBinaryCompressed (std::ostream &os,
337  const pcl::PCLPointCloud2 &cloud,
338  const Eigen::Vector4f &origin,
339  const Eigen::Quaternionf &orientation);
340 
341  /** \brief Generate the header of a BINARY_COMPRESSED PCD file format
342  * \param[out] os the stream into which to write the header
343  * \param[in] cloud the point cloud data message
344  * \param[in] origin the sensor acquisition origin
345  * \param[in] orientation the sensor acquisition orientation
346  */
347  std::string
348  generateHeaderBinaryCompressed (const pcl::PCLPointCloud2 &cloud,
349  const Eigen::Vector4f &origin,
350  const Eigen::Quaternionf &orientation);
351 
352  /** \brief Generate the header of a PCD file format
353  * \param[in] cloud the point cloud data message
354  * \param[in] origin the sensor acquisition origin
355  * \param[in] orientation the sensor acquisition orientation
356  */
357  std::string
358  generateHeaderASCII (const pcl::PCLPointCloud2 &cloud,
359  const Eigen::Vector4f &origin,
360  const Eigen::Quaternionf &orientation);
361 
362  /** \brief Generate the header of a PCD file format
363  * \param[in] cloud the point cloud data message
364  * \param[in] nr_points if given, use this to fill in WIDTH, HEIGHT (=1), and POINTS in the header
365  * By default, nr_points is set to INTMAX, and the data in the header is used instead.
366  */
367  template <typename PointT> static std::string
368  generateHeader (const pcl::PointCloud<PointT> &cloud,
369  const int nr_points = std::numeric_limits<int>::max ());
370 
371  /** \brief Save point cloud data to a PCD file containing n-D points, in ASCII format
372  * \param[in] file_name the output file name
373  * \param[in] cloud the point cloud data message
374  * \param[in] origin the sensor acquisition origin
375  * \param[in] orientation the sensor acquisition orientation
376  * \param[in] precision the specified output numeric stream precision (default: 8)
377  *
378  * Caution: PointCloud structures containing an RGB field have
379  * traditionally used packed float values to store RGB data. Storing a
380  * float as ASCII can introduce variations to the smallest bits, and
381  * thus significantly alter the data. This is a known issue, and the fix
382  * involves switching RGB data to be stored as a packed integer in
383  * future versions of PCL.
384  *
385  * As an intermediary solution, precision 8 is used, which guarantees lossless storage for RGB.
386  */
387  int
388  writeASCII (const std::string &file_name, const pcl::PCLPointCloud2 &cloud,
389  const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
390  const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
391  const int precision = 8);
392 
393  /** \brief Save point cloud data to a PCD file containing n-D points, in BINARY format
394  * \param[in] file_name the output file name
395  * \param[in] cloud the point cloud data message
396  * \param[in] origin the sensor acquisition origin
397  * \param[in] orientation the sensor acquisition orientation
398  */
399  int
400  writeBinary (const std::string &file_name, const pcl::PCLPointCloud2 &cloud,
401  const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
402  const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity ());
403 
404  /** \brief Save point cloud data to a PCD file containing n-D points, in BINARY_COMPRESSED format
405  * \param[in] file_name the output file name
406  * \param[in] cloud the point cloud data message
407  * \param[in] origin the sensor acquisition origin
408  * \param[in] orientation the sensor acquisition orientation
409  * \return
410  * (-1) for a general error
411  * (-2) if the input cloud is too large for the file format
412  * 0 on success
413  */
414  int
415  writeBinaryCompressed (const std::string &file_name, const pcl::PCLPointCloud2 &cloud,
416  const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
417  const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity ());
418 
419  /** \brief Save point cloud data to a std::ostream containing n-D points, in BINARY_COMPRESSED format
420  * \param[out] os the stream into which to write the data
421  * \param[in] cloud the point cloud data message
422  * \param[in] origin the sensor acquisition origin
423  * \param[in] orientation the sensor acquisition orientation
424  * \return
425  * (-1) for a general error
426  * (-2) if the input cloud is too large for the file format
427  * 0 on success
428  */
429  int
430  writeBinaryCompressed (std::ostream &os, const pcl::PCLPointCloud2 &cloud,
431  const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
432  const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity ());
433 
434  /** \brief Save point cloud data to a PCD file containing n-D points
435  * \param[in] file_name the output file name
436  * \param[in] cloud the point cloud data message
437  * \param[in] origin the sensor acquisition origin
438  * \param[in] orientation the sensor acquisition orientation
439  * \param[in] binary set to true if the file is to be written in a binary
440  * PCD format, false (default) for ASCII
441  *
442  * Caution: PointCloud structures containing an RGB field have
443  * traditionally used packed float values to store RGB data. Storing a
444  * float as ASCII can introduce variations to the smallest bits, and
445  * thus significantly alter the data. This is a known issue, and the fix
446  * involves switching RGB data to be stored as a packed integer in
447  * future versions of PCL.
448  *
449  * As an intermediary solution, precision 8 is used, which guarantees lossless storage for RGB.
450  */
451  inline int
452  write (const std::string &file_name, const pcl::PCLPointCloud2 &cloud,
453  const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
454  const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
455  const bool binary = false)
456  {
457  if (binary)
458  return (writeBinary (file_name, cloud, origin, orientation));
459  else
460  return (writeASCII (file_name, cloud, origin, orientation, 8));
461  }
462 
463  /** \brief Save point cloud data to a PCD file containing n-D points
464  * \param[in] file_name the output file name
465  * \param[in] cloud the point cloud data message (boost shared pointer)
466  * \param[in] binary set to true if the file is to be written in a binary PCD format,
467  * false (default) for ASCII
468  * \param[in] origin the sensor acquisition origin
469  * \param[in] orientation the sensor acquisition orientation
470  *
471  * Caution: PointCloud structures containing an RGB field have
472  * traditionally used packed float values to store RGB data. Storing a
473  * float as ASCII can introduce variations to the smallest bits, and
474  * thus significantly alter the data. This is a known issue, and the fix
475  * involves switching RGB data to be stored as a packed integer in
476  * future versions of PCL.
477  */
478  inline int
479  write (const std::string &file_name, const pcl::PCLPointCloud2::ConstPtr &cloud,
480  const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
481  const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
482  const bool binary = false)
483  {
484  return (write (file_name, *cloud, origin, orientation, binary));
485  }
486 
487  /** \brief Save point cloud data to a PCD file containing n-D points, in BINARY format
488  * \param[in] file_name the output file name
489  * \param[in] cloud the point cloud data message
490  */
491  template <typename PointT> int
492  writeBinary (const std::string &file_name,
493  const pcl::PointCloud<PointT> &cloud);
494 
495  /** \brief Save point cloud data to a binary comprssed PCD file
496  * \param[in] file_name the output file name
497  * \param[in] cloud the point cloud data message
498  * \return
499  * (-1) for a general error
500  * (-2) if the input cloud is too large for the file format
501  * 0 on success
502  */
503  template <typename PointT> int
504  writeBinaryCompressed (const std::string &file_name,
505  const pcl::PointCloud<PointT> &cloud);
506 
507  /** \brief Save point cloud data to a PCD file containing n-D points, in BINARY format
508  * \param[in] file_name the output file name
509  * \param[in] cloud the point cloud data message
510  * \param[in] indices the set of point indices that we want written to disk
511  */
512  template <typename PointT> int
513  writeBinary (const std::string &file_name,
514  const pcl::PointCloud<PointT> &cloud,
515  const std::vector<int> &indices);
516 
517  /** \brief Save point cloud data to a PCD file containing n-D points, in ASCII format
518  * \param[in] file_name the output file name
519  * \param[in] cloud the point cloud data message
520  * \param[in] precision the specified output numeric stream precision (default: 8)
521  */
522  template <typename PointT> int
523  writeASCII (const std::string &file_name,
524  const pcl::PointCloud<PointT> &cloud,
525  const int precision = 8);
526 
527  /** \brief Save point cloud data to a PCD file containing n-D points, in ASCII format
528  * \param[in] file_name the output file name
529  * \param[in] cloud the point cloud data message
530  * \param[in] indices the set of point indices that we want written to disk
531  * \param[in] precision the specified output numeric stream precision (default: 8)
532  */
533  template <typename PointT> int
534  writeASCII (const std::string &file_name,
535  const pcl::PointCloud<PointT> &cloud,
536  const std::vector<int> &indices,
537  const int precision = 8);
538 
539  /** \brief Save point cloud data to a PCD file containing n-D points
540  * \param[in] file_name the output file name
541  * \param[in] cloud the pcl::PointCloud data
542  * \param[in] binary set to true if the file is to be written in a binary
543  * PCD format, false (default) for ASCII
544  *
545  * Caution: PointCloud structures containing an RGB field have
546  * traditionally used packed float values to store RGB data. Storing a
547  * float as ASCII can introduce variations to the smallest bits, and
548  * thus significantly alter the data. This is a known issue, and the fix
549  * involves switching RGB data to be stored as a packed integer in
550  * future versions of PCL.
551  */
552  template<typename PointT> inline int
553  write (const std::string &file_name,
554  const pcl::PointCloud<PointT> &cloud,
555  const bool binary = false)
556  {
557  if (binary)
558  return (writeBinary<PointT> (file_name, cloud));
559  else
560  return (writeASCII<PointT> (file_name, cloud));
561  }
562 
563  /** \brief Save point cloud data to a PCD file containing n-D points
564  * \param[in] file_name the output file name
565  * \param[in] cloud the pcl::PointCloud data
566  * \param[in] indices the set of point indices that we want written to disk
567  * \param[in] binary set to true if the file is to be written in a binary
568  * PCD format, false (default) for ASCII
569  *
570  * Caution: PointCloud structures containing an RGB field have
571  * traditionally used packed float values to store RGB data. Storing a
572  * float as ASCII can introduce variations to the smallest bits, and
573  * thus significantly alter the data. This is a known issue, and the fix
574  * involves switching RGB data to be stored as a packed integer in
575  * future versions of PCL.
576  */
577  template<typename PointT> inline int
578  write (const std::string &file_name,
579  const pcl::PointCloud<PointT> &cloud,
580  const std::vector<int> &indices,
581  bool binary = false)
582  {
583  if (binary)
584  return (writeBinary<PointT> (file_name, cloud, indices));
585  else
586  return (writeASCII<PointT> (file_name, cloud, indices));
587  }
588 
589  protected:
590  /** \brief Set permissions for file locking (Boost 1.49+).
591  * \param[in] file_name the file name to set permission for file locking
592  * \param[in,out] lock the file lock
593  */
594  void
595  setLockingPermissions (const std::string &file_name,
596  boost::interprocess::file_lock &lock);
597 
598  /** \brief Reset permissions for file locking (Boost 1.49+).
599  * \param[in] file_name the file name to reset permission for file locking
600  * \param[in,out] lock the file lock
601  */
602  void
603  resetLockingPermissions (const std::string &file_name,
604  boost::interprocess::file_lock &lock);
605 
606  private:
607  /** \brief Set to true if msync() should be called before munmap(). Prevents data loss on NFS systems. */
608  bool map_synchronization_;
609  };
610 
611  namespace io
612  {
613  /** \brief Load a PCD v.6 file into a templated PointCloud type.
614  *
615  * Any PCD files > v.6 will generate a warning as a
616  * pcl/PCLPointCloud2 message cannot hold the sensor origin.
617  *
618  * \param[in] file_name the name of the file to load
619  * \param[out] cloud the resultant templated point cloud
620  * \ingroup io
621  */
622  inline int
623  loadPCDFile (const std::string &file_name, pcl::PCLPointCloud2 &cloud)
624  {
625  pcl::PCDReader p;
626  return (p.read (file_name, cloud));
627  }
628 
629  /** \brief Load any PCD file into a templated PointCloud type.
630  * \param[in] file_name the name of the file to load
631  * \param[out] cloud the resultant templated point cloud
632  * \param[out] origin the sensor acquisition origin (only for > PCD_V7 - null if not present)
633  * \param[out] orientation the sensor acquisition orientation (only for >
634  * PCD_V7 - identity if not present)
635  * \ingroup io
636  */
637  inline int
638  loadPCDFile (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
639  Eigen::Vector4f &origin, Eigen::Quaternionf &orientation)
640  {
641  pcl::PCDReader p;
642  int pcd_version;
643  return (p.read (file_name, cloud, origin, orientation, pcd_version));
644  }
645 
646  /** \brief Load any PCD file into a templated PointCloud type
647  * \param[in] file_name the name of the file to load
648  * \param[out] cloud the resultant templated point cloud
649  * \ingroup io
650  */
651  template<typename PointT> inline int
652  loadPCDFile (const std::string &file_name, pcl::PointCloud<PointT> &cloud)
653  {
654  pcl::PCDReader p;
655  return (p.read (file_name, cloud));
656  }
657 
658  /** \brief Save point cloud data to a PCD file containing n-D points
659  * \param[in] file_name the output file name
660  * \param[in] cloud the point cloud data message
661  * \param[in] origin the sensor acquisition origin
662  * \param[in] orientation the sensor acquisition orientation
663  * \param[in] binary_mode true for binary mode, false (default) for ASCII
664  *
665  * Caution: PointCloud structures containing an RGB field have
666  * traditionally used packed float values to store RGB data. Storing a
667  * float as ASCII can introduce variations to the smallest bits, and
668  * thus significantly alter the data. This is a known issue, and the fix
669  * involves switching RGB data to be stored as a packed integer in
670  * future versions of PCL.
671  * \ingroup io
672  */
673  inline int
674  savePCDFile (const std::string &file_name, const pcl::PCLPointCloud2 &cloud,
675  const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
676  const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
677  const bool binary_mode = false)
678  {
679  PCDWriter w;
680  return (w.write (file_name, cloud, origin, orientation, binary_mode));
681  }
682 
683  /** \brief Templated version for saving point cloud data to a PCD file
684  * containing a specific given cloud format
685  * \param[in] file_name the output file name
686  * \param[in] cloud the point cloud data message
687  * \param[in] binary_mode true for binary mode, false (default) for ASCII
688  *
689  * Caution: PointCloud structures containing an RGB field have
690  * traditionally used packed float values to store RGB data. Storing a
691  * float as ASCII can introduce variations to the smallest bits, and
692  * thus significantly alter the data. This is a known issue, and the fix
693  * involves switching RGB data to be stored as a packed integer in
694  * future versions of PCL.
695  * \ingroup io
696  */
697  template<typename PointT> inline int
698  savePCDFile (const std::string &file_name, const pcl::PointCloud<PointT> &cloud, bool binary_mode = false)
699  {
700  PCDWriter w;
701  return (w.write<PointT> (file_name, cloud, binary_mode));
702  }
703 
704  /**
705  * \brief Templated version for saving point cloud data to a PCD file
706  * containing a specific given cloud format.
707  *
708  * This version is to retain backwards compatibility.
709  * \param[in] file_name the output file name
710  * \param[in] cloud the point cloud data message
711  *
712  * Caution: PointCloud structures containing an RGB field have
713  * traditionally used packed float values to store RGB data. Storing a
714  * float as ASCII can introduce variations to the smallest bits, and
715  * thus significantly alter the data. This is a known issue, and the fix
716  * involves switching RGB data to be stored as a packed integer in
717  * future versions of PCL.
718  * \ingroup io
719  */
720  template<typename PointT> inline int
721  savePCDFileASCII (const std::string &file_name, const pcl::PointCloud<PointT> &cloud)
722  {
723  PCDWriter w;
724  return (w.write<PointT> (file_name, cloud, false));
725  }
726 
727  /**
728  * \brief Templated version for saving point cloud data to a PCD file
729  * containing a specific given cloud format. The resulting file will be an uncompressed binary.
730  *
731  * This version is to retain backwards compatibility.
732  * \param[in] file_name the output file name
733  * \param[in] cloud the point cloud data message
734  * \ingroup io
735  */
736  template<typename PointT> inline int
737  savePCDFileBinary (const std::string &file_name, const pcl::PointCloud<PointT> &cloud)
738  {
739  PCDWriter w;
740  return (w.write<PointT> (file_name, cloud, true));
741  }
742 
743  /**
744  * \brief Templated version for saving point cloud data to a PCD file
745  * containing a specific given cloud format
746  *
747  * \param[in] file_name the output file name
748  * \param[in] cloud the point cloud data message
749  * \param[in] indices the set of indices to save
750  * \param[in] binary_mode true for binary mode, false (default) for ASCII
751  *
752  * Caution: PointCloud structures containing an RGB field have
753  * traditionally used packed float values to store RGB data. Storing a
754  * float as ASCII can introduce variations to the smallest bits, and
755  * thus significantly alter the data. This is a known issue, and the fix
756  * involves switching RGB data to be stored as a packed integer in
757  * future versions of PCL.
758  * \ingroup io
759  */
760  template<typename PointT> int
761  savePCDFile (const std::string &file_name,
762  const pcl::PointCloud<PointT> &cloud,
763  const std::vector<int> &indices,
764  const bool binary_mode = false)
765  {
766  // Save the data
767  PCDWriter w;
768  return (w.write<PointT> (file_name, cloud, indices, binary_mode));
769  }
770 
771 
772  /**
773  * \brief Templated version for saving point cloud data to a PCD file
774  * containing a specific given cloud format. This method will write a compressed binary file.
775  *
776  * This version is to retain backwards compatibility.
777  * \param[in] file_name the output file name
778  * \param[in] cloud the point cloud data message
779  * \ingroup io
780  */
781  template<typename PointT> inline int
782  savePCDFileBinaryCompressed (const std::string &file_name, const pcl::PointCloud<PointT> &cloud)
783  {
784  PCDWriter w;
785  return (w.writeBinaryCompressed<PointT> (file_name, cloud));
786  }
787 
788  }
789 }
790 
791 #include <pcl/io/impl/pcd_io.hpp>
792 
793 #endif //#ifndef PCL_IO_PCD_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<T> object using a field_map.
Definition: conversions.h:169
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(), const bool binary=false)
Save point cloud data to a PCD file containing n-D points.
Definition: pcd_io.h:479
int writeBinaryCompressed(const std::string &file_name, const pcl::PCLPointCloud2 &cloud, const Eigen::Vector4f &origin=Eigen::Vector4f::Zero(), const Eigen::Quaternionf &orientation=Eigen::Quaternionf::Identity())
Save point cloud data to a PCD file containing n-D points, in BINARY_COMPRESSED format.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
~PCDReader()
Empty destructor.
Definition: pcd_io.h:58
int savePCDFileASCII(const std::string &file_name, const pcl::PointCloud< PointT > &cloud)
Templated version for saving point cloud data to a PCD file containing a specific given cloud format.
Definition: pcd_io.h:721
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
void setMapSynchronization(bool sync)
Set whether mmap() synchornization via msync() is desired before munmap() calls.
Definition: pcd_io.h:310
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:421
int savePCDFile(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_mode=false)
Save point cloud data to a PCD file containing n-D points.
Definition: pcd_io.h:674
Point Cloud Data (FILE) file format reader interface.
Definition: file_io.h:56
int savePCDFileBinary(const std::string &file_name, const pcl::PointCloud< PointT > &cloud)
Templated version for saving point cloud data to a PCD file containing a specific given cloud format.
Definition: pcd_io.h:737
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:423
boost::shared_ptr< ::pcl::PCLPointCloud2 const > ConstPtr
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 PCD file containing n-D points.
Definition: pcd_io.h:452
PCDReader()
Empty constructor.
Definition: pcd_io.h:56
PointCloud represents the base class in PCL for storing collections of 3D points.
int loadPCDFile(const std::string &file_name, pcl::PCLPointCloud2 &cloud)
Load a PCD v.6 file into a templated PointCloud type.
Definition: pcd_io.h:623
int write(const std::string &file_name, const pcl::PointCloud< PointT > &cloud, const std::vector< int > &indices, bool binary=false)
Save point cloud data to a PCD file containing n-D points.
Definition: pcd_io.h:578
int write(const std::string &file_name, const pcl::PointCloud< PointT > &cloud, const bool binary=false)
Save point cloud data to a PCD file containing n-D points.
Definition: pcd_io.h:553
void write(std::ostream &stream, Type value)
Function for writing data to a stream.
Definition: region_xy.h:64
int read(const std::string &file_name, pcl::PointCloud< PointT > &cloud, const int offset=0)
Read a point cloud data from any PCD file, and convert it to the given template format.
Definition: pcd_io.h:275
Point Cloud Data (PCD) file format reader.
Definition: pcd_io.h:52
A point structure representing Euclidean xyz coordinates, and the RGB color.
Point Cloud Data (PCD) file format writer.
Definition: pcd_io.h:295
int savePCDFileBinaryCompressed(const std::string &file_name, const pcl::PointCloud< PointT > &cloud)
Templated version for saving point cloud data to a PCD file containing a specific given cloud format.
Definition: pcd_io.h:782
int read(const std::string &file_name, pcl::PCLPointCloud2 &cloud, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &pcd_version, const int offset=0)
Read a point cloud data from a PCD file and store it into a pcl/PCLPointCloud2.