Point Cloud Library (PCL)  1.7.1
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 
47 namespace pcl
48 {
49  /** \brief Point Cloud Data (FILE) file format reader interface.
50  * Any (FILE) format file reader should implement its virtual methodes.
51  * \author Nizar Sallem
52  * \ingroup io
53  */
54  class PCL_EXPORTS FileReader
55  {
56  public:
57  /** \brief empty constructor */
59  /** \brief empty destructor */
60  virtual ~FileReader() {}
61  /** \brief Read a point cloud data header from a FILE file.
62  *
63  * Load only the meta information (number of points, their types, etc),
64  * and not the points themselves, from a given FILE file. Useful for fast
65  * evaluation of the underlying data structure.
66  *
67  * Returns:
68  * * < 0 (-1) on error
69  * * > 0 on success
70  * \param[in] file_name the name of the file to load
71  * \param[out] cloud the resultant point cloud dataset (only the header will be filled)
72  * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present)
73  * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present)
74  * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7)
75  * \param[out] data_type the type of data (binary data=1, ascii=0, etc)
76  * \param[out] data_idx the offset of cloud data within the file
77  * \param[in] offset the offset in the file where to expect the true header to begin.
78  * One usage example for setting the offset parameter is for reading
79  * data from a TAR "archive containing multiple files: TAR files always
80  * add a 512 byte header in front of the actual file, so set the offset
81  * to the next byte after the header (e.g., 513).
82  */
83  virtual int
84  readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
85  Eigen::Vector4f &origin, Eigen::Quaternionf &orientation,
86  int &file_version, int &data_type, unsigned int &data_idx, const int offset = 0) = 0;
87 
88  /** \brief Read a point cloud data from a FILE file and store it into a pcl/PCLPointCloud2.
89  * \param[in] file_name the name of the file containing the actual PointCloud data
90  * \param[out] cloud the resultant PointCloud message read from disk
91  * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present)
92  * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present)
93  * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7)
94  * \param[in] offset the offset in the file where to expect the true header to begin.
95  * One usage example for setting the offset parameter is for reading
96  * data from a TAR "archive containing multiple files: TAR files always
97  * add a 512 byte header in front of the actual file, so set the offset
98  * to the next byte after the header (e.g., 513).
99  */
100  virtual int
101  read (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
102  Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &file_version,
103  const int offset = 0) = 0;
104 
105  /** \brief Read a point cloud data from a FILE file (FILE_V6 only!) and store it into a pcl/PCLPointCloud2.
106  *
107  * \note This function is provided for backwards compatibility only and
108  * it can only read FILE_V6 files correctly, as pcl::PCLPointCloud2
109  * does not contain a sensor origin/orientation. Reading any file
110  * > FILE_V6 will generate a warning.
111  *
112  * \param[in] file_name the name of the file containing the actual PointCloud data
113  * \param[out] cloud the resultant PointCloud message read from disk
114  *
115  * \param[in] offset the offset in the file where to expect the true header to begin.
116  * One usage example for setting the offset parameter is for reading
117  * data from a TAR "archive containing multiple files: TAR files always
118  * add a 512 byte header in front of the actual file, so set the offset
119  * to the next byte after the header (e.g., 513).
120  */
121  int
122  read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, const int offset = 0)
123  {
124  Eigen::Vector4f origin;
125  Eigen::Quaternionf orientation;
126  int file_version;
127  return (read (file_name, cloud, origin, orientation, file_version, offset));
128  }
129 
130  /** \brief Read a point cloud data from any FILE file, and convert it to the given template format.
131  * \param[in] file_name the name of the file containing the actual PointCloud data
132  * \param[out] cloud the resultant PointCloud message read from disk
133  * \param[in] offset the offset in the file where to expect the true header to begin.
134  * One usage example for setting the offset parameter is for reading
135  * data from a TAR "archive containing multiple files: TAR files always
136  * add a 512 byte header in front of the actual file, so set the offset
137  * to the next byte after the header (e.g., 513).
138  */
139  template<typename PointT> inline int
140  read (const std::string &file_name, pcl::PointCloud<PointT> &cloud, const int offset =0)
141  {
142  pcl::PCLPointCloud2 blob;
143  int file_version;
144  int res = read (file_name, blob, cloud.sensor_origin_, cloud.sensor_orientation_,
145  file_version, offset);
146 
147  // Exit in case of error
148  if (res < 0)
149  return res;
150  pcl::fromPCLPointCloud2 (blob, cloud);
151  return (0);
152  }
153  };
154 
155  /** \brief Point Cloud Data (FILE) file format writer.
156  * Any (FILE) format file reader should implement its virtual methodes
157  * \author Nizar Sallem
158  * \ingroup io
159  */
160  class PCL_EXPORTS FileWriter
161  {
162  public:
163  /** \brief Empty constructor */
165 
166  /** \brief Empty destructor */
167  virtual ~FileWriter () {}
168 
169  /** \brief Save point cloud data to a FILE file containing n-D points
170  * \param[in] file_name the output file name
171  * \param[in] cloud the point cloud data message
172  * \param[in] origin the sensor acquisition origin
173  * \param[in] orientation the sensor acquisition orientation
174  * \param[in] binary set to true if the file is to be written in a binary
175  * FILE format, false (default) for ASCII
176  */
177  virtual int
178  write (const std::string &file_name, const pcl::PCLPointCloud2 &cloud,
179  const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
180  const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
181  const bool binary = false) = 0;
182 
183  /** \brief Save point cloud data to a FILE file containing n-D points
184  * \param[in] file_name the output file name
185  * \param[in] cloud the point cloud data message (boost shared pointer)
186  * \param[in] binary set to true if the file is to be written in a binary
187  * FILE format, false (default) for ASCII
188  * \param[in] origin the sensor acquisition origin
189  * \param[in] orientation the sensor acquisition orientation
190  */
191  inline int
192  write (const std::string &file_name, const pcl::PCLPointCloud2::ConstPtr &cloud,
193  const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
194  const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
195  const bool binary = false)
196  {
197  return (write (file_name, *cloud, origin, orientation, binary));
198  }
199 
200  /** \brief Save point cloud data to a FILE file containing n-D points
201  * \param[in] file_name the output file name
202  * \param[in] cloud the pcl::PointCloud data
203  * \param[in] binary set to true if the file is to be written in a binary
204  * FILE format, false (default) for ASCII
205  */
206  template<typename PointT> inline int
207  write (const std::string &file_name,
208  const pcl::PointCloud<PointT> &cloud,
209  const bool binary = false)
210  {
211  Eigen::Vector4f origin = cloud.sensor_origin_;
212  Eigen::Quaternionf orientation = cloud.sensor_orientation_;
213 
214  pcl::PCLPointCloud2 blob;
215  pcl::toPCLPointCloud2 (cloud, blob);
216 
217  // Save the data
218  return (write (file_name, blob, origin, orientation, binary));
219  }
220  };
221 
222  /** \brief insers a value of type Type (uchar, char, uint, int, float, double, ...) into a stringstream.
223  *
224  * If the value is NaN, it inserst "nan".
225  *
226  * \param[in] cloud the cloud to copy from
227  * \param[in] point_index the index of the point
228  * \param[in] point_size the size of the point in the cloud
229  * \param[in] field_idx the index of the dimension/field
230  * \param[in] fields_count the current fields count
231  * \param[out] stream the ostringstream to copy into
232  */
233  template <typename Type> inline void
235  const unsigned int point_index,
236  const int point_size,
237  const unsigned int field_idx,
238  const unsigned int fields_count,
239  std::ostream &stream)
240  {
241  Type value;
242  memcpy (&value, &cloud.data[point_index * point_size + cloud.fields[field_idx].offset + fields_count * sizeof (Type)], sizeof (Type));
243  if (pcl_isnan (value))
244  stream << "nan";
245  else
246  stream << boost::numeric_cast<Type>(value);
247  }
248  template <> inline void
250  const unsigned int point_index,
251  const int point_size,
252  const unsigned int field_idx,
253  const unsigned int fields_count,
254  std::ostream &stream)
255  {
256  int8_t value;
257  memcpy (&value, &cloud.data[point_index * point_size + cloud.fields[field_idx].offset + fields_count * sizeof (int8_t)], sizeof (int8_t));
258  if (pcl_isnan (value))
259  stream << "nan";
260  else
261  // Numeric cast doesn't give us what we want for int8_t
262  stream << boost::numeric_cast<int>(value);
263  }
264  template <> inline void
266  const unsigned int point_index,
267  const int point_size,
268  const unsigned int field_idx,
269  const unsigned int fields_count,
270  std::ostream &stream)
271  {
272  uint8_t value;
273  memcpy (&value, &cloud.data[point_index * point_size + cloud.fields[field_idx].offset + fields_count * sizeof (uint8_t)], sizeof (uint8_t));
274  if (pcl_isnan (value))
275  stream << "nan";
276  else
277  // Numeric cast doesn't give us what we want for uint8_t
278  stream << boost::numeric_cast<int>(value);
279  }
280 
281  /** \brief Check whether a given value of type Type (uchar, char, uint, int, float, double, ...) is finite or not
282  *
283  * \param[in] cloud the cloud that contains the data
284  * \param[in] point_index the index of the point
285  * \param[in] point_size the size of the point in the cloud
286  * \param[in] field_idx the index of the dimension/field
287  * \param[in] fields_count the current fields count
288  *
289  * \return true if the value is finite, false otherwise
290  */
291  template <typename Type> inline bool
293  const unsigned int point_index,
294  const int point_size,
295  const unsigned int field_idx,
296  const unsigned int fields_count)
297  {
298  Type value;
299  memcpy (&value, &cloud.data[point_index * point_size + cloud.fields[field_idx].offset + fields_count * sizeof (Type)], sizeof (Type));
300  if (!pcl_isfinite (value))
301  return (false);
302  return (true);
303  }
304 
305  /** \brief Copy one single value of type T (uchar, char, uint, int, float, double, ...) from a string
306  *
307  * Uses aoti/atof to do the conversion.
308  * Checks if the st is "nan" and converts it accordingly.
309  *
310  * \param[in] st the string containing the value to convert and copy
311  * \param[out] cloud the cloud to copy it to
312  * \param[in] point_index the index of the point
313  * \param[in] field_idx the index of the dimension/field
314  * \param[in] fields_count the current fields count
315  */
316  template <typename Type> inline void
317  copyStringValue (const std::string &st, pcl::PCLPointCloud2 &cloud,
318  unsigned int point_index, unsigned int field_idx, unsigned int fields_count)
319  {
320  Type value;
321  if (st == "nan")
322  {
323  value = std::numeric_limits<Type>::quiet_NaN ();
324  cloud.is_dense = false;
325  }
326  else
327  {
328  std::istringstream is (st);
329  is.imbue (std::locale::classic ());
330  if (!(is >> value))
331  value = static_cast<Type> (atof (st.c_str ()));
332  }
333 
334  memcpy (&cloud.data[point_index * cloud.point_step +
335  cloud.fields[field_idx].offset +
336  fields_count * sizeof (Type)], reinterpret_cast<char*> (&value), sizeof (Type));
337  }
338 
339  template <> inline void
340  copyStringValue<int8_t> (const std::string &st, pcl::PCLPointCloud2 &cloud,
341  unsigned int point_index, unsigned int field_idx, unsigned int fields_count)
342  {
343  int8_t value;
344  if (st == "nan")
345  {
346  value = static_cast<int8_t> (std::numeric_limits<int>::quiet_NaN ());
347  cloud.is_dense = false;
348  }
349  else
350  {
351  int val;
352  std::istringstream is (st);
353  is.imbue (std::locale::classic ());
354  //is >> val; -- unfortunately this fails on older GCC versions and CLANG on MacOS
355  if (!(is >> val))
356  val = static_cast<int> (atof (st.c_str ()));
357  value = static_cast<int8_t> (val);
358  }
359 
360  memcpy (&cloud.data[point_index * cloud.point_step +
361  cloud.fields[field_idx].offset +
362  fields_count * sizeof (int8_t)], reinterpret_cast<char*> (&value), sizeof (int8_t));
363  }
364 
365  template <> inline void
366  copyStringValue<uint8_t> (const std::string &st, pcl::PCLPointCloud2 &cloud,
367  unsigned int point_index, unsigned int field_idx, unsigned int fields_count)
368  {
369  uint8_t value;
370  if (st == "nan")
371  {
372  value = static_cast<uint8_t> (std::numeric_limits<int>::quiet_NaN ());
373  cloud.is_dense = false;
374  }
375  else
376  {
377  int val;
378  std::istringstream is (st);
379  is.imbue (std::locale::classic ());
380  //is >> val; -- unfortunately this fails on older GCC versions and CLANG on MacOS
381  if (!(is >> val))
382  val = static_cast<int> (atof (st.c_str ()));
383  value = static_cast<uint8_t> (val);
384  }
385 
386  memcpy (&cloud.data[point_index * cloud.point_step +
387  cloud.fields[field_idx].offset +
388  fields_count * sizeof (uint8_t)], reinterpret_cast<char*> (&value), sizeof (uint8_t));
389  }
390 }
391 
392 #endif //#ifndef PCL_IO_FILE_IO_H_