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 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
56 template <typename PointT> std::string
generateHeader(const pcl::PointCloud<PointT> & cloud,const int nr_points)57 pcl::PCDWriter::generateHeader (const pcl::PointCloud<PointT> &cloud, const int nr_points)
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 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
110 template <typename PointT> int
writeBinary(const std::string & file_name,const pcl::PointCloud<PointT> & cloud)111 pcl::PCDWriter::writeBinary (const std::string &file_name,
112 const pcl::PointCloud<PointT> &cloud)
113 {
114 if (cloud.empty ())
115 {
116 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Input point cloud has no data!");
117 return (-1);
118 }
119 int data_idx = 0;
120 std::ostringstream oss;
121 oss << generateHeader<PointT> (cloud) << "DATA binary\n";
122 oss.flush ();
123 data_idx = static_cast<int> (oss.tellp ());
124
125 #ifdef _WIN32
126 HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
127 if (h_native_file == INVALID_HANDLE_VALUE)
128 {
129 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during CreateFile!");
130 return (-1);
131 }
132 #else
133 int fd = io::raw_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
134 if (fd < 0)
135 {
136 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during open!");
137 return (-1);
138 }
139 #endif
140 // Mandatory lock file
141 boost::interprocess::file_lock file_lock;
142 setLockingPermissions (file_name, file_lock);
143
144 auto fields = pcl::getFields<PointT> ();
145 std::vector<int> fields_sizes;
146 std::size_t fsize = 0;
147 std::size_t data_size = 0;
148 std::size_t nri = 0;
149 // Compute the total size of the fields
150 for (const auto &field : fields)
151 {
152 if (field.name == "_")
153 continue;
154
155 int fs = field.count * getFieldSize (field.datatype);
156 fsize += fs;
157 fields_sizes.push_back (fs);
158 fields[nri++] = field;
159 }
160 fields.resize (nri);
161
162 data_size = cloud.size () * fsize;
163
164 // Prepare the map
165 #ifdef _WIN32
166 HANDLE fm = CreateFileMappingA (h_native_file, NULL, PAGE_READWRITE, 0, (DWORD) (data_idx + data_size), NULL);
167 if (fm == NULL)
168 {
169 throw pcl::IOException("[pcl::PCDWriter::writeBinary] Error during memory map creation ()!");
170 return (-1);
171 }
172 char *map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, data_idx + data_size));
173 CloseHandle (fm);
174
175 #else
176 // Allocate disk space for the entire file to prevent bus errors.
177 if (io::raw_fallocate (fd, data_idx + data_size) != 0)
178 {
179 io::raw_close (fd);
180 resetLockingPermissions (file_name, file_lock);
181 PCL_ERROR ("[pcl::PCDWriter::writeBinary] posix_fallocate errno: %d strerror: %s\n", errno, strerror (errno));
182
183 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during posix_fallocate ()!");
184 return (-1);
185 }
186
187 char *map = static_cast<char*> (::mmap (nullptr, data_idx + data_size, PROT_WRITE, MAP_SHARED, fd, 0));
188 if (map == reinterpret_cast<char*> (-1)) //MAP_FAILED)
189 {
190 io::raw_close (fd);
191 resetLockingPermissions (file_name, file_lock);
192 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during mmap ()!");
193 return (-1);
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 return (-1);
228 }
229 #endif
230 // Close file
231 #ifdef _WIN32
232 CloseHandle (h_native_file);
233 #else
234 io::raw_close (fd);
235 #endif
236 resetLockingPermissions (file_name, file_lock);
237 return (0);
238 }
239
240 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
241 template <typename PointT> int
writeBinaryCompressed(const std::string & file_name,const pcl::PointCloud<PointT> & cloud)242 pcl::PCDWriter::writeBinaryCompressed (const std::string &file_name,
243 const pcl::PointCloud<PointT> &cloud)
244 {
245 if (cloud.empty ())
246 {
247 throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Input point cloud has no data!");
248 return (-1);
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 // Compress the valid data
343 unsigned int compressed_size = pcl::lzfCompress (only_valid_data,
344 static_cast<std::uint32_t> (data_size),
345 &temp_buf[8],
346 static_cast<std::uint32_t> (static_cast<float>(data_size) * 1.5f));
347 unsigned int compressed_final_size = 0;
348 // Was the compression successful?
349 if (compressed_size)
350 {
351 char *header = &temp_buf[0];
352 memcpy (&header[0], &compressed_size, sizeof (unsigned int));
353 memcpy (&header[4], &data_size, sizeof (unsigned int));
354 data_size = compressed_size + 8;
355 compressed_final_size = static_cast<std::uint32_t> (data_size) + data_idx;
356 }
357 else
358 {
359 #ifndef _WIN32
360 io::raw_close (fd);
361 #endif
362 resetLockingPermissions (file_name, file_lock);
363 throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during compression!");
364 return (-1);
365 }
366
367 // Prepare the map
368 #ifdef _WIN32
369 HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, compressed_final_size, NULL);
370 char *map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, compressed_final_size));
371 CloseHandle (fm);
372
373 #else
374 // Allocate disk space for the entire file to prevent bus errors.
375 if (io::raw_fallocate (fd, compressed_final_size) != 0)
376 {
377 io::raw_close (fd);
378 resetLockingPermissions (file_name, file_lock);
379 PCL_ERROR ("[pcl::PCDWriter::writeBinaryCompressed] posix_fallocate errno: %d strerror: %s\n", errno, strerror (errno));
380
381 throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during posix_fallocate ()!");
382 return (-1);
383 }
384
385 char *map = static_cast<char*> (::mmap (nullptr, compressed_final_size, PROT_WRITE, MAP_SHARED, fd, 0));
386 if (map == reinterpret_cast<char*> (-1)) //MAP_FAILED)
387 {
388 io::raw_close (fd);
389 resetLockingPermissions (file_name, file_lock);
390 throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during mmap ()!");
391 return (-1);
392 }
393 #endif
394
395 // Copy the header
396 memcpy (&map[0], oss.str ().c_str (), data_idx);
397 // Copy the compressed data
398 memcpy (&map[data_idx], temp_buf, data_size);
399
400 #ifndef _WIN32
401 // If the user set the synchronization flag on, call msync
402 if (map_synchronization_)
403 msync (map, compressed_final_size, MS_SYNC);
404 #endif
405
406 // Unmap the pages of memory
407 #ifdef _WIN32
408 UnmapViewOfFile (map);
409 #else
410 if (::munmap (map, (compressed_final_size)) == -1)
411 {
412 io::raw_close (fd);
413 resetLockingPermissions (file_name, file_lock);
414 throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during munmap ()!");
415 return (-1);
416 }
417 #endif
418
419 // Close file
420 #ifdef _WIN32
421 CloseHandle (h_native_file);
422 #else
423 io::raw_close (fd);
424 #endif
425 resetLockingPermissions (file_name, file_lock);
426
427 free (only_valid_data);
428 free (temp_buf);
429 return (0);
430 }
431
432 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
433 template <typename PointT> int
writeASCII(const std::string & file_name,const pcl::PointCloud<PointT> & cloud,const int precision)434 pcl::PCDWriter::writeASCII (const std::string &file_name, const pcl::PointCloud<PointT> &cloud,
435 const int precision)
436 {
437 if (cloud.empty ())
438 {
439 throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Input point cloud has no data!");
440 return (-1);
441 }
442
443 if (cloud.width * cloud.height != cloud.size ())
444 {
445 throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Number of points different than width * height!");
446 return (-1);
447 }
448
449 std::ofstream fs;
450 fs.open (file_name.c_str (), std::ios::binary); // Open file
451
452 if (!fs.is_open () || fs.fail ())
453 {
454 throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Could not open file for writing!");
455 return (-1);
456 }
457
458 // Mandatory lock file
459 boost::interprocess::file_lock file_lock;
460 setLockingPermissions (file_name, file_lock);
461
462 fs.precision (precision);
463 fs.imbue (std::locale::classic ());
464
465 const auto fields = pcl::getFields<PointT> ();
466
467 // Write the header information
468 fs << generateHeader<PointT> (cloud) << "DATA ascii\n";
469
470 std::ostringstream stream;
471 stream.precision (precision);
472 stream.imbue (std::locale::classic ());
473 // Iterate through the points
474 for (const auto& point: cloud)
475 {
476 for (std::size_t d = 0; d < fields.size (); ++d)
477 {
478 // Ignore invalid padded dimensions that are inherited from binary data
479 if (fields[d].name == "_")
480 continue;
481
482 int count = fields[d].count;
483 if (count == 0)
484 count = 1; // we simply cannot tolerate 0 counts (coming from older converter code)
485
486 for (int c = 0; c < count; ++c)
487 {
488 switch (fields[d].datatype)
489 {
490 case pcl::PCLPointField::INT8:
491 {
492 std::int8_t value;
493 memcpy (&value, reinterpret_cast<const char*> (&point) + fields[d].offset + c * sizeof (std::int8_t), sizeof (std::int8_t));
494 stream << boost::numeric_cast<std::int32_t>(value);
495 break;
496 }
497 case pcl::PCLPointField::UINT8:
498 {
499 std::uint8_t value;
500 memcpy (&value, reinterpret_cast<const char*> (&point) + fields[d].offset + c * sizeof (std::uint8_t), sizeof (std::uint8_t));
501 stream << boost::numeric_cast<std::uint32_t>(value);
502 break;
503 }
504 case pcl::PCLPointField::INT16:
505 {
506 std::int16_t value;
507 memcpy (&value, reinterpret_cast<const char*> (&point) + fields[d].offset + c * sizeof (std::int16_t), sizeof (std::int16_t));
508 stream << boost::numeric_cast<std::int16_t>(value);
509 break;
510 }
511 case pcl::PCLPointField::UINT16:
512 {
513 std::uint16_t value;
514 memcpy (&value, reinterpret_cast<const char*> (&point) + fields[d].offset + c * sizeof (std::uint16_t), sizeof (std::uint16_t));
515 stream << boost::numeric_cast<std::uint16_t>(value);
516 break;
517 }
518 case pcl::PCLPointField::INT32:
519 {
520 std::int32_t value;
521 memcpy (&value, reinterpret_cast<const char*> (&point) + fields[d].offset + c * sizeof (std::int32_t), sizeof (std::int32_t));
522 stream << boost::numeric_cast<std::int32_t>(value);
523 break;
524 }
525 case pcl::PCLPointField::UINT32:
526 {
527 std::uint32_t value;
528 memcpy (&value, reinterpret_cast<const char*> (&point) + fields[d].offset + c * sizeof (std::uint32_t), sizeof (std::uint32_t));
529 stream << boost::numeric_cast<std::uint32_t>(value);
530 break;
531 }
532 case pcl::PCLPointField::FLOAT32:
533 {
534 /*
535 * Despite the float type, store the rgb field as uint32
536 * because several fully opaque color values are mapped to
537 * nan.
538 */
539 if ("rgb" == fields[d].name)
540 {
541 std::uint32_t value;
542 memcpy (&value, reinterpret_cast<const char*> (&point) + fields[d].offset + c * sizeof (float), sizeof (float));
543 stream << boost::numeric_cast<std::uint32_t>(value);
544 break;
545 }
546 float value;
547 memcpy (&value, reinterpret_cast<const char*> (&point) + fields[d].offset + c * sizeof (float), sizeof (float));
548 if (std::isnan (value))
549 stream << "nan";
550 else
551 stream << boost::numeric_cast<float>(value);
552 break;
553 }
554 case pcl::PCLPointField::FLOAT64:
555 {
556 double value;
557 memcpy (&value, reinterpret_cast<const char*> (&point) + fields[d].offset + c * sizeof (double), sizeof (double));
558 if (std::isnan (value))
559 stream << "nan";
560 else
561 stream << boost::numeric_cast<double>(value);
562 break;
563 }
564 default:
565 PCL_WARN ("[pcl::PCDWriter::writeASCII] Incorrect field data type specified (%d)!\n", fields[d].datatype);
566 break;
567 }
568
569 if (d < fields.size () - 1 || c < static_cast<int> (fields[d].count - 1))
570 stream << " ";
571 }
572 }
573 // Copy the stream, trim it, and write it to disk
574 std::string result = stream.str ();
575 boost::trim (result);
576 stream.str ("");
577 fs << result << "\n";
578 }
579 fs.close (); // Close file
580 resetLockingPermissions (file_name, file_lock);
581 return (0);
582 }
583
584
585 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
586 template <typename PointT> int
writeBinary(const std::string & file_name,const pcl::PointCloud<PointT> & cloud,const pcl::Indices & indices)587 pcl::PCDWriter::writeBinary (const std::string &file_name,
588 const pcl::PointCloud<PointT> &cloud,
589 const pcl::Indices &indices)
590 {
591 if (cloud.empty () || indices.empty ())
592 {
593 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Input point cloud has no data or empty indices given!");
594 return (-1);
595 }
596 int data_idx = 0;
597 std::ostringstream oss;
598 oss << generateHeader<PointT> (cloud, static_cast<int> (indices.size ())) << "DATA binary\n";
599 oss.flush ();
600 data_idx = static_cast<int> (oss.tellp ());
601
602 #ifdef _WIN32
603 HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
604 if (h_native_file == INVALID_HANDLE_VALUE)
605 {
606 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during CreateFile!");
607 return (-1);
608 }
609 #else
610 int fd = io::raw_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
611 if (fd < 0)
612 {
613 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during open!");
614 return (-1);
615 }
616 #endif
617 // Mandatory lock file
618 boost::interprocess::file_lock file_lock;
619 setLockingPermissions (file_name, file_lock);
620
621 auto fields = pcl::getFields<PointT> ();
622 std::vector<int> fields_sizes;
623 std::size_t fsize = 0;
624 std::size_t data_size = 0;
625 std::size_t nri = 0;
626 // Compute the total size of the fields
627 for (const auto &field : fields)
628 {
629 if (field.name == "_")
630 continue;
631
632 int fs = field.count * getFieldSize (field.datatype);
633 fsize += fs;
634 fields_sizes.push_back (fs);
635 fields[nri++] = field;
636 }
637 fields.resize (nri);
638
639 data_size = indices.size () * fsize;
640
641 // Prepare the map
642 #ifdef _WIN32
643 HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, data_idx + data_size, NULL);
644 char *map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, data_idx + data_size));
645 CloseHandle (fm);
646
647 #else
648 // Allocate disk space for the entire file to prevent bus errors.
649 if (io::raw_fallocate (fd, data_idx + data_size) != 0)
650 {
651 io::raw_close (fd);
652 resetLockingPermissions (file_name, file_lock);
653 PCL_ERROR ("[pcl::PCDWriter::writeBinary] posix_fallocate errno: %d strerror: %s\n", errno, strerror (errno));
654
655 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during posix_fallocate ()!");
656 return (-1);
657 }
658
659 char *map = static_cast<char*> (::mmap (nullptr, data_idx + data_size, PROT_WRITE, MAP_SHARED, fd, 0));
660 if (map == reinterpret_cast<char*> (-1)) //MAP_FAILED)
661 {
662 io::raw_close (fd);
663 resetLockingPermissions (file_name, file_lock);
664 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during mmap ()!");
665 return (-1);
666 }
667 #endif
668
669 // Copy the header
670 memcpy (&map[0], oss.str ().c_str (), data_idx);
671
672 char *out = &map[0] + data_idx;
673 // Copy the data
674 for (const auto &index : indices)
675 {
676 int nrj = 0;
677 for (const auto &field : fields)
678 {
679 memcpy (out, reinterpret_cast<const char*> (&cloud[index]) + field.offset, fields_sizes[nrj]);
680 out += fields_sizes[nrj++];
681 }
682 }
683
684 #ifndef _WIN32
685 // If the user set the synchronization flag on, call msync
686 if (map_synchronization_)
687 msync (map, data_idx + data_size, MS_SYNC);
688 #endif
689
690 // Unmap the pages of memory
691 #ifdef _WIN32
692 UnmapViewOfFile (map);
693 #else
694 if (::munmap (map, (data_idx + data_size)) == -1)
695 {
696 io::raw_close (fd);
697 resetLockingPermissions (file_name, file_lock);
698 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during munmap ()!");
699 return (-1);
700 }
701 #endif
702 // Close file
703 #ifdef _WIN32
704 CloseHandle(h_native_file);
705 #else
706 io::raw_close (fd);
707 #endif
708
709 resetLockingPermissions (file_name, file_lock);
710 return (0);
711 }
712
713 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
714 template <typename PointT> int
writeASCII(const std::string & file_name,const pcl::PointCloud<PointT> & cloud,const pcl::Indices & indices,const int precision)715 pcl::PCDWriter::writeASCII (const std::string &file_name,
716 const pcl::PointCloud<PointT> &cloud,
717 const pcl::Indices &indices,
718 const int precision)
719 {
720 if (cloud.empty () || indices.empty ())
721 {
722 throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Input point cloud has no data or empty indices given!");
723 return (-1);
724 }
725
726 if (cloud.width * cloud.height != cloud.size ())
727 {
728 throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Number of points different than width * height!");
729 return (-1);
730 }
731
732 std::ofstream fs;
733 fs.open (file_name.c_str (), std::ios::binary); // Open file
734 if (!fs.is_open () || fs.fail ())
735 {
736 throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Could not open file for writing!");
737 return (-1);
738 }
739
740 // Mandatory lock file
741 boost::interprocess::file_lock file_lock;
742 setLockingPermissions (file_name, file_lock);
743
744 fs.precision (precision);
745 fs.imbue (std::locale::classic ());
746
747 const auto fields = pcl::getFields<PointT> ();
748
749 // Write the header information
750 fs << generateHeader<PointT> (cloud, static_cast<int> (indices.size ())) << "DATA ascii\n";
751
752 std::ostringstream stream;
753 stream.precision (precision);
754 stream.imbue (std::locale::classic ());
755
756 // Iterate through the points
757 for (const auto &index : indices)
758 {
759 for (std::size_t d = 0; d < fields.size (); ++d)
760 {
761 // Ignore invalid padded dimensions that are inherited from binary data
762 if (fields[d].name == "_")
763 continue;
764
765 int count = fields[d].count;
766 if (count == 0)
767 count = 1; // we simply cannot tolerate 0 counts (coming from older converter code)
768
769 for (int c = 0; c < count; ++c)
770 {
771 switch (fields[d].datatype)
772 {
773 case pcl::PCLPointField::INT8:
774 {
775 std::int8_t value;
776 memcpy (&value, reinterpret_cast<const char*> (&cloud[index]) + fields[d].offset + c * sizeof (std::int8_t), sizeof (std::int8_t));
777 stream << boost::numeric_cast<std::int32_t>(value);
778 break;
779 }
780 case pcl::PCLPointField::UINT8:
781 {
782 std::uint8_t value;
783 memcpy (&value, reinterpret_cast<const char*> (&cloud[index]) + fields[d].offset + c * sizeof (std::uint8_t), sizeof (std::uint8_t));
784 stream << boost::numeric_cast<std::uint32_t>(value);
785 break;
786 }
787 case pcl::PCLPointField::INT16:
788 {
789 std::int16_t value;
790 memcpy (&value, reinterpret_cast<const char*> (&cloud[index]) + fields[d].offset + c * sizeof (std::int16_t), sizeof (std::int16_t));
791 stream << boost::numeric_cast<std::int16_t>(value);
792 break;
793 }
794 case pcl::PCLPointField::UINT16:
795 {
796 std::uint16_t value;
797 memcpy (&value, reinterpret_cast<const char*> (&cloud[index]) + fields[d].offset + c * sizeof (std::uint16_t), sizeof (std::uint16_t));
798 stream << boost::numeric_cast<std::uint16_t>(value);
799 break;
800 }
801 case pcl::PCLPointField::INT32:
802 {
803 std::int32_t value;
804 memcpy (&value, reinterpret_cast<const char*> (&cloud[index]) + fields[d].offset + c * sizeof (std::int32_t), sizeof (std::int32_t));
805 stream << boost::numeric_cast<std::int32_t>(value);
806 break;
807 }
808 case pcl::PCLPointField::UINT32:
809 {
810 std::uint32_t value;
811 memcpy (&value, reinterpret_cast<const char*> (&cloud[index]) + fields[d].offset + c * sizeof (std::uint32_t), sizeof (std::uint32_t));
812 stream << boost::numeric_cast<std::uint32_t>(value);
813 break;
814 }
815 case pcl::PCLPointField::FLOAT32:
816 {
817 /*
818 * Despite the float type, store the rgb field as uint32
819 * because several fully opaque color values are mapped to
820 * nan.
821 */
822 if ("rgb" == fields[d].name)
823 {
824 std::uint32_t value;
825 memcpy (&value, reinterpret_cast<const char*> (&cloud[index]) + fields[d].offset + c * sizeof (float), sizeof (float));
826 stream << boost::numeric_cast<std::uint32_t>(value);
827 }
828 else
829 {
830 float value;
831 memcpy (&value, reinterpret_cast<const char*> (&cloud[index]) + fields[d].offset + c * sizeof (float), sizeof (float));
832 if (std::isnan (value))
833 stream << "nan";
834 else
835 stream << boost::numeric_cast<float>(value);
836 }
837 break;
838 }
839 case pcl::PCLPointField::FLOAT64:
840 {
841 double value;
842 memcpy (&value, reinterpret_cast<const char*> (&cloud[index]) + fields[d].offset + c * sizeof (double), sizeof (double));
843 if (std::isnan (value))
844 stream << "nan";
845 else
846 stream << boost::numeric_cast<double>(value);
847 break;
848 }
849 default:
850 PCL_WARN ("[pcl::PCDWriter::writeASCII] Incorrect field data type specified (%d)!\n", fields[d].datatype);
851 break;
852 }
853
854 if (d < fields.size () - 1 || c < static_cast<int> (fields[d].count - 1))
855 stream << " ";
856 }
857 }
858 // Copy the stream, trim it, and write it to disk
859 std::string result = stream.str ();
860 boost::trim (result);
861 stream.str ("");
862 fs << result << "\n";
863 }
864 fs.close (); // Close file
865
866 resetLockingPermissions (file_name, file_lock);
867
868 return (0);
869 }
870
871 #endif //#ifndef PCL_IO_PCD_IO_H_
872