Point Cloud Library (PCL) 1.13.0
Loading...
Searching...
No Matches
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
49namespace 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 */
56 class PCL_EXPORTS FileReader
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 {
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 */
162 class PCL_EXPORTS FileWriter
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
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
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
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>
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 if (!(is >> value))
346 value = static_cast<Type>(atof(st.c_str()));
347 }
348
349 memcpy(&cloud.data[point_index * cloud.point_step + cloud.fields[field_idx].offset +
350 fields_count * sizeof(Type)],
351 reinterpret_cast<char*>(&value),
352 sizeof(Type));
353 }
354
355 template <>
356 inline void
357 copyStringValue<std::int8_t>(const std::string& st,
358 pcl::PCLPointCloud2& cloud,
359 pcl::index_t point_index,
360 unsigned int field_idx,
361 unsigned int fields_count,
362 std::istringstream& is)
363 {
364 std::int8_t value;
365 int val;
366 is.str(st);
367 // is >> val; -- unfortunately this fails on older GCC versions and CLANG on MacOS
368 if (!(is >> val)) {
369 val = static_cast<int>(atof(st.c_str()));
370 }
371 value = static_cast<std::int8_t>(val);
372
373 memcpy(&cloud.data[point_index * cloud.point_step + cloud.fields[field_idx].offset +
374 fields_count * sizeof(std::int8_t)],
375 reinterpret_cast<char*>(&value),
376 sizeof(std::int8_t));
377 }
378
379 template <>
380 inline void
381 copyStringValue<std::uint8_t>(const std::string& st,
382 pcl::PCLPointCloud2& cloud,
383 pcl::index_t point_index,
384 unsigned int field_idx,
385 unsigned int fields_count,
386 std::istringstream& is)
387 {
388 std::uint8_t value;
389 int val;
390 is.str(st);
391 // is >> val; -- unfortunately this fails on older GCC versions and CLANG on
392 // MacOS
393 if (!(is >> val)) {
394 val = static_cast<int>(atof(st.c_str()));
395 }
396 value = static_cast<std::uint8_t>(val);
397
398 memcpy(&cloud.data[point_index * cloud.point_step + cloud.fields[field_idx].offset +
399 fields_count * sizeof(std::uint8_t)],
400 reinterpret_cast<char*>(&value),
401 sizeof(std::uint8_t));
402 }
403 } // namespace detail
404
405 /**
406 * \brief Copy one single value of type T (uchar, char, uint, int, float, double, ...) from a string
407 * \details Uses `istringstream` to do the conversion in classic locale
408 * Checks if the st is "nan" and converts it accordingly.
409 *
410 * \param[in] st the string containing the value to convert and copy
411 * \param[out] cloud the cloud to copy it to
412 * \param[in] point_index the index of the point
413 * \param[in] field_idx the index of the dimension/field
414 * \param[in] fields_count the current fields count
415 */
416 template <typename Type> inline void
417 copyStringValue (const std::string &st, pcl::PCLPointCloud2 &cloud,
418 pcl::index_t point_index, unsigned int field_idx, unsigned int fields_count)
419 {
420 std::istringstream is;
421 is.imbue (std::locale::classic ());
422 detail::copyStringValue<Type> (st, cloud,point_index, field_idx, fields_count, is);
423 }
424/**
425 * \brief Copy one single value of type T (uchar, char, uint, int, float, double, ...) from a string
426 * \details Uses the provided `istringstream` to do the conversion, respecting its locale settings
427 * Checks if the st is "nan" and converts it accordingly.
428 *
429 * \param[in] st the string containing the value to convert and copy
430 * \param[out] cloud the cloud to copy it to
431 * \param[in] point_index the index of the point
432 * \param[in] field_idx the index of the dimension/field
433 * \param[in] fields_count the current fields count
434 * \param[in,out] is input string stream for helping to convert st into cloud
435 */
436 template <typename Type> inline void
437 copyStringValue (const std::string &st, pcl::PCLPointCloud2 &cloud,
438 pcl::index_t point_index, unsigned int field_idx, unsigned int fields_count,
439 std::istringstream& is)
440 {
441 detail::copyStringValue<Type> (st, cloud,point_index, field_idx, fields_count, is);
442 }
443}
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.
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
void copyStringValue< std::int8_t >(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:357
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 copyStringValue< std::uint8_t >(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:381
void read(std::istream &stream, Type &value)
Function for reading data from a stream.
Definition region_xy.h:46
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:417
void toPCLPointCloud2(const pcl::PointCloud< PointT > &cloud, pcl::PCLPointCloud2 &msg)
Convert a pcl::PointCloud<T> object to a PCLPointCloud2 binary data blob.
void copyValueString< std::uint8_t >(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)
Definition file_io.h:281
void copyValueString< std::int8_t >(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)
Definition file_io.h:267
void write(std::ostream &stream, Type value)
Function for writing data to a stream.
Definition region_xy.h:63
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
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.
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::uint8_t is_dense
std::vector<::pcl::PCLPointField > fields
std::vector< std::uint8_t > data
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr