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