Point Cloud Library (PCL)  1.11.1-dev
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 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
56 template <typename PointT> std::string
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
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
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
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  {
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  }
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  }
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  }
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  }
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  }
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  }
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  }
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
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
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  {
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  }
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  }
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  }
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  }
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  }
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  }
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  }
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_
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:394
pcl::PCDWriter::generateHeader
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
pcl::io::raw_open
int raw_open(const char *pathname, int flags, int mode)
Definition: low_level_io.h:75
pcl::PCDWriter::writeASCII
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.
pcl::PointCloud::empty
bool empty() const
Definition: point_cloud.h:440
pcl::PCLPointField::INT8
@ INT8
Definition: PCLPointField.h:20
pcl::PCDWriter::writeBinaryCompressed
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.
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: distances.h:55
pcl::PCLPointField::INT32
@ INT32
Definition: PCLPointField.h:24
pcl::PCLPointField::FLOAT32
@ FLOAT32
Definition: PCLPointField.h:26
pcl::PCLPointField::UINT32
@ UINT32
Definition: PCLPointField.h:25
pcl::PCLPointField::UINT8
@ UINT8
Definition: PCLPointField.h:21
pcl::io::raw_close
int raw_close(int fd)
Definition: low_level_io.h:85
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:392
pcl::PCLPointField::INT16
@ INT16
Definition: PCLPointField.h:22
pcl::PCLPointField::UINT16
@ UINT16
Definition: PCLPointField.h:23
pcl::PCDWriter::writeBinary
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.
pcl::getFieldSize
int getFieldSize(const int datatype)
Obtains the size of a specific field data type in bytes.
Definition: io.h:119
pcl::PCLPointField::FLOAT64
@ FLOAT64
Definition: PCLPointField.h:27
pcl::PointCloud::sensor_origin_
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:400
pcl::PointCloud::sensor_orientation_
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:402
pcl::IOException
An exception that is thrown during an IO error (typical read/write errors)
Definition: exceptions.h:178
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:437
pcl::lzfCompress
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...
pcl::io::raw_fallocate
int raw_fallocate(int fd, long length)
Definition: low_level_io.h:110
pcl::getFieldType
int getFieldType(const int size, char type)
Obtains the type of the PCLPointField from a specific size and type.
Definition: io.h:158