Point Cloud Library (PCL)  1.14.1-dev
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 #pragma once
39 
40 #include <pcl/conversions.h> // for fromPCLPointCloud2, toPCLPointCloud2
41 #include <pcl/point_cloud.h> // for PointCloud
42 #include <pcl/PCLPointCloud2.h> // for PCLPointCloud2
43 #include <cmath>
44 #include <sstream>
45 #include <Eigen/Geometry> // for Quaternionf
46 #include <boost/numeric/conversion/cast.hpp> // for numeric_cast
47 #include <boost/algorithm/string/predicate.hpp> // for iequals
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 methods.
53  * \author Nizar Sallem
54  * \ingroup io
55  */
57  {
58  public:
59  /** \brief empty constructor */
60  FileReader() = default;
61  /** \brief empty destructor */
62  virtual ~FileReader() = default;
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 methods
159  * \author Nizar Sallem
160  * \ingroup io
161  */
163  {
164  public:
165  /** \brief Empty constructor */
166  FileWriter () = default;
167 
168  /** \brief Empty destructor */
169  virtual ~FileWriter () = default;
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 inserts a value of type Type (uchar, char, uint, int, float, double, ...) into a stringstream.
225  *
226  * If the value is NaN, it inserts "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
236  std::enable_if_t<std::is_floating_point<Type>::value>
238  const pcl::index_t point_index,
239  const int point_size,
240  const unsigned int field_idx,
241  const unsigned int fields_count,
242  std::ostream &stream)
243  {
244  Type value;
245  memcpy (&value, &cloud.data[point_index * point_size + cloud.fields[field_idx].offset + fields_count * sizeof (Type)], sizeof (Type));
246  if (std::isnan (value))
247  stream << "nan";
248  else
249  stream << value;
250  }
251 
252  template <typename Type> inline
253  std::enable_if_t<std::is_integral<Type>::value>
255  const pcl::index_t point_index,
256  const int point_size,
257  const unsigned int field_idx,
258  const unsigned int fields_count,
259  std::ostream &stream)
260  {
261  Type value;
262  memcpy (&value, &cloud.data[point_index * point_size + cloud.fields[field_idx].offset + fields_count * sizeof (Type)], sizeof (Type));
263  stream << value;
264  }
265 
266  template <> inline void
267  copyValueString<std::int8_t> (const pcl::PCLPointCloud2 &cloud,
268  const pcl::index_t 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  std::int8_t value;
275  memcpy (&value, &cloud.data[point_index * point_size + cloud.fields[field_idx].offset + fields_count * sizeof (std::int8_t)], sizeof (std::int8_t));
276  //Cast to int to prevent value is handled as char
277  stream << boost::numeric_cast<int>(value);
278  }
279 
280  template <> inline void
281  copyValueString<std::uint8_t> (const pcl::PCLPointCloud2 &cloud,
282  const pcl::index_t point_index,
283  const int point_size,
284  const unsigned int field_idx,
285  const unsigned int fields_count,
286  std::ostream &stream)
287  {
288  std::uint8_t value;
289  memcpy (&value, &cloud.data[point_index * point_size + cloud.fields[field_idx].offset + fields_count * sizeof (std::uint8_t)], sizeof (std::uint8_t));
290  //Cast to unsigned int to prevent value is handled as char
291  stream << boost::numeric_cast<unsigned int>(value);
292  }
293 
294  /** \brief Check whether a given value of type Type (uchar, char, uint, int, float, double, ...) is finite or not
295  *
296  * \param[in] cloud the cloud that contains the data
297  * \param[in] point_index the index of the point
298  * \param[in] point_size the size of the point in the cloud
299  * \param[in] field_idx the index of the dimension/field
300  * \param[in] fields_count the current fields count
301  *
302  * \return true if the value is finite, false otherwise
303  */
304  template <typename Type> inline
305  std::enable_if_t<std::is_floating_point<Type>::value, bool>
307  const pcl::index_t point_index,
308  const int point_size,
309  const unsigned int field_idx,
310  const unsigned int fields_count)
311  {
312  Type value;
313  memcpy (&value, &cloud.data[point_index * point_size + cloud.fields[field_idx].offset + fields_count * sizeof (Type)], sizeof (Type));
314  return std::isfinite (value);
315  }
316 
317  template <typename Type> inline
318  std::enable_if_t<std::is_integral<Type>::value, bool>
319  isValueFinite (const pcl::PCLPointCloud2& /* cloud */,
320  const pcl::index_t /* point_index */,
321  const int /* point_size */,
322  const unsigned int /* field_idx */,
323  const unsigned int /* fields_count */)
324  {
325  return true;
326  }
327 
328  namespace detail {
329  template <typename Type>
330  inline void
331  copyStringValue(const std::string& st,
332  pcl::PCLPointCloud2& cloud,
333  pcl::index_t point_index,
334  unsigned int field_idx,
335  unsigned int fields_count,
336  std::istringstream& is)
337  {
338  Type value;
339  if (boost::iequals(st, "nan")) {
340  value = std::numeric_limits<Type>::quiet_NaN();
341  cloud.is_dense = false;
342  }
343  else {
344  is.str(st);
345  is.clear(); // clear error state flags
346  if (!(is >> value))
347  value = static_cast<Type>(atof(st.c_str()));
348  }
349 
350  memcpy(&cloud.data[point_index * cloud.point_step + cloud.fields[field_idx].offset +
351  fields_count * sizeof(Type)],
352  reinterpret_cast<char*>(&value),
353  sizeof(Type));
354  }
355 
356  template <>
357  inline void
358  copyStringValue<std::int8_t>(const std::string& st,
359  pcl::PCLPointCloud2& cloud,
360  pcl::index_t point_index,
361  unsigned int field_idx,
362  unsigned int fields_count,
363  std::istringstream& is)
364  {
365  std::int8_t value;
366  int val;
367  is.str(st);
368  is.clear(); // clear error state flags
369  // is >> val; -- unfortunately this fails on older GCC versions and CLANG on MacOS
370  if (!(is >> val)) {
371  val = static_cast<int>(atof(st.c_str()));
372  }
373  value = static_cast<std::int8_t>(val);
374 
375  memcpy(&cloud.data[point_index * cloud.point_step + cloud.fields[field_idx].offset +
376  fields_count * sizeof(std::int8_t)],
377  reinterpret_cast<char*>(&value),
378  sizeof(std::int8_t));
379  }
380 
381  template <>
382  inline void
383  copyStringValue<std::uint8_t>(const std::string& st,
384  pcl::PCLPointCloud2& cloud,
385  pcl::index_t point_index,
386  unsigned int field_idx,
387  unsigned int fields_count,
388  std::istringstream& is)
389  {
390  std::uint8_t value;
391  int val;
392  is.str(st);
393  is.clear(); // clear error state flags
394  // is >> val; -- unfortunately this fails on older GCC versions and CLANG on
395  // MacOS
396  if (!(is >> val)) {
397  val = static_cast<int>(atof(st.c_str()));
398  }
399  value = static_cast<std::uint8_t>(val);
400 
401  memcpy(&cloud.data[point_index * cloud.point_step + cloud.fields[field_idx].offset +
402  fields_count * sizeof(std::uint8_t)],
403  reinterpret_cast<char*>(&value),
404  sizeof(std::uint8_t));
405  }
406  } // namespace detail
407 
408  /**
409  * \brief Copy one single value of type T (uchar, char, uint, int, float, double, ...) from a string
410  * \details Uses `istringstream` to do the conversion in classic locale
411  * Checks if the st is "nan" and converts it accordingly.
412  *
413  * \param[in] st the string containing the value to convert and copy
414  * \param[out] cloud the cloud to copy it to
415  * \param[in] point_index the index of the point
416  * \param[in] field_idx the index of the dimension/field
417  * \param[in] fields_count the current fields count
418  */
419  template <typename Type> inline void
420  copyStringValue (const std::string &st, pcl::PCLPointCloud2 &cloud,
421  pcl::index_t point_index, unsigned int field_idx, unsigned int fields_count)
422  {
423  std::istringstream is;
424  is.imbue (std::locale::classic ());
425  detail::copyStringValue<Type> (st, cloud,point_index, field_idx, fields_count, is);
426  }
427 /**
428  * \brief Copy one single value of type T (uchar, char, uint, int, float, double, ...) from a string
429  * \details Uses the provided `istringstream` to do the conversion, respecting its locale settings
430  * Checks if the st is "nan" and converts it accordingly.
431  *
432  * \param[in] st the string containing the value to convert and copy
433  * \param[out] cloud the cloud to copy it to
434  * \param[in] point_index the index of the point
435  * \param[in] field_idx the index of the dimension/field
436  * \param[in] fields_count the current fields count
437  * \param[in,out] is input string stream for helping to convert st into cloud
438  */
439  template <typename Type> inline void
440  copyStringValue (const std::string &st, pcl::PCLPointCloud2 &cloud,
441  pcl::index_t point_index, unsigned int field_idx, unsigned int fields_count,
442  std::istringstream& is)
443  {
444  detail::copyStringValue<Type> (st, cloud,point_index, field_idx, fields_count, is);
445  }
446 }
Point Cloud Data (FILE) file format reader interface.
Definition: file_io.h:57
virtual int read(const std::string &file_name, pcl::PCLPointCloud2 &cloud, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &file_version, const int offset=0)=0
Read a point cloud data from a FILE file and store it into a pcl/PCLPointCloud2.
FileReader()=default
empty constructor
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 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
virtual ~FileReader()=default
empty destructor
virtual int readHeader(const std::string &file_name, pcl::PCLPointCloud2 &cloud, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &file_version, int &data_type, unsigned int &data_idx, const int offset=0)=0
Read a point cloud data header from a FILE file.
Point Cloud Data (FILE) file format writer.
Definition: file_io.h:163
virtual 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)=0
Save point cloud data to a FILE file containing n-D points.
FileWriter()=default
Empty constructor.
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
virtual ~FileWriter()=default
Empty destructor.
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
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:408
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:406
void copyStringValue(const std::string &st, pcl::PCLPointCloud2 &cloud, pcl::index_t point_index, unsigned int field_idx, unsigned int fields_count, std::istringstream &is)
Definition: file_io.h:331
void read(std::istream &stream, Type &value)
Function for reading data from a stream.
Definition: region_xy.h:46
std::enable_if_t< std::is_floating_point< Type >::value, bool > isValueFinite(const pcl::PCLPointCloud2 &cloud, const pcl::index_t 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, ....
Definition: file_io.h:306
std::enable_if_t< std::is_floating_point< Type >::value > copyValueString(const pcl::PCLPointCloud2 &cloud, const pcl::index_t point_index, const int point_size, const unsigned int field_idx, const unsigned int fields_count, std::ostream &stream)
inserts a value of type Type (uchar, char, uint, int, float, double, ...) into a stringstream.
Definition: file_io.h:237
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition: types.h:112
void copyStringValue(const std::string &st, pcl::PCLPointCloud2 &cloud, pcl::index_t 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:420
void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map, const std::uint8_t *msg_data)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud<T> object using a field_map.
Definition: conversions.h:229
void toPCLPointCloud2(const pcl::PointCloud< PointT > &cloud, pcl::PCLPointCloud2 &msg, bool padding)
Convert a pcl::PointCloud<T> object to a PCLPointCloud2 binary data blob.
Definition: conversions.h:372
void write(std::ostream &stream, Type value)
Function for writing data to a stream.
Definition: region_xy.h:63
#define PCL_EXPORTS
Definition: pcl_macros.h:323
std::uint8_t is_dense
std::vector<::pcl::PCLPointField > fields
std::vector< std::uint8_t > data
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr