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