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