Point Cloud Library (PCL) 1.13.0
pcd_io.hpp
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 Willow Garage, Inc. 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#ifndef PCL_IO_PCD_IO_IMPL_H_
41#define PCL_IO_PCD_IO_IMPL_H_
42
43#include <boost/algorithm/string/trim.hpp> // for trim
44#include <fstream>
45#include <fcntl.h>
46#include <string>
47#include <cstdlib>
48#include <pcl/common/io.h> // for getFields, ...
49#include <pcl/console/print.h>
50#include <pcl/io/low_level_io.h>
51#include <pcl/io/pcd_io.h>
52
53#include <pcl/io/lzf.h>
54
55/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
56template <typename PointT> std::string
58{
59 std::ostringstream oss;
60 oss.imbue (std::locale::classic ());
61
62 oss << "# .PCD v0.7 - Point Cloud Data file format"
63 "\nVERSION 0.7"
64 "\nFIELDS";
65
66 const auto fields = pcl::getFields<PointT> ();
67
68 std::stringstream field_names, field_types, field_sizes, field_counts;
69 for (const auto &field : fields)
70 {
71 if (field.name == "_")
72 continue;
73 // Add the regular dimension
74 field_names << " " << field.name;
75 field_sizes << " " << pcl::getFieldSize (field.datatype);
76 if ("rgb" == field.name)
77 field_types << " " << "U";
78 else
79 field_types << " " << pcl::getFieldType (field.datatype);
80 int count = std::abs (static_cast<int> (field.count));
81 if (count == 0) count = 1; // check for 0 counts (coming from older converter code)
82 field_counts << " " << count;
83 }
84 oss << field_names.str ();
85 oss << "\nSIZE" << field_sizes.str ()
86 << "\nTYPE" << field_types.str ()
87 << "\nCOUNT" << field_counts.str ();
88 // If the user passes in a number of points value, use that instead
89 if (nr_points != std::numeric_limits<int>::max ())
90 oss << "\nWIDTH " << nr_points << "\nHEIGHT " << 1 << "\n";
91 else
92 oss << "\nWIDTH " << cloud.width << "\nHEIGHT " << cloud.height << "\n";
93
94 oss << "VIEWPOINT " << cloud.sensor_origin_[0] << " " << cloud.sensor_origin_[1] << " " << cloud.sensor_origin_[2] << " " <<
95 cloud.sensor_orientation_.w () << " " <<
96 cloud.sensor_orientation_.x () << " " <<
97 cloud.sensor_orientation_.y () << " " <<
98 cloud.sensor_orientation_.z () << "\n";
99
100 // If the user passes in a number of points value, use that instead
101 if (nr_points != std::numeric_limits<int>::max ())
102 oss << "POINTS " << nr_points << "\n";
103 else
104 oss << "POINTS " << cloud.size () << "\n";
105
106 return (oss.str ());
107}
108
109/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
110template <typename PointT> int
111pcl::PCDWriter::writeBinary (const std::string &file_name,
112 const pcl::PointCloud<PointT> &cloud)
113{
114 if (cloud.empty ())
115 {
116 PCL_WARN ("[pcl::PCDWriter::writeBinary] Input point cloud has no data!\n");
117 }
118 int data_idx = 0;
119 std::ostringstream oss;
120 oss << generateHeader<PointT> (cloud) << "DATA binary\n";
121 oss.flush ();
122 data_idx = static_cast<int> (oss.tellp ());
123
124#ifdef _WIN32
125 HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
126 if (h_native_file == INVALID_HANDLE_VALUE)
127 {
128 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during CreateFile!");
129 return (-1);
130 }
131#else
132 int fd = io::raw_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
133 if (fd < 0)
134 {
135 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during open!");
136 return (-1);
137 }
138#endif
139 // Mandatory lock file
140 boost::interprocess::file_lock file_lock;
141 setLockingPermissions (file_name, file_lock);
142
143 auto fields = pcl::getFields<PointT> ();
144 std::vector<int> fields_sizes;
145 std::size_t fsize = 0;
146 std::size_t data_size = 0;
147 std::size_t nri = 0;
148 // Compute the total size of the fields
149 for (const auto &field : fields)
150 {
151 if (field.name == "_")
152 continue;
153
154 int fs = field.count * getFieldSize (field.datatype);
155 fsize += fs;
156 fields_sizes.push_back (fs);
157 fields[nri++] = field;
158 }
159 fields.resize (nri);
160
161 data_size = cloud.size () * fsize;
162
163 // Prepare the map
164#ifdef _WIN32
165 HANDLE fm = CreateFileMappingA (h_native_file, NULL, PAGE_READWRITE, 0, (DWORD) (data_idx + data_size), NULL);
166 if (fm == NULL)
167 {
168 throw pcl::IOException("[pcl::PCDWriter::writeBinary] Error during memory map creation ()!");
169 return (-1);
170 }
171 char *map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, data_idx + data_size));
172 CloseHandle (fm);
173
174#else
175 // Allocate disk space for the entire file to prevent bus errors.
176 const int allocate_res = io::raw_fallocate (fd, data_idx + data_size);
177 if (allocate_res != 0)
178 {
179 io::raw_close (fd);
180 resetLockingPermissions (file_name, file_lock);
181 PCL_ERROR ("[pcl::PCDWriter::writeBinary] raw_fallocate(length=%zu) returned %i. errno: %d strerror: %s\n",
182 data_idx + data_size, allocate_res, errno, strerror (errno));
183
184 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during raw_fallocate ()!");
185 return (-1);
186 }
187
188 char *map = static_cast<char*> (::mmap (nullptr, data_idx + data_size, PROT_WRITE, MAP_SHARED, fd, 0));
189 if (map == reinterpret_cast<char*> (-1)) //MAP_FAILED)
190 {
191 io::raw_close (fd);
192 resetLockingPermissions (file_name, file_lock);
193 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during mmap ()!");
194 return (-1);
195 }
196#endif
197
198 // Copy the header
199 memcpy (&map[0], oss.str ().c_str (), data_idx);
200
201 // Copy the data
202 char *out = &map[0] + data_idx;
203 for (const auto& point: cloud)
204 {
205 int nrj = 0;
206 for (const auto &field : fields)
207 {
208 memcpy (out, reinterpret_cast<const char*> (&point) + field.offset, fields_sizes[nrj]);
209 out += fields_sizes[nrj++];
210 }
211 }
212
213 // If the user set the synchronization flag on, call msync
214#ifndef _WIN32
215 if (map_synchronization_)
216 msync (map, data_idx + data_size, MS_SYNC);
217#endif
218
219 // Unmap the pages of memory
220#ifdef _WIN32
221 UnmapViewOfFile (map);
222#else
223 if (::munmap (map, (data_idx + data_size)) == -1)
224 {
225 io::raw_close (fd);
226 resetLockingPermissions (file_name, file_lock);
227 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during munmap ()!");
228 return (-1);
229 }
230#endif
231 // Close file
232#ifdef _WIN32
233 CloseHandle (h_native_file);
234#else
235 io::raw_close (fd);
236#endif
237 resetLockingPermissions (file_name, file_lock);
238 return (0);
239}
240
241/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
242template <typename PointT> int
243pcl::PCDWriter::writeBinaryCompressed (const std::string &file_name,
244 const pcl::PointCloud<PointT> &cloud)
245{
246 if (cloud.empty ())
247 {
248 PCL_WARN ("[pcl::PCDWriter::writeBinaryCompressed] Input point cloud has no data!\n");
249 }
250 int data_idx = 0;
251 std::ostringstream oss;
252 oss << generateHeader<PointT> (cloud) << "DATA binary_compressed\n";
253 oss.flush ();
254 data_idx = static_cast<int> (oss.tellp ());
255
256#ifdef _WIN32
257 HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
258 if (h_native_file == INVALID_HANDLE_VALUE)
259 {
260 throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during CreateFile!");
261 return (-1);
262 }
263#else
264 int fd = io::raw_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
265 if (fd < 0)
266 {
267 throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during open!");
268 return (-1);
269 }
270#endif
271
272 // Mandatory lock file
273 boost::interprocess::file_lock file_lock;
274 setLockingPermissions (file_name, file_lock);
275
276 auto fields = pcl::getFields<PointT> ();
277 std::size_t fsize = 0;
278 std::size_t data_size = 0;
279 std::size_t nri = 0;
280 std::vector<int> fields_sizes (fields.size ());
281 // Compute the total size of the fields
282 for (const auto &field : fields)
283 {
284 if (field.name == "_")
285 continue;
286
287 fields_sizes[nri] = field.count * pcl::getFieldSize (field.datatype);
288 fsize += fields_sizes[nri];
289 fields[nri] = field;
290 ++nri;
291 }
292 fields_sizes.resize (nri);
293 fields.resize (nri);
294
295 // Compute the size of data
296 data_size = cloud.size () * fsize;
297
298 // If the data is to large the two 32 bit integers used to store the
299 // compressed and uncompressed size will overflow.
300 if (data_size * 3 / 2 > std::numeric_limits<std::uint32_t>::max ())
301 {
302 PCL_ERROR ("[pcl::PCDWriter::writeBinaryCompressed] The input data exceeds the maximum size for compressed version 0.7 pcds of %l bytes.\n",
303 static_cast<std::size_t> (std::numeric_limits<std::uint32_t>::max ()) * 2 / 3);
304 return (-2);
305 }
306
307 //////////////////////////////////////////////////////////////////////
308 // Empty array holding only the valid data
309 // data_size = nr_points * point_size
310 // = nr_points * (sizeof_field_1 + sizeof_field_2 + ... sizeof_field_n)
311 // = sizeof_field_1 * nr_points + sizeof_field_2 * nr_points + ... sizeof_field_n * nr_points
312 char *only_valid_data = static_cast<char*> (malloc (data_size));
313
314 // Convert the XYZRGBXYZRGB structure to XXYYZZRGBRGB to aid compression. For
315 // this, we need a vector of fields.size () (4 in this case), which points to
316 // each individual plane:
317 // pters[0] = &only_valid_data[offset_of_plane_x];
318 // pters[1] = &only_valid_data[offset_of_plane_y];
319 // pters[2] = &only_valid_data[offset_of_plane_z];
320 // pters[3] = &only_valid_data[offset_of_plane_RGB];
321 //
322 std::vector<char*> pters (fields.size ());
323 std::size_t toff = 0;
324 for (std::size_t i = 0; i < pters.size (); ++i)
325 {
326 pters[i] = &only_valid_data[toff];
327 toff += static_cast<std::size_t>(fields_sizes[i]) * cloud.size();
328 }
329
330 // Go over all the points, and copy the data in the appropriate places
331 for (const auto& point: cloud)
332 {
333 for (std::size_t j = 0; j < fields.size (); ++j)
334 {
335 memcpy (pters[j], reinterpret_cast<const char*> (&point) + fields[j].offset, fields_sizes[j]);
336 // Increment the pointer
337 pters[j] += fields_sizes[j];
338 }
339 }
340
341 char* temp_buf = static_cast<char*> (malloc (static_cast<std::size_t> (static_cast<float> (data_size) * 1.5f + 8.0f)));
342 unsigned int compressed_final_size = 0;
343 if (data_size != 0) {
344 // Compress the valid data
345 unsigned int compressed_size = pcl::lzfCompress (only_valid_data,
346 static_cast<std::uint32_t> (data_size),
347 &temp_buf[8],
348 static_cast<std::uint32_t> (static_cast<float>(data_size) * 1.5f));
349 // Was the compression successful?
350 if (compressed_size > 0)
351 {
352 char *header = &temp_buf[0];
353 memcpy (&header[0], &compressed_size, sizeof (unsigned int));
354 memcpy (&header[4], &data_size, sizeof (unsigned int));
355 data_size = compressed_size + 8;
356 compressed_final_size = static_cast<std::uint32_t> (data_size) + data_idx;
357 }
358 else
359 {
360 #ifndef _WIN32
361 io::raw_close (fd);
362 #endif
363 resetLockingPermissions (file_name, file_lock);
364 PCL_WARN("[pcl::PCDWriter::writeBinaryCompressed] Error during compression!\n");
365 return (-1);
366 }
367 }
368 else
369 {
370 // empty cloud case
371 compressed_final_size = 8 + data_idx;
372 auto *header = reinterpret_cast<std::uint32_t*>(&temp_buf[0]);
373 header[0] = 0; // compressed_size is 0
374 header[1] = 0; // data_size is 0
375 data_size = 8; // correct data_size to header size
376 }
377
378 // Prepare the map
379#ifdef _WIN32
380 HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, compressed_final_size, NULL);
381 char *map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, compressed_final_size));
382 CloseHandle (fm);
383
384#else
385 // Allocate disk space for the entire file to prevent bus errors.
386 const int allocate_res = io::raw_fallocate (fd, compressed_final_size);
387 if (allocate_res != 0)
388 {
389 io::raw_close (fd);
390 resetLockingPermissions (file_name, file_lock);
391 PCL_ERROR ("[pcl::PCDWriter::writeBinaryCompressed] raw_fallocate(length=%u) returned %i. errno: %d strerror: %s\n",
392 compressed_final_size, allocate_res, errno, strerror (errno));
393
394 throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during raw_fallocate ()!");
395 return (-1);
396 }
397
398 char *map = static_cast<char*> (::mmap (nullptr, compressed_final_size, PROT_WRITE, MAP_SHARED, fd, 0));
399 if (map == reinterpret_cast<char*> (-1)) //MAP_FAILED)
400 {
401 io::raw_close (fd);
402 resetLockingPermissions (file_name, file_lock);
403 throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during mmap ()!");
404 return (-1);
405 }
406#endif
407
408 // Copy the header
409 memcpy (&map[0], oss.str ().c_str (), data_idx);
410 // Copy the compressed data
411 memcpy (&map[data_idx], temp_buf, data_size);
412
413#ifndef _WIN32
414 // If the user set the synchronization flag on, call msync
415 if (map_synchronization_)
416 msync (map, compressed_final_size, MS_SYNC);
417#endif
418
419 // Unmap the pages of memory
420#ifdef _WIN32
421 UnmapViewOfFile (map);
422#else
423 if (::munmap (map, (compressed_final_size)) == -1)
424 {
425 io::raw_close (fd);
426 resetLockingPermissions (file_name, file_lock);
427 throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during munmap ()!");
428 return (-1);
429 }
430#endif
431
432 // Close file
433#ifdef _WIN32
434 CloseHandle (h_native_file);
435#else
436 io::raw_close (fd);
437#endif
438 resetLockingPermissions (file_name, file_lock);
439
440 free (only_valid_data);
441 free (temp_buf);
442 return (0);
443}
444
445//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
446template <typename PointT> int
447pcl::PCDWriter::writeASCII (const std::string &file_name, const pcl::PointCloud<PointT> &cloud,
448 const int precision)
449{
450 if (cloud.empty ())
451 {
452 PCL_WARN ("[pcl::PCDWriter::writeASCII] Input point cloud has no data!\n");
453 }
454
455 if (cloud.width * cloud.height != cloud.size ())
456 {
457 throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Number of points different than width * height!");
458 return (-1);
459 }
460
461 std::ofstream fs;
462 fs.open (file_name.c_str (), std::ios::binary); // Open file
463
464 if (!fs.is_open () || fs.fail ())
465 {
466 throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Could not open file for writing!");
467 return (-1);
468 }
469
470 // Mandatory lock file
471 boost::interprocess::file_lock file_lock;
472 setLockingPermissions (file_name, file_lock);
473
474 fs.precision (precision);
475 fs.imbue (std::locale::classic ());
476
477 const auto fields = pcl::getFields<PointT> ();
478
479 // Write the header information
480 fs << generateHeader<PointT> (cloud) << "DATA ascii\n";
481
482 std::ostringstream stream;
483 stream.precision (precision);
484 stream.imbue (std::locale::classic ());
485 // Iterate through the points
486 for (const auto& point: cloud)
487 {
488 for (std::size_t d = 0; d < fields.size (); ++d)
489 {
490 // Ignore invalid padded dimensions that are inherited from binary data
491 if (fields[d].name == "_")
492 continue;
493
494 int count = fields[d].count;
495 if (count == 0)
496 count = 1; // we simply cannot tolerate 0 counts (coming from older converter code)
497
498 for (int c = 0; c < count; ++c)
499 {
500 switch (fields[d].datatype)
501 {
503 {
504 bool value;
505 memcpy (&value, reinterpret_cast<const char*> (&point) + fields[d].offset + c * sizeof (bool), sizeof (bool));
506 stream << boost::numeric_cast<std::int32_t>(value);
507 break;
508 }
510 {
511 std::int8_t value;
512 memcpy (&value, reinterpret_cast<const char*> (&point) + fields[d].offset + c * sizeof (std::int8_t), sizeof (std::int8_t));
513 stream << boost::numeric_cast<std::int32_t>(value);
514 break;
515 }
517 {
518 std::uint8_t value;
519 memcpy (&value, reinterpret_cast<const char*> (&point) + fields[d].offset + c * sizeof (std::uint8_t), sizeof (std::uint8_t));
520 stream << boost::numeric_cast<std::uint32_t>(value);
521 break;
522 }
524 {
525 std::int16_t value;
526 memcpy (&value, reinterpret_cast<const char*> (&point) + fields[d].offset + c * sizeof (std::int16_t), sizeof (std::int16_t));
527 stream << boost::numeric_cast<std::int16_t>(value);
528 break;
529 }
531 {
532 std::uint16_t value;
533 memcpy (&value, reinterpret_cast<const char*> (&point) + fields[d].offset + c * sizeof (std::uint16_t), sizeof (std::uint16_t));
534 stream << boost::numeric_cast<std::uint16_t>(value);
535 break;
536 }
538 {
539 std::int32_t value;
540 memcpy (&value, reinterpret_cast<const char*> (&point) + fields[d].offset + c * sizeof (std::int32_t), sizeof (std::int32_t));
541 stream << boost::numeric_cast<std::int32_t>(value);
542 break;
543 }
545 {
546 std::uint32_t value;
547 memcpy (&value, reinterpret_cast<const char*> (&point) + fields[d].offset + c * sizeof (std::uint32_t), sizeof (std::uint32_t));
548 stream << boost::numeric_cast<std::uint32_t>(value);
549 break;
550 }
552 {
553 std::int64_t value;
554 memcpy (&value, reinterpret_cast<const char*> (&point) + fields[d].offset + c * sizeof (std::int64_t), sizeof (std::int64_t));
555 stream << boost::numeric_cast<std::int64_t>(value);
556 break;
557 }
559 {
560 std::uint64_t value;
561 memcpy (&value, reinterpret_cast<const char*> (&point) + fields[d].offset + c * sizeof (std::uint64_t), sizeof (std::uint64_t));
562 stream << boost::numeric_cast<std::uint64_t>(value);
563 break;
564 }
566 {
567 /*
568 * Despite the float type, store the rgb field as uint32
569 * because several fully opaque color values are mapped to
570 * nan.
571 */
572 if ("rgb" == fields[d].name)
573 {
574 std::uint32_t value;
575 memcpy (&value, reinterpret_cast<const char*> (&point) + fields[d].offset + c * sizeof (float), sizeof (float));
576 stream << boost::numeric_cast<std::uint32_t>(value);
577 break;
578 }
579 float value;
580 memcpy (&value, reinterpret_cast<const char*> (&point) + fields[d].offset + c * sizeof (float), sizeof (float));
581 if (std::isnan (value))
582 stream << "nan";
583 else
584 stream << boost::numeric_cast<float>(value);
585 break;
586 }
588 {
589 double value;
590 memcpy (&value, reinterpret_cast<const char*> (&point) + fields[d].offset + c * sizeof (double), sizeof (double));
591 if (std::isnan (value))
592 stream << "nan";
593 else
594 stream << boost::numeric_cast<double>(value);
595 break;
596 }
597 default:
598 PCL_WARN ("[pcl::PCDWriter::writeASCII] Incorrect field data type specified (%d)!\n", fields[d].datatype);
599 break;
600 }
601
602 if (d < fields.size () - 1 || c < static_cast<int> (fields[d].count - 1))
603 stream << " ";
604 }
605 }
606 // Copy the stream, trim it, and write it to disk
607 std::string result = stream.str ();
608 boost::trim (result);
609 stream.str ("");
610 fs << result << "\n";
611 }
612 fs.close (); // Close file
613 resetLockingPermissions (file_name, file_lock);
614 return (0);
615}
616
617
618/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
619template <typename PointT> int
620pcl::PCDWriter::writeBinary (const std::string &file_name,
621 const pcl::PointCloud<PointT> &cloud,
622 const pcl::Indices &indices)
623{
624 if (cloud.empty () || indices.empty ())
625 {
626 PCL_WARN ("[pcl::PCDWriter::writeBinary] Input point cloud has no data or empty indices given!\n");
627 }
628 int data_idx = 0;
629 std::ostringstream oss;
630 oss << generateHeader<PointT> (cloud, static_cast<int> (indices.size ())) << "DATA binary\n";
631 oss.flush ();
632 data_idx = static_cast<int> (oss.tellp ());
633
634#ifdef _WIN32
635 HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
636 if (h_native_file == INVALID_HANDLE_VALUE)
637 {
638 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during CreateFile!");
639 return (-1);
640 }
641#else
642 int fd = io::raw_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
643 if (fd < 0)
644 {
645 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during open!");
646 return (-1);
647 }
648#endif
649 // Mandatory lock file
650 boost::interprocess::file_lock file_lock;
651 setLockingPermissions (file_name, file_lock);
652
653 auto fields = pcl::getFields<PointT> ();
654 std::vector<int> fields_sizes;
655 std::size_t fsize = 0;
656 std::size_t data_size = 0;
657 std::size_t nri = 0;
658 // Compute the total size of the fields
659 for (const auto &field : fields)
660 {
661 if (field.name == "_")
662 continue;
663
664 int fs = field.count * getFieldSize (field.datatype);
665 fsize += fs;
666 fields_sizes.push_back (fs);
667 fields[nri++] = field;
668 }
669 fields.resize (nri);
670
671 data_size = indices.size () * fsize;
672
673 // Prepare the map
674#ifdef _WIN32
675 HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, data_idx + data_size, NULL);
676 char *map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, data_idx + data_size));
677 CloseHandle (fm);
678
679#else
680 // Allocate disk space for the entire file to prevent bus errors.
681 const int allocate_res = io::raw_fallocate (fd, data_idx + data_size);
682 if (allocate_res != 0)
683 {
684 io::raw_close (fd);
685 resetLockingPermissions (file_name, file_lock);
686 PCL_ERROR ("[pcl::PCDWriter::writeBinary] raw_fallocate(length=%zu) returned %i. errno: %d strerror: %s\n",
687 data_idx + data_size, allocate_res, errno, strerror (errno));
688
689 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during raw_fallocate ()!");
690 return (-1);
691 }
692
693 char *map = static_cast<char*> (::mmap (nullptr, data_idx + data_size, PROT_WRITE, MAP_SHARED, fd, 0));
694 if (map == reinterpret_cast<char*> (-1)) //MAP_FAILED)
695 {
696 io::raw_close (fd);
697 resetLockingPermissions (file_name, file_lock);
698 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during mmap ()!");
699 return (-1);
700 }
701#endif
702
703 // Copy the header
704 memcpy (&map[0], oss.str ().c_str (), data_idx);
705
706 char *out = &map[0] + data_idx;
707 // Copy the data
708 for (const auto &index : indices)
709 {
710 int nrj = 0;
711 for (const auto &field : fields)
712 {
713 memcpy (out, reinterpret_cast<const char*> (&cloud[index]) + field.offset, fields_sizes[nrj]);
714 out += fields_sizes[nrj++];
715 }
716 }
717
718#ifndef _WIN32
719 // If the user set the synchronization flag on, call msync
720 if (map_synchronization_)
721 msync (map, data_idx + data_size, MS_SYNC);
722#endif
723
724 // Unmap the pages of memory
725#ifdef _WIN32
726 UnmapViewOfFile (map);
727#else
728 if (::munmap (map, (data_idx + data_size)) == -1)
729 {
730 io::raw_close (fd);
731 resetLockingPermissions (file_name, file_lock);
732 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during munmap ()!");
733 return (-1);
734 }
735#endif
736 // Close file
737#ifdef _WIN32
738 CloseHandle(h_native_file);
739#else
740 io::raw_close (fd);
741#endif
742
743 resetLockingPermissions (file_name, file_lock);
744 return (0);
745}
746
747//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
748template <typename PointT> int
749pcl::PCDWriter::writeASCII (const std::string &file_name,
750 const pcl::PointCloud<PointT> &cloud,
751 const pcl::Indices &indices,
752 const int precision)
753{
754 if (cloud.empty () || indices.empty ())
755 {
756 PCL_WARN ("[pcl::PCDWriter::writeASCII] Input point cloud has no data or empty indices given!\n");
757 }
758
759 if (cloud.width * cloud.height != cloud.size ())
760 {
761 throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Number of points different than width * height!");
762 return (-1);
763 }
764
765 std::ofstream fs;
766 fs.open (file_name.c_str (), std::ios::binary); // Open file
767 if (!fs.is_open () || fs.fail ())
768 {
769 throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Could not open file for writing!");
770 return (-1);
771 }
772
773 // Mandatory lock file
774 boost::interprocess::file_lock file_lock;
775 setLockingPermissions (file_name, file_lock);
776
777 fs.precision (precision);
778 fs.imbue (std::locale::classic ());
779
780 const auto fields = pcl::getFields<PointT> ();
781
782 // Write the header information
783 fs << generateHeader<PointT> (cloud, static_cast<int> (indices.size ())) << "DATA ascii\n";
784
785 std::ostringstream stream;
786 stream.precision (precision);
787 stream.imbue (std::locale::classic ());
788
789 // Iterate through the points
790 for (const auto &index : indices)
791 {
792 for (std::size_t d = 0; d < fields.size (); ++d)
793 {
794 // Ignore invalid padded dimensions that are inherited from binary data
795 if (fields[d].name == "_")
796 continue;
797
798 int count = fields[d].count;
799 if (count == 0)
800 count = 1; // we simply cannot tolerate 0 counts (coming from older converter code)
801
802 for (int c = 0; c < count; ++c)
803 {
804 switch (fields[d].datatype)
805 {
807 {
808 bool value;
809 memcpy (&value, reinterpret_cast<const char*> (&cloud.points[index]) + fields[d].offset + c * sizeof (bool), sizeof (bool));
810 stream << boost::numeric_cast<std::int32_t>(value);
811 break;
812 }
814 {
815 std::int8_t value;
816 memcpy (&value, reinterpret_cast<const char*> (&cloud[index]) + fields[d].offset + c * sizeof (std::int8_t), sizeof (std::int8_t));
817 stream << boost::numeric_cast<std::int32_t>(value);
818 break;
819 }
821 {
822 std::uint8_t value;
823 memcpy (&value, reinterpret_cast<const char*> (&cloud[index]) + fields[d].offset + c * sizeof (std::uint8_t), sizeof (std::uint8_t));
824 stream << boost::numeric_cast<std::uint32_t>(value);
825 break;
826 }
828 {
829 std::int16_t value;
830 memcpy (&value, reinterpret_cast<const char*> (&cloud[index]) + fields[d].offset + c * sizeof (std::int16_t), sizeof (std::int16_t));
831 stream << boost::numeric_cast<std::int16_t>(value);
832 break;
833 }
835 {
836 std::uint16_t value;
837 memcpy (&value, reinterpret_cast<const char*> (&cloud[index]) + fields[d].offset + c * sizeof (std::uint16_t), sizeof (std::uint16_t));
838 stream << boost::numeric_cast<std::uint16_t>(value);
839 break;
840 }
842 {
843 std::int32_t value;
844 memcpy (&value, reinterpret_cast<const char*> (&cloud[index]) + fields[d].offset + c * sizeof (std::int32_t), sizeof (std::int32_t));
845 stream << boost::numeric_cast<std::int32_t>(value);
846 break;
847 }
849 {
850 std::uint32_t value;
851 memcpy (&value, reinterpret_cast<const char*> (&cloud[index]) + fields[d].offset + c * sizeof (std::uint32_t), sizeof (std::uint32_t));
852 stream << boost::numeric_cast<std::uint32_t>(value);
853 break;
854 }
856 {
857 std::int64_t value;
858 memcpy (&value, reinterpret_cast<const char*> (&cloud.points[index]) + fields[d].offset + c * sizeof (std::int64_t), sizeof (std::int64_t));
859 stream << boost::numeric_cast<std::int64_t>(value);
860 break;
861 }
863 {
864 std::uint64_t value;
865 memcpy (&value, reinterpret_cast<const char*> (&cloud.points[index]) + fields[d].offset + c * sizeof (std::uint64_t), sizeof (std::uint64_t));
866 stream << boost::numeric_cast<std::uint64_t>(value);
867 break;
868 }
870 {
871 /*
872 * Despite the float type, store the rgb field as uint32
873 * because several fully opaque color values are mapped to
874 * nan.
875 */
876 if ("rgb" == fields[d].name)
877 {
878 std::uint32_t value;
879 memcpy (&value, reinterpret_cast<const char*> (&cloud[index]) + fields[d].offset + c * sizeof (float), sizeof (float));
880 stream << boost::numeric_cast<std::uint32_t>(value);
881 }
882 else
883 {
884 float value;
885 memcpy (&value, reinterpret_cast<const char*> (&cloud[index]) + fields[d].offset + c * sizeof (float), sizeof (float));
886 if (std::isnan (value))
887 stream << "nan";
888 else
889 stream << boost::numeric_cast<float>(value);
890 }
891 break;
892 }
894 {
895 double value;
896 memcpy (&value, reinterpret_cast<const char*> (&cloud[index]) + fields[d].offset + c * sizeof (double), sizeof (double));
897 if (std::isnan (value))
898 stream << "nan";
899 else
900 stream << boost::numeric_cast<double>(value);
901 break;
902 }
903 default:
904 PCL_WARN ("[pcl::PCDWriter::writeASCII] Incorrect field data type specified (%d)!\n", fields[d].datatype);
905 break;
906 }
907
908 if (d < fields.size () - 1 || c < static_cast<int> (fields[d].count - 1))
909 stream << " ";
910 }
911 }
912 // Copy the stream, trim it, and write it to disk
913 std::string result = stream.str ();
914 boost::trim (result);
915 stream.str ("");
916 fs << result << "\n";
917 }
918 fs.close (); // Close file
919
920 resetLockingPermissions (file_name, file_lock);
921
922 return (0);
923}
924
925#endif //#ifndef PCL_IO_PCD_IO_H_
An exception that is thrown during an IO error (typical read/write errors)
Definition: exceptions.h:179
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())
Save point cloud data to a PCD file containing n-D points, in BINARY format.
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(), const int precision=8)
Save point cloud data to a PCD file containing n-D points, in ASCII format.
static std::string generateHeader(const pcl::PointCloud< PointT > &cloud, const int nr_points=std::numeric_limits< int >::max())
Generate the header of a PCD file format.
Definition: pcd_io.hpp:57
int writeBinaryCompressed(const std::string &file_name, const pcl::PCLPointCloud2 &cloud, const Eigen::Vector4f &origin=Eigen::Vector4f::Zero(), const Eigen::Quaternionf &orientation=Eigen::Quaternionf::Identity())
Save point cloud data to a PCD file containing n-D points, in BINARY_COMPRESSED format.
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
bool empty() const
Definition: point_cloud.h:446
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:408
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
std::size_t size() const
Definition: point_cloud.h:443
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:406
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:395
int getFieldSize(const int datatype)
Obtains the size of a specific field data type in bytes.
Definition: io.h:119
int getFieldType(const int size, char type)
Obtains the type of the PCLPointField from a specific size and type.
Definition: io.h:163
int raw_close(int fd)
Definition: low_level_io.h:85
int raw_fallocate(int fd, long length)
Definition: low_level_io.h:110
int raw_open(const char *pathname, int flags, int mode)
Definition: low_level_io.h:75
PCL_EXPORTS unsigned int lzfCompress(const void *const in_data, unsigned int in_len, void *out_data, unsigned int out_len)
Compress in_len bytes stored at the memory block starting at in_data and write the result to out_data...
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133