Point Cloud Library (PCL)  1.14.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  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 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
240 template <typename PointT> int
241 pcl::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 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
438 template <typename PointT> int
439 pcl::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  {
492  case pcl::PCLPointField::BOOL:
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  }
499  case pcl::PCLPointField::INT8:
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  }
506  case pcl::PCLPointField::UINT8:
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  }
513  case pcl::PCLPointField::INT16:
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  }
520  case pcl::PCLPointField::UINT16:
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  }
527  case pcl::PCLPointField::INT32:
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  }
534  case pcl::PCLPointField::UINT32:
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  }
541  case pcl::PCLPointField::INT64:
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  }
548  case pcl::PCLPointField::UINT64:
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  }
555  case pcl::PCLPointField::FLOAT32:
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  }
577  case pcl::PCLPointField::FLOAT64:
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 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
609 template <typename PointT> int
610 pcl::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 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
732 template <typename PointT> int
733 pcl::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  {
788  case pcl::PCLPointField::BOOL:
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  }
795  case pcl::PCLPointField::INT8:
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  }
802  case pcl::PCLPointField::UINT8:
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  }
809  case pcl::PCLPointField::INT16:
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  }
816  case pcl::PCLPointField::UINT16:
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  }
823  case pcl::PCLPointField::INT32:
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  }
830  case pcl::PCLPointField::UINT32:
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  }
837  case pcl::PCLPointField::INT64:
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  }
844  case pcl::PCLPointField::UINT64:
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  }
851  case pcl::PCLPointField::FLOAT32:
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  }
875  case pcl::PCLPointField::FLOAT64:
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_
pcl::IOException
An exception that is thrown during an IO error (typical read/write errors)
Definition: exceptions.h:182
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::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::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::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: point_cloud.h:173
pcl::PointCloud::empty
bool empty() const
Definition: point_cloud.h:446
pcl::PointCloud::sensor_orientation_
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:408
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:443
pcl::PointCloud::sensor_origin_
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:406
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:395
pcl::getFieldSize
int getFieldSize(const int datatype)
Obtains the size of a specific field data type in bytes.
Definition: io.h:127
pcl::getFieldType
int getFieldType(const int size, char type)
Obtains the type of the PCLPointField from a specific size and type.
Definition: io.h:171
pcl::io::raw_close
int raw_close(int fd)
Definition: low_level_io.h:85
pcl::io::raw_fallocate
int raw_fallocate(int fd, long length)
Definition: low_level_io.h:110
pcl::io::raw_open
int raw_open(const char *pathname, int flags, int mode)
Definition: low_level_io.h:75
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::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133