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