Point Cloud Library (PCL)  1.7.2
file_io.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder(s) nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  * $Id: file_io.h 827 2011-05-04 02:00:04Z nizar $
35  *
36  */
37 
38 #ifndef PCL_IO_FILE_IO_H_
39 #define PCL_IO_FILE_IO_H_
40 
41 #include <pcl/pcl_macros.h>
42 #include <pcl/common/io.h>
43 #include <pcl/io/boost.h>
44 #include <cmath>
45 #include <sstream>
46 #include <pcl/PolygonMesh.h>
47 #include <pcl/TextureMesh.h>
48 
49 namespace pcl
50 {
51  /** \brief Point Cloud Data (FILE) file format reader interface.
52  * Any (FILE) format file reader should implement its virtual methodes.
53  * \author Nizar Sallem
54  * \ingroup io
55  */
56  class PCL_EXPORTS FileReader
57  {
58  public:
59  /** \brief empty constructor */
61  /** \brief empty destructor */
62  virtual ~FileReader() {}
63  /** \brief Read a point cloud data header from a FILE file.
64  *
65  * Load only the meta information (number of points, their types, etc),
66  * and not the points themselves, from a given FILE file. Useful for fast
67  * evaluation of the underlying data structure.
68  *
69  * Returns:
70  * * < 0 (-1) on error
71  * * > 0 on success
72  * \param[in] file_name the name of the file to load
73  * \param[out] cloud the resultant point cloud dataset (only the header will be filled)
74  * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present)
75  * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present)
76  * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7)
77  * \param[out] data_type the type of data (binary data=1, ascii=0, etc)
78  * \param[out] data_idx the offset of cloud data within the file
79  * \param[in] offset the offset in the file where to expect the true header to begin.
80  * One usage example for setting the offset parameter is for reading
81  * data from a TAR "archive containing multiple files: TAR files always
82  * add a 512 byte header in front of the actual file, so set the offset
83  * to the next byte after the header (e.g., 513).
84  */
85  virtual int
86  readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
87  Eigen::Vector4f &origin, Eigen::Quaternionf &orientation,
88  int &file_version, int &data_type, unsigned int &data_idx, const int offset = 0) = 0;
89 
90  /** \brief Read a point cloud data from a FILE file and store it into a pcl/PCLPointCloud2.
91  * \param[in] file_name the name of the file containing the actual PointCloud data
92  * \param[out] cloud the resultant PointCloud message read from disk
93  * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present)
94  * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present)
95  * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7)
96  * \param[in] offset the offset in the file where to expect the true header to begin.
97  * One usage example for setting the offset parameter is for reading
98  * data from a TAR "archive containing multiple files: TAR files always
99  * add a 512 byte header in front of the actual file, so set the offset
100  * to the next byte after the header (e.g., 513).
101  */
102  virtual int
103  read (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
104  Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &file_version,
105  const int offset = 0) = 0;
106 
107  /** \brief Read a point cloud data from a FILE file (FILE_V6 only!) and store it into a pcl/PCLPointCloud2.
108  *
109  * \note This function is provided for backwards compatibility only and
110  * it can only read FILE_V6 files correctly, as pcl::PCLPointCloud2
111  * does not contain a sensor origin/orientation. Reading any file
112  * > FILE_V6 will generate a warning.
113  *
114  * \param[in] file_name the name of the file containing the actual PointCloud data
115  * \param[out] cloud the resultant PointCloud message read from disk
116  *
117  * \param[in] offset the offset in the file where to expect the true header to begin.
118  * One usage example for setting the offset parameter is for reading
119  * data from a TAR "archive containing multiple files: TAR files always
120  * add a 512 byte header in front of the actual file, so set the offset
121  * to the next byte after the header (e.g., 513).
122  */
123  int
124  read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, const int offset = 0)
125  {
126  Eigen::Vector4f origin;
127  Eigen::Quaternionf orientation;
128  int file_version;
129  return (read (file_name, cloud, origin, orientation, file_version, offset));
130  }
131 
132  /** \brief Read a point cloud data from any FILE file, and convert it to the given template format.
133  * \param[in] file_name the name of the file containing the actual PointCloud data
134  * \param[out] cloud the resultant PointCloud message read from disk
135  * \param[in] offset the offset in the file where to expect the true header to begin.
136  * One usage example for setting the offset parameter is for reading
137  * data from a TAR "archive containing multiple files: TAR files always
138  * add a 512 byte header in front of the actual file, so set the offset
139  * to the next byte after the header (e.g., 513).
140  */
141  template<typename PointT> inline int
142  read (const std::string &file_name, pcl::PointCloud<PointT> &cloud, const int offset =0)
143  {
144  pcl::PCLPointCloud2 blob;
145  int file_version;
146  int res = read (file_name, blob, cloud.sensor_origin_, cloud.sensor_orientation_,
147  file_version, offset);
148 
149  // Exit in case of error
150  if (res < 0)
151  return res;
152  pcl::fromPCLPointCloud2 (blob, cloud);
153  return (0);
154  }
155  };
156 
157  /** \brief Point Cloud Data (FILE) file format writer.
158  * Any (FILE) format file reader should implement its virtual methodes
159  * \author Nizar Sallem
160  * \ingroup io
161  */
162  class PCL_EXPORTS FileWriter
163  {
164  public:
165  /** \brief Empty constructor */
167 
168  /** \brief Empty destructor */
169  virtual ~FileWriter () {}
170 
171  /** \brief Save point cloud data to a FILE file containing n-D points
172  * \param[in] file_name the output file name
173  * \param[in] cloud the point cloud data message
174  * \param[in] origin the sensor acquisition origin
175  * \param[in] orientation the sensor acquisition orientation
176  * \param[in] binary set to true if the file is to be written in a binary
177  * FILE format, false (default) for ASCII
178  */
179  virtual int
180  write (const std::string &file_name, const pcl::PCLPointCloud2 &cloud,
181  const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
182  const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
183  const bool binary = false) = 0;
184 
185  /** \brief Save point cloud data to a FILE file containing n-D points
186  * \param[in] file_name the output file name
187  * \param[in] cloud the point cloud data message (boost shared pointer)
188  * \param[in] binary set to true if the file is to be written in a binary
189  * FILE format, false (default) for ASCII
190  * \param[in] origin the sensor acquisition origin
191  * \param[in] orientation the sensor acquisition orientation
192  */
193  inline int
194  write (const std::string &file_name, const pcl::PCLPointCloud2::ConstPtr &cloud,
195  const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
196  const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
197  const bool binary = false)
198  {
199  return (write (file_name, *cloud, origin, orientation, binary));
200  }
201 
202  /** \brief Save point cloud data to a FILE file containing n-D points
203  * \param[in] file_name the output file name
204  * \param[in] cloud the pcl::PointCloud data
205  * \param[in] binary set to true if the file is to be written in a binary
206  * FILE format, false (default) for ASCII
207  */
208  template<typename PointT> inline int
209  write (const std::string &file_name,
210  const pcl::PointCloud<PointT> &cloud,
211  const bool binary = false)
212  {
213  Eigen::Vector4f origin = cloud.sensor_origin_;
214  Eigen::Quaternionf orientation = cloud.sensor_orientation_;
215 
216  pcl::PCLPointCloud2 blob;
217  pcl::toPCLPointCloud2 (cloud, blob);
218 
219  // Save the data
220  return (write (file_name, blob, origin, orientation, binary));
221  }
222  };
223 
224  /** \brief insers a value of type Type (uchar, char, uint, int, float, double, ...) into a stringstream.
225  *
226  * If the value is NaN, it inserst "nan".
227  *
228  * \param[in] cloud the cloud to copy from
229  * \param[in] point_index the index of the point
230  * \param[in] point_size the size of the point in the cloud
231  * \param[in] field_idx the index of the dimension/field
232  * \param[in] fields_count the current fields count
233  * \param[out] stream the ostringstream to copy into
234  */
235  template <typename Type> inline void
237  const unsigned int point_index,
238  const int point_size,
239  const unsigned int field_idx,
240  const unsigned int fields_count,
241  std::ostream &stream)
242  {
243  Type value;
244  memcpy (&value, &cloud.data[point_index * point_size + cloud.fields[field_idx].offset + fields_count * sizeof (Type)], sizeof (Type));
245  if (pcl_isnan (value))
246  stream << "nan";
247  else
248  stream << boost::numeric_cast<Type>(value);
249  }
250  template <> inline void
252  const unsigned int point_index,
253  const int point_size,
254  const unsigned int field_idx,
255  const unsigned int fields_count,
256  std::ostream &stream)
257  {
258  int8_t value;
259  memcpy (&value, &cloud.data[point_index * point_size + cloud.fields[field_idx].offset + fields_count * sizeof (int8_t)], sizeof (int8_t));
260  if (pcl_isnan (value))
261  stream << "nan";
262  else
263  // Numeric cast doesn't give us what we want for int8_t
264  stream << boost::numeric_cast<int>(value);
265  }
266  template <> inline void
268  const unsigned int point_index,
269  const int point_size,
270  const unsigned int field_idx,
271  const unsigned int fields_count,
272  std::ostream &stream)
273  {
274  uint8_t value;
275  memcpy (&value, &cloud.data[point_index * point_size + cloud.fields[field_idx].offset + fields_count * sizeof (uint8_t)], sizeof (uint8_t));
276  if (pcl_isnan (value))
277  stream << "nan";
278  else
279  // Numeric cast doesn't give us what we want for uint8_t
280  stream << boost::numeric_cast<int>(value);
281  }
282 
283  /** \brief Check whether a given value of type Type (uchar, char, uint, int, float, double, ...) is finite or not
284  *
285  * \param[in] cloud the cloud that contains the data
286  * \param[in] point_index the index of the point
287  * \param[in] point_size the size of the point in the cloud
288  * \param[in] field_idx the index of the dimension/field
289  * \param[in] fields_count the current fields count
290  *
291  * \return true if the value is finite, false otherwise
292  */
293  template <typename Type> inline bool
295  const unsigned int point_index,
296  const int point_size,
297  const unsigned int field_idx,
298  const unsigned int fields_count)
299  {
300  Type value;
301  memcpy (&value, &cloud.data[point_index * point_size + cloud.fields[field_idx].offset + fields_count * sizeof (Type)], sizeof (Type));
302  if (!pcl_isfinite (value))
303  return (false);
304  return (true);
305  }
306 
307  /** \brief Copy one single value of type T (uchar, char, uint, int, float, double, ...) from a string
308  *
309  * Uses aoti/atof to do the conversion.
310  * Checks if the st is "nan" and converts it accordingly.
311  *
312  * \param[in] st the string containing the value to convert and copy
313  * \param[out] cloud the cloud to copy it to
314  * \param[in] point_index the index of the point
315  * \param[in] field_idx the index of the dimension/field
316  * \param[in] fields_count the current fields count
317  */
318  template <typename Type> inline void
319  copyStringValue (const std::string &st, pcl::PCLPointCloud2 &cloud,
320  unsigned int point_index, unsigned int field_idx, unsigned int fields_count)
321  {
322  Type value;
323  if (st == "nan")
324  {
325  value = std::numeric_limits<Type>::quiet_NaN ();
326  cloud.is_dense = false;
327  }
328  else
329  {
330  std::istringstream is (st);
331  is.imbue (std::locale::classic ());
332  if (!(is >> value))
333  value = static_cast<Type> (atof (st.c_str ()));
334  }
335 
336  memcpy (&cloud.data[point_index * cloud.point_step +
337  cloud.fields[field_idx].offset +
338  fields_count * sizeof (Type)], reinterpret_cast<char*> (&value), sizeof (Type));
339  }
340 
341  template <> inline void
342  copyStringValue<int8_t> (const std::string &st, pcl::PCLPointCloud2 &cloud,
343  unsigned int point_index, unsigned int field_idx, unsigned int fields_count)
344  {
345  int8_t value;
346  if (st == "nan")
347  {
348  value = static_cast<int8_t> (std::numeric_limits<int>::quiet_NaN ());
349  cloud.is_dense = false;
350  }
351  else
352  {
353  int val;
354  std::istringstream is (st);
355  is.imbue (std::locale::classic ());
356  //is >> val; -- unfortunately this fails on older GCC versions and CLANG on MacOS
357  if (!(is >> val))
358  val = static_cast<int> (atof (st.c_str ()));
359  value = static_cast<int8_t> (val);
360  }
361 
362  memcpy (&cloud.data[point_index * cloud.point_step +
363  cloud.fields[field_idx].offset +
364  fields_count * sizeof (int8_t)], reinterpret_cast<char*> (&value), sizeof (int8_t));
365  }
366 
367  template <> inline void
368  copyStringValue<uint8_t> (const std::string &st, pcl::PCLPointCloud2 &cloud,
369  unsigned int point_index, unsigned int field_idx, unsigned int fields_count)
370  {
371  uint8_t value;
372  if (st == "nan")
373  {
374  value = static_cast<uint8_t> (std::numeric_limits<int>::quiet_NaN ());
375  cloud.is_dense = false;
376  }
377  else
378  {
379  int val;
380  std::istringstream is (st);
381  is.imbue (std::locale::classic ());
382  //is >> val; -- unfortunately this fails on older GCC versions and CLANG on MacOS
383  if (!(is >> val))
384  val = static_cast<int> (atof (st.c_str ()));
385  value = static_cast<uint8_t> (val);
386  }
387 
388  memcpy (&cloud.data[point_index * cloud.point_step +
389  cloud.fields[field_idx].offset +
390  fields_count * sizeof (uint8_t)], reinterpret_cast<char*> (&value), sizeof (uint8_t));
391  }
392 
393  namespace io
394  {
395  /** \brief Load a file into a PointCloud2 according to extension.
396  * \param[in] file_name the name of the file to load
397  * \param[out] blob the resultant pcl::PointCloud2 blob
398  * \ingroup io
399  */
400  PCL_EXPORTS int
401  load (const std::string& file_name, pcl::PCLPointCloud2& blob);
402 
403  /** \brief Load a file into a template PointCloud type according to extension.
404  * \param[in] file_name the name of the file to load
405  * \param[out] cloud the resultant templated point cloud
406  * \ingroup io
407  */
408  template<typename PointT> int
409  load (const std::string& file_name, pcl::PointCloud<PointT>& cloud);
410 
411  /** \brief Load a file into a PolygonMesh according to extension.
412  * \param[in] file_name the name of the file to load
413  * \param[out] mesh the resultant pcl::PolygonMesh
414  * \ingroup io
415  */
416  PCL_EXPORTS int
417  load (const std::string& file_name, pcl::PolygonMesh& mesh);
418 
419  /** \brief Load a file into a TextureMesh according to extension.
420  * \param[in] file_name the name of the file to load
421  * \param[out] mesh the resultant pcl::TextureMesh
422  * \ingroup io
423  */
424  PCL_EXPORTS int
425  load (const std::string& file_name, pcl::TextureMesh& mesh);
426 
427  /** \brief Save point cloud data to a binary file when available else to ASCII.
428  * \param[in] file_name the output file name
429  * \param[in] blob the point cloud data message
430  * \param[in] precision float precision when saving to ASCII files
431  * \ingroup io
432  */
433  PCL_EXPORTS int
434  save (const std::string& file_name, const pcl::PCLPointCloud2& blob, unsigned precision = 5);
435 
436  /** \brief Save point cloud to a binary file when available else to ASCII.
437  * \param[in] file_name the output file name
438  * \param[in] cloud the point cloud
439  * \param[in] precision float precision when saving to ASCII files
440  * \ingroup io
441  */
442  template<typename PointT> int
443  save (const std::string& file_name, const pcl::PointCloud<PointT>& cloud, unsigned precision = 5);
444 
445  /** \brief Saves a TextureMesh to a binary file when available else to ASCII.
446  * \param[in] file_name the name of the file to write to disk
447  * \param[in] tex_mesh the texture mesh to save
448  * \param[in] precision float precision when saving to ASCII files
449  * \ingroup io
450  */
451  PCL_EXPORTS int
452  save (const std::string &file_name, const pcl::TextureMesh &tex_mesh, unsigned precision = 5);
453 
454  /** \brief Saves a PolygonMesh to a binary file when available else to ASCII.
455  * \param[in] file_name the name of the file to write to disk
456  * \param[in] mesh the polygonal mesh to save
457  * \param[in] precision float precision when saving to ASCII files
458  * \ingroup io
459  */
460  PCL_EXPORTS int
461  save (const std::string &file_name, const pcl::PolygonMesh &mesh, unsigned precision = 5);
462  }
463 }
464 
465 #endif //#ifndef PCL_IO_FILE_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:167
void copyStringValue< uint8_t >(const std::string &st, pcl::PCLPointCloud2 &cloud, unsigned int point_index, unsigned int field_idx, unsigned int fields_count)
Definition: file_io.h:368
PCL_EXPORTS int load(const std::string &file_name, pcl::PCLPointCloud2 &blob)
Load a file into a PointCloud2 according to extension.
bool isValueFinite(const pcl::PCLPointCloud2 &cloud, const unsigned int point_index, const int point_size, const unsigned int field_idx, const unsigned int fields_count)
Check whether a given value of type Type (uchar, char, uint, int, float, double, ...) is finite or not.
Definition: file_io.h:294
void copyStringValue< int8_t >(const std::string &st, pcl::PCLPointCloud2 &cloud, unsigned int point_index, unsigned int field_idx, unsigned int fields_count)
Definition: file_io.h:342
pcl::uint32_t point_step
FileWriter()
Empty constructor.
Definition: file_io.h:166
void copyValueString(const pcl::PCLPointCloud2 &cloud, const unsigned int point_index, const int point_size, const unsigned int field_idx, const unsigned int fields_count, std::ostream &stream)
insers a value of type Type (uchar, char, uint, int, float, double, ...) into a stringstream.
Definition: file_io.h:236
pcl::uint8_t is_dense
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
virtual ~FileReader()
empty destructor
Definition: file_io.h:62
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:421
int read(const std::string &file_name, pcl::PointCloud< PointT > &cloud, const int offset=0)
Read a point cloud data from any FILE file, and convert it to the given template format.
Definition: file_io.h:142
int write(const std::string &file_name, const pcl::PointCloud< PointT > &cloud, const bool binary=false)
Save point cloud data to a FILE file containing n-D points.
Definition: file_io.h:209
void copyValueString< int8_t >(const pcl::PCLPointCloud2 &cloud, const unsigned int point_index, const int point_size, const unsigned int field_idx, const unsigned int fields_count, std::ostream &stream)
Definition: file_io.h:251
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
boost::shared_ptr< ::pcl::PCLPointCloud2 const > ConstPtr
PointCloud represents the base class in PCL for storing collections of 3D points. ...
std::vector< ::pcl::PCLPointField > fields
virtual ~FileWriter()
Empty destructor.
Definition: file_io.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 FILE file containing n-D points.
Definition: file_io.h:194
void toPCLPointCloud2(const pcl::PointCloud< PointT > &cloud, pcl::PCLPointCloud2 &msg)
Convert a pcl::PointCloud object to a PCLPointCloud2 binary data blob.
Definition: conversions.h:237
void write(std::ostream &stream, Type value)
Function for writing data to a stream.
Definition: region_xy.h:64
PCL_EXPORTS int save(const std::string &file_name, const pcl::PCLPointCloud2 &blob, unsigned precision=5)
Save point cloud data to a binary file when available else to ASCII.
FileReader()
empty constructor
Definition: file_io.h:60
int read(const std::string &file_name, pcl::PCLPointCloud2 &cloud, const int offset=0)
Read a point cloud data from a FILE file (FILE_V6 only!) and store it into a pcl/PCLPointCloud2.
Definition: file_io.h:124
void copyStringValue(const std::string &st, pcl::PCLPointCloud2 &cloud, unsigned int point_index, unsigned int field_idx, unsigned int fields_count)
Copy one single value of type T (uchar, char, uint, int, float, double, ...) from a string...
Definition: file_io.h:319
std::vector< pcl::uint8_t > data
void copyValueString< uint8_t >(const pcl::PCLPointCloud2 &cloud, const unsigned int point_index, const int point_size, const unsigned int field_idx, const unsigned int fields_count, std::ostream &stream)
Definition: file_io.h:267