Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
ply_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/memory.h>
43#include <pcl/pcl_macros.h>
44#include <pcl/common/io.h> // for copyPointCloud
45#include <pcl/io/file_io.h>
46#include <pcl/io/ply/ply_parser.h>
47#include <pcl/PolygonMesh.h>
48
49#include <sstream>
50#include <tuple>
51
52namespace pcl
53{
54 /** \brief Point Cloud Data (PLY) file format reader.
55 *
56 * The PLY data format is organized in the following way:
57 * lines beginning with "comment" are treated as comments
58 * - ply
59 * - format [ascii|binary_little_endian|binary_big_endian] 1.0
60 * - element vertex COUNT
61 * - property float x
62 * - property float y
63 * - [property float z]
64 * - [property float normal_x]
65 * - [property float normal_y]
66 * - [property float normal_z]
67 * - [property uchar red]
68 * - [property uchar green]
69 * - [property uchar blue] ...
70 * - ascii/binary point coordinates
71 * - [element camera 1]
72 * - [property float view_px] ...
73 * - [element range_grid COUNT]
74 * - [property list uchar int vertex_indices]
75 * - end header
76 *
77 * \author Nizar Sallem
78 * \ingroup io
79 */
80 class PCL_EXPORTS PLYReader : public FileReader
81 {
82 public:
83 enum
84 {
85 PLY_V0 = 0,
86 PLY_V1 = 1
87 };
88
90 : origin_ (Eigen::Vector4f::Zero ())
91 , orientation_ (Eigen::Matrix3f::Identity ())
92 {}
93
95 : origin_ (Eigen::Vector4f::Zero ())
96 , orientation_ (Eigen::Matrix3f::Identity ())
97 {
98 *this = p;
99 }
100
101 PLYReader&
102 operator = (const PLYReader &p)
103 {
104 origin_ = p.origin_;
105 orientation_ = p.orientation_;
106 range_grid_ = p.range_grid_;
107 polygons_ = p.polygons_;
108 return (*this);
109 }
110
111 ~PLYReader () override { delete range_grid_; }
112 /** \brief Read a point cloud data header from a PLY file.
113 *
114 * Load only the meta information (number of points, their types, etc),
115 * and not the points themselves, from a given PLY file. Useful for fast
116 * evaluation of the underlying data structure.
117 *
118 * Returns:
119 * * < 0 (-1) on error
120 * * > 0 on success
121 * \param[in] file_name the name of the file to load
122 * \param[out] cloud the resultant point cloud dataset (only the header will be filled)
123 * \param[out] origin the sensor data acquisition origin (translation)
124 * \param[out] orientation the sensor data acquisition origin (rotation)
125 * \param[out] ply_version the PLY version read from the file
126 * \param[out] data_type the type of PLY data stored in the file
127 * \param[out] data_idx the data index
128 * \param[in] offset the offset in the file where to expect the true header to begin.
129 * One usage example for setting the offset parameter is for reading
130 * data from a TAR "archive containing multiple files: TAR files always
131 * add a 512 byte header in front of the actual file, so set the offset
132 * to the next byte after the header (e.g., 513).
133 */
134 int
135 readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
136 Eigen::Vector4f &origin, Eigen::Quaternionf &orientation,
137 int &ply_version, int &data_type, unsigned int &data_idx, const int offset = 0) override;
138
139 /** \brief Read a point cloud data from a PLY file and store it into a pcl/PCLPointCloud2.
140 * \param[in] file_name the name of the file containing the actual PointCloud data
141 * \param[out] cloud the resultant PointCloud message read from disk
142 * \param[out] origin the sensor data acquisition origin (translation)
143 * \param[out] orientation the sensor data acquisition origin (rotation)
144 * \param[out] ply_version the PLY version read from the file
145 * \param[in] offset the offset in the file where to expect the true header to begin.
146 * One usage example for setting the offset parameter is for reading
147 * data from a TAR "archive containing multiple files: TAR files always
148 * add a 512 byte header in front of the actual file, so set the offset
149 * to the next byte after the header (e.g., 513).
150 */
151 int
152 read (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
153 Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int& ply_version, const int offset = 0) override;
154
155 /** \brief Read a point cloud data from a PLY file and store it into a pcl/PCLPointCloud2.
156 * \note This function is provided for backwards compatibility only
157 * \param[in] file_name the name of the file containing the actual PointCloud data
158 * \param[out] cloud the resultant PointCloud message read from disk
159 * \param[in] offset the offset in the file where to expect the true header to begin.
160 * One usage example for setting the offset parameter is for reading
161 * data from a TAR "archive containing multiple files: TAR files always
162 * add a 512 byte header in front of the actual file, so set the offset
163 * to the next byte after the header (e.g., 513).
164 */
165 inline int
166 read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, const int offset = 0)
167 {
168 Eigen::Vector4f origin;
169 Eigen::Quaternionf orientation;
170 int ply_version;
171 return read (file_name, cloud, origin, orientation, ply_version, offset);
172 }
173
174 /** \brief Read a point cloud data from any PLY file, and convert it to the given template format.
175 * \param[in] file_name the name of the file containing the actual PointCloud data
176 * \param[out] cloud the resultant PointCloud message read from disk
177 * \param[in] offset the offset in the file where to expect the true header to begin.
178 * One usage example for setting the offset parameter is for reading
179 * data from a TAR "archive containing multiple files: TAR files always
180 * add a 512 byte header in front of the actual file, so set the offset
181 * to the next byte after the header (e.g., 513).
182 */
183 template<typename PointT> inline int
184 read (const std::string &file_name, pcl::PointCloud<PointT> &cloud, const int offset = 0)
185 {
187 int ply_version;
188 int res = read (file_name, blob, cloud.sensor_origin_, cloud.sensor_orientation_,
189 ply_version, offset);
190
191 // Exit in case of error
192 if (res < 0)
193 return (res);
194 pcl::fromPCLPointCloud2 (blob, cloud);
195 return (0);
196 }
197
198 /** \brief Read a point cloud data from a PLY file and store it into a pcl/PolygonMesh.
199 *
200 * \param[in] file_name the name of the file containing the actual PointCloud data
201 * \param[out] mesh the resultant PolygonMesh message read from disk
202 * \param[out] origin the sensor data acquisition origin (translation)
203 * \param[out] orientation the sensor data acquisition origin (rotation)
204 * \param[out] ply_version the PLY version read from the file
205 * \param[in] offset the offset in the file where to expect the true header to begin.
206 * One usage example for setting the offset parameter is for reading
207 * data from a TAR "archive containing multiple files: TAR files always
208 * add a 512 byte header in front of the actual file, so set the offset
209 * to the next byte after the header (e.g., 513).
210 */
211 int
212 read (const std::string &file_name, pcl::PolygonMesh &mesh,
213 Eigen::Vector4f &origin, Eigen::Quaternionf &orientation,
214 int& ply_version, const int offset = 0);
215
216 /** \brief Read a point cloud data from a PLY file and store it into a pcl/PolygonMesh.
217 *
218 * \param[in] file_name the name of the file containing the actual PointCloud data
219 * \param[out] mesh the resultant PolygonMesh message read from disk
220 * \param[in] offset the offset in the file where to expect the true header to begin.
221 * One usage example for setting the offset parameter is for reading
222 * data from a TAR "archive containing multiple files: TAR files always
223 * add a 512 byte header in front of the actual file, so set the offset
224 * to the next byte after the header (e.g., 513).
225 */
226 int
227 read (const std::string &file_name, pcl::PolygonMesh &mesh, const int offset = 0);
228
229 private:
231
232 bool
233 parse (const std::string& istream_filename);
234
235 /** \brief Info callback function
236 * \param[in] filename PLY file read
237 * \param[in] line_number line triggering the callback
238 * \param[in] message information message
239 */
240 void
241 infoCallback (const std::string& filename, std::size_t line_number, const std::string& message)
242 {
243 PCL_DEBUG ("[pcl::PLYReader] %s:%lu: %s\n", filename.c_str (), line_number, message.c_str ());
244 }
245
246 /** \brief Warning callback function
247 * \param[in] filename PLY file read
248 * \param[in] line_number line triggering the callback
249 * \param[in] message warning message
250 */
251 void
252 warningCallback (const std::string& filename, std::size_t line_number, const std::string& message)
253 {
254 PCL_WARN ("[pcl::PLYReader] %s:%lu: %s\n", filename.c_str (), line_number, message.c_str ());
255 }
256
257 /** \brief Error callback function
258 * \param[in] filename PLY file read
259 * \param[in] line_number line triggering the callback
260 * \param[in] message error message
261 */
262 void
263 errorCallback (const std::string& filename, std::size_t line_number, const std::string& message)
264 {
265 PCL_ERROR ("[pcl::PLYReader] %s:%lu: %s\n", filename.c_str (), line_number, message.c_str ());
266 }
267
268 /** \brief function called when the keyword element is parsed
269 * \param[in] element_name element name
270 * \param[in] count number of instances
271 */
272 std::tuple<std::function<void ()>, std::function<void ()> >
273 elementDefinitionCallback (const std::string& element_name, std::size_t count);
274
275 bool
276 endHeaderCallback ();
277
278 /** \brief function called when a scalar property is parsed
279 * \param[in] element_name element name to which the property belongs
280 * \param[in] property_name property name
281 */
282 template <typename ScalarType> std::function<void (ScalarType)>
283 scalarPropertyDefinitionCallback (const std::string& element_name, const std::string& property_name);
284
285 /** \brief function called when a list property is parsed
286 * \param[in] element_name element name to which the property belongs
287 * \param[in] property_name list property name
288 */
289 template <typename SizeType, typename ScalarType>
290 std::tuple<std::function<void (SizeType)>, std::function<void (ScalarType)>, std::function<void ()> >
291 listPropertyDefinitionCallback (const std::string& element_name, const std::string& property_name);
292
293 /** \brief function called at the beginning of a list property parsing.
294 * \param[in] size number of elements in the list
295 */
296 template <typename SizeType> void
297 vertexListPropertyBeginCallback (const std::string& property_name, SizeType size);
298
299 /** \brief function called when a list element is parsed.
300 * \param[in] value the list's element value
301 */
302 template <typename ContentType> void
303 vertexListPropertyContentCallback (ContentType value);
304
305 /** \brief function called at the end of a list property parsing */
306 inline void
307 vertexListPropertyEndCallback ();
308
309 /** Callback function for an anonymous vertex scalar property.
310 * Writes down a double value in cloud data.
311 * param[in] value double value parsed
312 */
313 template<typename Scalar> void
314 vertexScalarPropertyCallback (Scalar value);
315
316 /** Callback function for vertex RGB color.
317 * This callback is in charge of packing red green and blue in a single int
318 * before writing it down in cloud data.
319 * param[in] color_name color name in {red, green, blue}
320 * param[in] color value of {red, green, blue} property
321 */
322 inline void
323 vertexColorCallback (const std::string& color_name, pcl::io::ply::uint8 color);
324
325 /** Callback function for vertex intensity.
326 * converts intensity from int to float before writing it down in cloud data.
327 * param[in] intensity
328 */
329 inline void
330 vertexIntensityCallback (pcl::io::ply::uint8 intensity);
331
332 /** Callback function for vertex alpha.
333 * extracts RGB value, append alpha and put it back
334 * param[in] alpha
335 */
336 inline void
337 vertexAlphaCallback (pcl::io::ply::uint8 alpha);
338
339 /** Callback function for origin x component.
340 * param[in] value origin x value
341 */
342 inline void
343 originXCallback (const float& value) { origin_[0] = value; }
344
345 /** Callback function for origin y component.
346 * param[in] value origin y value
347 */
348 inline void
349 originYCallback (const float& value) { origin_[1] = value; }
350
351 /** Callback function for origin z component.
352 * param[in] value origin z value
353 */
354 inline void
355 originZCallback (const float& value) { origin_[2] = value; }
356
357 /** Callback function for orientation x axis x component.
358 * param[in] value orientation x axis x value
359 */
360 inline void
361 orientationXaxisXCallback (const float& value) { orientation_ (0,0) = value; }
362
363 /** Callback function for orientation x axis y component.
364 * param[in] value orientation x axis y value
365 */
366 inline void
367 orientationXaxisYCallback (const float& value) { orientation_ (0,1) = value; }
368
369 /** Callback function for orientation x axis z component.
370 * param[in] value orientation x axis z value
371 */
372 inline void
373 orientationXaxisZCallback (const float& value) { orientation_ (0,2) = value; }
374
375 /** Callback function for orientation y axis x component.
376 * param[in] value orientation y axis x value
377 */
378 inline void
379 orientationYaxisXCallback (const float& value) { orientation_ (1,0) = value; }
380
381 /** Callback function for orientation y axis y component.
382 * param[in] value orientation y axis y value
383 */
384 inline void
385 orientationYaxisYCallback (const float& value) { orientation_ (1,1) = value; }
386
387 /** Callback function for orientation y axis z component.
388 * param[in] value orientation y axis z value
389 */
390 inline void
391 orientationYaxisZCallback (const float& value) { orientation_ (1,2) = value; }
392
393 /** Callback function for orientation z axis x component.
394 * param[in] value orientation z axis x value
395 */
396 inline void
397 orientationZaxisXCallback (const float& value) { orientation_ (2,0) = value; }
398
399 /** Callback function for orientation z axis y component.
400 * param[in] value orientation z axis y value
401 */
402 inline void
403 orientationZaxisYCallback (const float& value) { orientation_ (2,1) = value; }
404
405 /** Callback function for orientation z axis z component.
406 * param[in] value orientation z axis z value
407 */
408 inline void
409 orientationZaxisZCallback (const float& value) { orientation_ (2,2) = value; }
410
411 /** Callback function to set the cloud height
412 * param[in] height cloud height
413 */
414 inline void
415 cloudHeightCallback (const int &height) { cloud_->height = height; }
416
417 /** Callback function to set the cloud width
418 * param[in] width cloud width
419 */
420 inline void
421 cloudWidthCallback (const int &width) { cloud_->width = width; }
422
423 /** Append a scalar property to the cloud fields.
424 * param[in] name property name
425 * param[in] count property count: 1 for scalar properties and higher for a
426 * list property.
427 */
428 template<typename Scalar> void
429 appendScalarProperty (const std::string& name, const std::size_t& count = 1);
430
431 /** Amend property from cloud fields identified by \a old_name renaming
432 * it \a new_name.
433 * * Returns:
434 * * false on error
435 * * true success
436 * param[in] old_name property old name
437 * param[in] new_name property new name
438 */
439 bool
440 amendProperty (const std::string& old_name, const std::string& new_name, std::uint8_t datatype = 0);
441
442 /** Callback function for the begin of vertex line */
443 void
444 vertexBeginCallback ();
445
446 /** Callback function for the end of vertex line */
447 void
448 vertexEndCallback ();
449
450 /** Callback function for the begin of range_grid line */
451 void
452 rangeGridBeginCallback ();
453
454 /** Callback function for the begin of range_grid vertex_indices property
455 * param[in] size vertex_indices list size
456 */
457 void
458 rangeGridVertexIndicesBeginCallback (pcl::io::ply::uint8 size);
459
460 /** Callback function for each range_grid vertex_indices element
461 * param[in] vertex_index index of the vertex in vertex_indices
462 */
463 void
464 rangeGridVertexIndicesElementCallback (pcl::io::ply::int32 vertex_index);
465
466 /** Callback function for the end of a range_grid vertex_indices property */
467 void
468 rangeGridVertexIndicesEndCallback ();
469
470 /** Callback function for the end of a range_grid element end */
471 void
472 rangeGridEndCallback ();
473
474 /** Callback function for obj_info */
475 void
476 objInfoCallback (const std::string& line);
477
478 /** Callback function for the begin of face line */
479 void
480 faceBeginCallback ();
481
482 /** Callback function for the begin of face vertex_indices property
483 * param[in] size vertex_indices list size
484 */
485 void
486 faceVertexIndicesBeginCallback (pcl::io::ply::uint8 size);
487
488 /** Callback function for each face vertex_indices element
489 * param[in] vertex_index index of the vertex in vertex_indices
490 */
491 void
492 faceVertexIndicesElementCallback (pcl::io::ply::int32 vertex_index);
493
494 /** Callback function for the end of a face vertex_indices property */
495 void
496 faceVertexIndicesEndCallback ();
497
498 /** Callback function for the end of a face element end */
499 void
500 faceEndCallback ();
501
502 /// origin
503 Eigen::Vector4f origin_;
504
505 /// orientation
506 Eigen::Matrix3f orientation_;
507
508 //vertex element artifacts
509 pcl::PCLPointCloud2 *cloud_{nullptr};
510 std::size_t vertex_count_{0};
511 int vertex_offset_before_{0};
512 //range element artifacts
513 std::vector<std::vector <int> > *range_grid_{nullptr};
514 std::size_t rgb_offset_before_{0};
515 bool do_resize_{false};
516 //face element artifact
517 std::vector<pcl::Vertices> *polygons_{nullptr};
518 public:
520
521 private:
522 // RGB values stored by vertexColorCallback()
523 std::int32_t r_{0}, g_{0}, b_{0};
524 // Color values stored by vertexAlphaCallback()
525 std::uint32_t a_{0}, rgba_{0};
526 };
527
528 /** \brief Point Cloud Data (PLY) file format writer.
529 * \author Nizar Sallem
530 * \ingroup io
531 */
532 class PCL_EXPORTS PLYWriter : public FileWriter
533 {
534 public:
535 ///Constructor
536 PLYWriter () = default;
537
538 ///Destructor
539 ~PLYWriter () override = default;
540
541 /** \brief Generate the header of a PLY v.7 file format
542 * \param[in] cloud the point cloud data message
543 * \param[in] origin the sensor data acquisition origin (translation)
544 * \param[in] orientation the sensor data acquisition origin (rotation)
545 * \param[in] valid_points number of valid points (finite ones for range_grid and
546 * all of them for camer)
547 * \param[in] use_camera if set to true then PLY file will use element camera else
548 * element range_grid will be used.
549 */
550 inline std::string
552 const Eigen::Vector4f &origin,
553 const Eigen::Quaternionf &orientation,
554 int valid_points,
555 bool use_camera = true)
556 {
557 return (generateHeader (cloud, origin, orientation, true, use_camera, valid_points));
558 }
559
560 /** \brief Generate the header of a PLY v.7 file format
561 * \param[in] cloud the point cloud data message
562 * \param[in] origin the sensor data acquisition origin (translation)
563 * \param[in] orientation the sensor data acquisition origin (rotation)
564 * \param[in] valid_points number of valid points (finite ones for range_grid and
565 * all of them for camer)
566 * \param[in] use_camera if set to true then PLY file will use element camera else
567 * element range_grid will be used.
568 */
569 inline std::string
571 const Eigen::Vector4f &origin,
572 const Eigen::Quaternionf &orientation,
573 int valid_points,
574 bool use_camera = true)
575 {
576 return (generateHeader (cloud, origin, orientation, false, use_camera, valid_points));
577 }
578
579 /** \brief Save point cloud data to a PLY file containing n-D points, in ASCII format
580 * \param[in] file_name the output file name
581 * \param[in] cloud the point cloud data message
582 * \param[in] origin the sensor data acquisition origin (translation)
583 * \param[in] orientation the sensor data acquisition origin (rotation)
584 * \param[in] precision the specified output numeric stream precision (default: 8)
585 * \param[in] use_camera if set to true then PLY file will use element camera else
586 * element range_grid will be used.
587 */
588 int
589 writeASCII (const std::string &file_name, const pcl::PCLPointCloud2 &cloud,
590 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
591 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
592 int precision = 8,
593 bool use_camera = true);
594 /** \brief Save point cloud data to a std::ostream containing n-D points, in BINARY format
595 * \param[in] os the output buffer
596 * \param[in] cloud the point cloud data message
597 * \param[in] origin the sensor data acquisition origin
598 * (translation) \param[in] orientation the sensor data acquisition origin
599 * (rotation) \param[in] use_camera if set to true then PLY file will use element
600 * camera else element range_grid will be used
601 */
602 int
603 writeBinary(std::ostream& os, const pcl::PCLPointCloud2& cloud,
604 const Eigen::Vector4f& origin = Eigen::Vector4f::Zero (), const Eigen::Quaternionf& orientation= Eigen::Quaternionf::Identity (),
605 bool use_camera=true);
606
607 /** \brief Save point cloud data to a PLY file containing n-D points, in BINARY format
608 * \param[in] file_name the output file name
609 * \param[in] cloud the point cloud data message
610 * \param[in] origin the sensor data acquisition origin (translation)
611 * \param[in] orientation the sensor data acquisition origin (rotation)
612 * \param[in] use_camera if set to true then PLY file will use element camera else
613 * element range_grid will be used
614 */
615 int
616 writeBinary (const std::string &file_name, const pcl::PCLPointCloud2 &cloud,
617 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
618 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
619 bool use_camera = true);
620
621 /** \brief Save point cloud data to a PLY file containing n-D points
622 * \param[in] file_name the output file name
623 * \param[in] cloud the point cloud data message
624 * \param[in] origin the sensor acquisition origin
625 * \param[in] orientation the sensor acquisition orientation
626 * \param[in] binary set to true if the file is to be written in a binary
627 * PLY format, false (default) for ASCII
628 */
629 inline int
630 write (const std::string &file_name, const pcl::PCLPointCloud2 &cloud,
631 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
632 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
633 const bool binary = false) override
634 {
635 if (binary)
636 return (this->writeBinary (file_name, cloud, origin, orientation, true));
637 return (this->writeASCII (file_name, cloud, origin, orientation, 8, true));
638 }
639
640 /** \brief Save point cloud data to a PLY file containing n-D points
641 * \param[in] file_name the output file name
642 * \param[in] cloud the point cloud data message
643 * \param[in] origin the sensor acquisition origin
644 * \param[in] orientation the sensor acquisition orientation
645 * \param[in] binary set to true if the file is to be written in a binary
646 * PLY format, false (default) for ASCII
647 * \param[in] use_camera set to true to use camera element and false to
648 * use range_grid element
649 */
650 inline int
651 write (const std::string &file_name, const pcl::PCLPointCloud2 &cloud,
652 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
653 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
654 bool binary = false,
655 bool use_camera = true)
656 {
657 if (binary)
658 return (this->writeBinary (file_name, cloud, origin, orientation, use_camera));
659 return (this->writeASCII (file_name, cloud, origin, orientation, 8, use_camera));
660 }
661
662 /** \brief Save point cloud data to a PLY file containing n-D points
663 * \param[in] file_name the output file name
664 * \param[in] cloud the point cloud data message (boost shared pointer)
665 * \param[in] origin the sensor acquisition origin
666 * \param[in] orientation the sensor acquisition orientation
667 * \param[in] binary set to true if the file is to be written in a binary
668 * PLY format, false (default) for ASCII
669 * \param[in] use_camera set to true to use camera element and false to
670 * use range_grid element
671 */
672 inline int
673 write (const std::string &file_name, const pcl::PCLPointCloud2::ConstPtr &cloud,
674 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
675 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
676 bool binary = false,
677 bool use_camera = true)
678 {
679 return (write (file_name, *cloud, origin, orientation, binary, use_camera));
680 }
681
682 /** \brief Save point cloud data to a PLY file containing n-D points
683 * \param[in] file_name the output file name
684 * \param[in] cloud the pcl::PointCloud data
685 * \param[in] binary set to true if the file is to be written in a binary
686 * PLY format, false (default) for ASCII
687 * \param[in] use_camera set to true to use camera element and false to
688 * use range_grid element
689 */
690 template<typename PointT> inline int
691 write (const std::string &file_name,
692 const pcl::PointCloud<PointT> &cloud,
693 bool binary = false,
694 bool use_camera = true)
695 {
696 Eigen::Vector4f origin = cloud.sensor_origin_;
697 Eigen::Quaternionf orientation = cloud.sensor_orientation_;
698
700 pcl::toPCLPointCloud2 (cloud, blob);
701
702 // Save the data
703 return (this->write (file_name, blob, origin, orientation, binary, use_camera));
704 }
705
706 private:
707 /** \brief Generate a PLY header.
708 * \param[in] cloud the input point cloud
709 * \param[in] binary whether the PLY file should be saved as binary data (true) or ascii (false)
710 */
711 std::string
712 generateHeader (const pcl::PCLPointCloud2 &cloud,
713 const Eigen::Vector4f &origin,
714 const Eigen::Quaternionf &orientation,
715 bool binary,
716 bool use_camera,
717 int valid_points);
718
719 void
720 writeContentWithCameraASCII (int nr_points,
721 const pcl::PCLPointCloud2 &cloud,
722 const Eigen::Vector4f &origin,
723 const Eigen::Quaternionf &orientation,
724 std::ofstream& fs);
725
726 void
727 writeContentWithRangeGridASCII (int nr_points,
728 const pcl::PCLPointCloud2 &cloud,
729 std::ostringstream& fs,
730 int& nb_valid_points);
731 };
732
733 namespace io
734 {
735 /** \brief Load a PLY v.6 file into a PCLPointCloud2 type.
736 *
737 * Any PLY files containing sensor data will generate a warning as a
738 * pcl/PCLPointCloud2 message cannot hold the sensor origin.
739 *
740 * \param[in] file_name the name of the file to load
741 * \param[in] cloud the resultant templated point cloud
742 * \ingroup io
743 */
744 inline int
745 loadPLYFile (const std::string &file_name, pcl::PCLPointCloud2 &cloud)
746 {
748 return (p.read (file_name, cloud));
749 }
750
751 /** \brief Load any PLY file into a PCLPointCloud2 type.
752 * \param[in] file_name the name of the file to load
753 * \param[out] cloud the resultant templated point cloud
754 * \param[out] origin the sensor acquisition origin (only for > PLY_V7 - null if not present)
755 * \param[out] orientation the sensor acquisition orientation if available,
756 * identity if not present
757 * \ingroup io
758 */
759 inline int
760 loadPLYFile (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
761 Eigen::Vector4f &origin, Eigen::Quaternionf &orientation)
762 {
764 int ply_version;
765 return (p.read (file_name, cloud, origin, orientation, ply_version));
766 }
767
768 /** \brief Load any PLY file into a templated PointCloud type
769 * \param[in] file_name the name of the file to load
770 * \param[in] cloud the resultant templated point cloud
771 * \ingroup io
772 */
773 template<typename PointT> inline int
774 loadPLYFile (const std::string &file_name, pcl::PointCloud<PointT> &cloud)
775 {
777 return (p.read (file_name, cloud));
778 }
779
780 /** \brief Load a PLY file into a PolygonMesh type.
781 *
782 * Any PLY files containing sensor data will generate a warning as a
783 * pcl/PolygonMesh message cannot hold the sensor origin.
784 *
785 * \param[in] file_name the name of the file to load
786 * \param[in] mesh the resultant polygon mesh
787 * \ingroup io
788 */
789 inline int
790 loadPLYFile (const std::string &file_name, pcl::PolygonMesh &mesh)
791 {
793 return (p.read (file_name, mesh));
794 }
795
796 /** \brief Save point cloud data to a PLY file containing n-D points
797 * \param[in] file_name the output file name
798 * \param[in] cloud the point cloud data message
799 * \param[in] origin the sensor data acquisition origin (translation)
800 * \param[in] orientation the sensor data acquisition origin (rotation)
801 * \param[in] binary_mode true for binary mode, false (default) for ASCII
802 * \param[in] use_camera
803 * \ingroup io
804 */
805 inline int
806 savePLYFile (const std::string &file_name, const pcl::PCLPointCloud2 &cloud,
807 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
808 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
809 bool binary_mode = false, bool use_camera = true)
810 {
811 PLYWriter w;
812 return (w.write (file_name, cloud, origin, orientation, binary_mode, use_camera));
813 }
814
815 /** \brief Templated version for saving point cloud data to a PLY file
816 * containing a specific given cloud format
817 * \param[in] file_name the output file name
818 * \param[in] cloud the point cloud data message
819 * \param[in] binary_mode true for binary mode, false (default) for ASCII
820 * \ingroup io
821 */
822 template<typename PointT> inline int
823 savePLYFile (const std::string &file_name, const pcl::PointCloud<PointT> &cloud, bool binary_mode = false)
824 {
825 PLYWriter w;
826 return (w.write<PointT> (file_name, cloud, binary_mode));
827 }
828
829 /** \brief Templated version for saving point cloud data to a PLY file
830 * containing a specific given cloud format.
831 * \param[in] file_name the output file name
832 * \param[in] cloud the point cloud data message
833 * \ingroup io
834 */
835 template<typename PointT> inline int
836 savePLYFileASCII (const std::string &file_name, const pcl::PointCloud<PointT> &cloud)
837 {
838 PLYWriter w;
839 return (w.write<PointT> (file_name, cloud, false));
840 }
841
842 /** \brief Templated version for saving point cloud data to a PLY file containing a specific given cloud format.
843 * \param[in] file_name the output file name
844 * \param[in] cloud the point cloud data message
845 * \ingroup io
846 */
847 template<typename PointT> inline int
848 savePLYFileBinary (const std::string &file_name, const pcl::PointCloud<PointT> &cloud)
849 {
850 PLYWriter w;
851 return (w.write<PointT> (file_name, cloud, true));
852 }
853
854 /** \brief Templated version for saving point cloud data to a PLY file containing a specific given cloud format
855 * \param[in] file_name the output file name
856 * \param[in] cloud the point cloud data message
857 * \param[in] indices the set of indices to save
858 * \param[in] binary_mode true for binary mode, false (default) for ASCII
859 * \ingroup io
860 */
861 template<typename PointT> int
862 savePLYFile (const std::string &file_name, const pcl::PointCloud<PointT> &cloud,
863 const pcl::Indices &indices, bool binary_mode = false)
864 {
865 // Copy indices to a new point cloud
866 pcl::PointCloud<PointT> cloud_out;
867 copyPointCloud (cloud, indices, cloud_out);
868 // Save the data
869 PLYWriter w;
870 return (w.write<PointT> (file_name, cloud_out, binary_mode));
871 }
872
873 /** \brief Saves a PolygonMesh in ascii PLY format.
874 * \param[in] file_name the name of the file to write to disk
875 * \param[in] mesh the polygonal mesh to save
876 * \param[in] precision the output ASCII precision default 5
877 * \ingroup io
878 */
879 PCL_EXPORTS int
880 savePLYFile (const std::string &file_name, const pcl::PolygonMesh &mesh, unsigned precision = 5);
881
882 /** \brief Saves a PolygonMesh in binary PLY format.
883 * \param[in] file_name the name of the file to write to disk
884 * \param[in] mesh the polygonal mesh to save
885 * \ingroup io
886 */
887 PCL_EXPORTS int
888 savePLYFileBinary (const std::string &file_name, const pcl::PolygonMesh &mesh);
889 }
890}
Point Cloud Data (FILE) file format reader interface.
Definition file_io.h:57
Point Cloud Data (FILE) file format writer.
Definition file_io.h:163
Point Cloud Data (PLY) file format reader.
Definition ply_io.h:81
int read(const std::string &file_name, pcl::PCLPointCloud2 &cloud, const int offset=0)
Read a point cloud data from a PLY file and store it into a pcl/PCLPointCloud2.
Definition ply_io.h:166
int read(const std::string &file_name, pcl::PointCloud< PointT > &cloud, const int offset=0)
Read a point cloud data from any PLY file, and convert it to the given template format.
Definition ply_io.h:184
int read(const std::string &file_name, pcl::PolygonMesh &mesh, const int offset=0)
Read a point cloud data from a PLY file and store it into a pcl/PolygonMesh.
~PLYReader() override
Definition ply_io.h:111
int read(const std::string &file_name, pcl::PCLPointCloud2 &cloud, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &ply_version, const int offset=0) override
Read a point cloud data from a PLY file and store it into a pcl/PCLPointCloud2.
PLYReader(const PLYReader &p)
Definition ply_io.h:94
int readHeader(const std::string &file_name, pcl::PCLPointCloud2 &cloud, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &ply_version, int &data_type, unsigned int &data_idx, const int offset=0) override
Read a point cloud data header from a PLY file.
int read(const std::string &file_name, pcl::PolygonMesh &mesh, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &ply_version, const int offset=0)
Read a point cloud data from a PLY file and store it into a pcl/PolygonMesh.
Point Cloud Data (PLY) file format writer.
Definition ply_io.h:533
std::string generateHeaderBinary(const pcl::PCLPointCloud2 &cloud, const Eigen::Vector4f &origin, const Eigen::Quaternionf &orientation, int valid_points, bool use_camera=true)
Generate the header of a PLY v.7 file format.
Definition ply_io.h:551
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(), bool binary=false, bool use_camera=true)
Save point cloud data to a PLY file containing n-D points.
Definition ply_io.h:651
int writeBinary(const std::string &file_name, const pcl::PCLPointCloud2 &cloud, const Eigen::Vector4f &origin=Eigen::Vector4f::Zero(), const Eigen::Quaternionf &orientation=Eigen::Quaternionf::Identity(), bool use_camera=true)
Save point cloud data to a PLY file containing n-D points, in BINARY format.
~PLYWriter() override=default
Destructor.
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 PLY file containing n-D points.
Definition ply_io.h:630
int write(const std::string &file_name, const pcl::PointCloud< PointT > &cloud, bool binary=false, bool use_camera=true)
Save point cloud data to a PLY file containing n-D points.
Definition ply_io.h:691
int writeBinary(std::ostream &os, const pcl::PCLPointCloud2 &cloud, const Eigen::Vector4f &origin=Eigen::Vector4f::Zero(), const Eigen::Quaternionf &orientation=Eigen::Quaternionf::Identity(), bool use_camera=true)
Save point cloud data to a std::ostream containing n-D points, in BINARY format.
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(), bool binary=false, bool use_camera=true)
Save point cloud data to a PLY file containing n-D points.
Definition ply_io.h:673
std::string generateHeaderASCII(const pcl::PCLPointCloud2 &cloud, const Eigen::Vector4f &origin, const Eigen::Quaternionf &orientation, int valid_points, bool use_camera=true)
Generate the header of a PLY v.7 file format.
Definition ply_io.h:570
int writeASCII(const std::string &file_name, const pcl::PCLPointCloud2 &cloud, const Eigen::Vector4f &origin=Eigen::Vector4f::Zero(), const Eigen::Quaternionf &orientation=Eigen::Quaternionf::Identity(), int precision=8, bool use_camera=true)
Save point cloud data to a PLY file containing n-D points, in ASCII format.
PLYWriter()=default
Constructor.
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).
Class ply_parser parses a PLY file and generates appropriate atomic parsers for the body.
Definition ply_parser.h:74
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition memory.h:86
void copyPointCloud(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Copy all the fields from a given point cloud into a new point cloud.
Definition io.hpp:142
Defines functions, macros and traits for allocating and using memory.
Definition bfgs.h:10
std::int32_t int32
Definition ply.h:60
std::uint8_t uint8
Definition ply.h:61
void read(std::istream &stream, Type &value)
Function for reading data from a stream.
Definition region_xy.h:46
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.
void toPCLPointCloud2(const pcl::PointCloud< PointT > &cloud, pcl::PCLPointCloud2 &msg, bool padding)
Convert a pcl::PointCloud<T> object to a PCLPointCloud2 binary data blob.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
void write(std::ostream &stream, Type value)
Function for writing data to a stream.
Definition region_xy.h:63
Defines all the PCL and non-PCL macros used.
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr
A point structure representing Euclidean xyz coordinates, and the RGB color.