Point Cloud Library (PCL)  1.9.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 <fstream>
44 #include <fcntl.h>
45 #include <string>
46 #include <cstdlib>
47 #include <pcl/console/print.h>
48 #include <pcl/io/boost.h>
49 #include <pcl/io/low_level_io.h>
50 #include <pcl/io/pcd_io.h>
51 
52 #include <pcl/io/lzf.h>
53 
54 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
55 template <typename PointT> std::string
56 pcl::PCDWriter::generateHeader (const pcl::PointCloud<PointT> &cloud, const int nr_points)
57 {
58  std::ostringstream oss;
59  oss.imbue (std::locale::classic ());
60 
61  oss << "# .PCD v0.7 - Point Cloud Data file format"
62  "\nVERSION 0.7"
63  "\nFIELDS";
64 
65  const auto fields = pcl::getFields<PointT> ();
66 
67  std::stringstream field_names, field_types, field_sizes, field_counts;
68  for (const auto &field : fields)
69  {
70  if (field.name == "_")
71  continue;
72  // Add the regular dimension
73  field_names << " " << field.name;
74  field_sizes << " " << pcl::getFieldSize (field.datatype);
75  if ("rgb" == field.name)
76  field_types << " " << "U";
77  else
78  field_types << " " << pcl::getFieldType (field.datatype);
79  int count = std::abs (static_cast<int> (field.count));
80  if (count == 0) count = 1; // check for 0 counts (coming from older converter code)
81  field_counts << " " << count;
82  }
83  oss << field_names.str ();
84  oss << "\nSIZE" << field_sizes.str ()
85  << "\nTYPE" << field_types.str ()
86  << "\nCOUNT" << field_counts.str ();
87  // If the user passes in a number of points value, use that instead
88  if (nr_points != std::numeric_limits<int>::max ())
89  oss << "\nWIDTH " << nr_points << "\nHEIGHT " << 1 << "\n";
90  else
91  oss << "\nWIDTH " << cloud.width << "\nHEIGHT " << cloud.height << "\n";
92 
93  oss << "VIEWPOINT " << cloud.sensor_origin_[0] << " " << cloud.sensor_origin_[1] << " " << cloud.sensor_origin_[2] << " " <<
94  cloud.sensor_orientation_.w () << " " <<
95  cloud.sensor_orientation_.x () << " " <<
96  cloud.sensor_orientation_.y () << " " <<
97  cloud.sensor_orientation_.z () << "\n";
98 
99  // If the user passes in a number of points value, use that instead
100  if (nr_points != std::numeric_limits<int>::max ())
101  oss << "POINTS " << nr_points << "\n";
102  else
103  oss << "POINTS " << cloud.points.size () << "\n";
104 
105  return (oss.str ());
106 }
107 
108 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
109 template <typename PointT> int
110 pcl::PCDWriter::writeBinary (const std::string &file_name,
111  const pcl::PointCloud<PointT> &cloud)
112 {
113  if (cloud.empty ())
114  {
115  throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Input point cloud has no data!");
116  return (-1);
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 #if _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.points.size () * fsize;
162 
163  // Prepare the map
164 #if _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  if (io::raw_fallocate (fd, data_idx + data_size) != 0)
177  {
178  io::raw_close (fd);
179  resetLockingPermissions (file_name, file_lock);
180  PCL_ERROR ("[pcl::PCDWriter::writeBinary] posix_fallocate errno: %d strerror: %s\n", errno, strerror (errno));
181 
182  throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during posix_fallocate ()!");
183  return (-1);
184  }
185 
186  char *map = static_cast<char*> (::mmap (nullptr, data_idx + data_size, PROT_WRITE, MAP_SHARED, fd, 0));
187  if (map == reinterpret_cast<char*> (-1)) //MAP_FAILED)
188  {
189  io::raw_close (fd);
190  resetLockingPermissions (file_name, file_lock);
191  throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during mmap ()!");
192  return (-1);
193  }
194 #endif
195 
196  // Copy the header
197  memcpy (&map[0], oss.str ().c_str (), data_idx);
198 
199  // Copy the data
200  char *out = &map[0] + data_idx;
201  for (std::size_t i = 0; i < cloud.points.size (); ++i)
202  {
203  int nrj = 0;
204  for (const auto &field : fields)
205  {
206  memcpy (out, reinterpret_cast<const char*> (&cloud.points[i]) + field.offset, fields_sizes[nrj]);
207  out += fields_sizes[nrj++];
208  }
209  }
210 
211  // If the user set the synchronization flag on, call msync
212 #if !_WIN32
213  if (map_synchronization_)
214  msync (map, data_idx + data_size, MS_SYNC);
215 #endif
216 
217  // Unmap the pages of memory
218 #if _WIN32
219  UnmapViewOfFile (map);
220 #else
221  if (::munmap (map, (data_idx + data_size)) == -1)
222  {
223  io::raw_close (fd);
224  resetLockingPermissions (file_name, file_lock);
225  throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during munmap ()!");
226  return (-1);
227  }
228 #endif
229  // Close file
230 #if _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.points.empty ())
245  {
246  throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Input point cloud has no data!");
247  return (-1);
248  }
249  int data_idx = 0;
250  std::ostringstream oss;
251  oss << generateHeader<PointT> (cloud) << "DATA binary_compressed\n";
252  oss.flush ();
253  data_idx = static_cast<int> (oss.tellp ());
254 
255 #if _WIN32
256  HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
257  if (h_native_file == INVALID_HANDLE_VALUE)
258  {
259  throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during CreateFile!");
260  return (-1);
261  }
262 #else
263  int fd = io::raw_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
264  if (fd < 0)
265  {
266  throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during open!");
267  return (-1);
268  }
269 #endif
270 
271  // Mandatory lock file
272  boost::interprocess::file_lock file_lock;
273  setLockingPermissions (file_name, file_lock);
274 
275  auto fields = pcl::getFields<PointT> ();
276  std::size_t fsize = 0;
277  std::size_t data_size = 0;
278  std::size_t nri = 0;
279  std::vector<int> fields_sizes (fields.size ());
280  // Compute the total size of the fields
281  for (const auto &field : fields)
282  {
283  if (field.name == "_")
284  continue;
285 
286  fields_sizes[nri] = field.count * pcl::getFieldSize (field.datatype);
287  fsize += fields_sizes[nri];
288  fields[nri] = field;
289  ++nri;
290  }
291  fields_sizes.resize (nri);
292  fields.resize (nri);
293 
294  // Compute the size of data
295  data_size = cloud.points.size () * fsize;
296 
297  // If the data is to large the two 32 bit integers used to store the
298  // compressed and uncompressed size will overflow.
299  if (data_size * 3 / 2 > std::numeric_limits<std::uint32_t>::max ())
300  {
301  PCL_ERROR ("[pcl::PCDWriter::writeBinaryCompressed] The input data exceeds the maximum size for compressed version 0.7 pcds of %l bytes.\n",
302  static_cast<std::size_t> (std::numeric_limits<std::uint32_t>::max ()) * 2 / 3);
303  return (-2);
304  }
305 
306  //////////////////////////////////////////////////////////////////////
307  // Empty array holding only the valid data
308  // data_size = nr_points * point_size
309  // = nr_points * (sizeof_field_1 + sizeof_field_2 + ... sizeof_field_n)
310  // = sizeof_field_1 * nr_points + sizeof_field_2 * nr_points + ... sizeof_field_n * nr_points
311  char *only_valid_data = static_cast<char*> (malloc (data_size));
312 
313  // Convert the XYZRGBXYZRGB structure to XXYYZZRGBRGB to aid compression. For
314  // this, we need a vector of fields.size () (4 in this case), which points to
315  // each individual plane:
316  // pters[0] = &only_valid_data[offset_of_plane_x];
317  // pters[1] = &only_valid_data[offset_of_plane_y];
318  // pters[2] = &only_valid_data[offset_of_plane_z];
319  // pters[3] = &only_valid_data[offset_of_plane_RGB];
320  //
321  std::vector<char*> pters (fields.size ());
322  std::size_t toff = 0;
323  for (std::size_t i = 0; i < pters.size (); ++i)
324  {
325  pters[i] = &only_valid_data[toff];
326  toff += static_cast<std::size_t>(fields_sizes[i]) * cloud.points.size();
327  }
328 
329  // Go over all the points, and copy the data in the appropriate places
330  for (std::size_t i = 0; i < cloud.points.size (); ++i)
331  {
332  for (std::size_t j = 0; j < fields.size (); ++j)
333  {
334  memcpy (pters[j], reinterpret_cast<const char*> (&cloud.points[i]) + fields[j].offset, fields_sizes[j]);
335  // Increment the pointer
336  pters[j] += fields_sizes[j];
337  }
338  }
339 
340  char* temp_buf = static_cast<char*> (malloc (static_cast<std::size_t> (static_cast<float> (data_size) * 1.5f + 8.0f)));
341  // Compress the valid data
342  unsigned int compressed_size = pcl::lzfCompress (only_valid_data,
343  static_cast<std::uint32_t> (data_size),
344  &temp_buf[8],
345  static_cast<std::uint32_t> (static_cast<float>(data_size) * 1.5f));
346  unsigned int compressed_final_size = 0;
347  // Was the compression successful?
348  if (compressed_size)
349  {
350  char *header = &temp_buf[0];
351  memcpy (&header[0], &compressed_size, sizeof (unsigned int));
352  memcpy (&header[4], &data_size, sizeof (unsigned int));
353  data_size = compressed_size + 8;
354  compressed_final_size = static_cast<std::uint32_t> (data_size) + data_idx;
355  }
356  else
357  {
358 #if !_WIN32
359  io::raw_close (fd);
360 #endif
361  resetLockingPermissions (file_name, file_lock);
362  throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during compression!");
363  return (-1);
364  }
365 
366  // Prepare the map
367 #if _WIN32
368  HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, compressed_final_size, NULL);
369  char *map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, compressed_final_size));
370  CloseHandle (fm);
371 
372 #else
373  // Allocate disk space for the entire file to prevent bus errors.
374  if (io::raw_fallocate (fd, compressed_final_size) != 0)
375  {
376  io::raw_close (fd);
377  resetLockingPermissions (file_name, file_lock);
378  PCL_ERROR ("[pcl::PCDWriter::writeBinaryCompressed] posix_fallocate errno: %d strerror: %s\n", errno, strerror (errno));
379 
380  throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during posix_fallocate ()!");
381  return (-1);
382  }
383 
384  char *map = static_cast<char*> (::mmap (nullptr, compressed_final_size, PROT_WRITE, MAP_SHARED, fd, 0));
385  if (map == reinterpret_cast<char*> (-1)) //MAP_FAILED)
386  {
387  io::raw_close (fd);
388  resetLockingPermissions (file_name, file_lock);
389  throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during mmap ()!");
390  return (-1);
391  }
392 #endif
393 
394  // Copy the header
395  memcpy (&map[0], oss.str ().c_str (), data_idx);
396  // Copy the compressed data
397  memcpy (&map[data_idx], temp_buf, data_size);
398 
399 #if !_WIN32
400  // If the user set the synchronization flag on, call msync
401  if (map_synchronization_)
402  msync (map, compressed_final_size, MS_SYNC);
403 #endif
404 
405  // Unmap the pages of memory
406 #if _WIN32
407  UnmapViewOfFile (map);
408 #else
409  if (::munmap (map, (compressed_final_size)) == -1)
410  {
411  io::raw_close (fd);
412  resetLockingPermissions (file_name, file_lock);
413  throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during munmap ()!");
414  return (-1);
415  }
416 #endif
417 
418  // Close file
419 #if _WIN32
420  CloseHandle (h_native_file);
421 #else
422  io::raw_close (fd);
423 #endif
424  resetLockingPermissions (file_name, file_lock);
425 
426  free (only_valid_data);
427  free (temp_buf);
428  return (0);
429 }
430 
431 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
432 template <typename PointT> int
433 pcl::PCDWriter::writeASCII (const std::string &file_name, const pcl::PointCloud<PointT> &cloud,
434  const int precision)
435 {
436  if (cloud.empty ())
437  {
438  throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Input point cloud has no data!");
439  return (-1);
440  }
441 
442  if (cloud.width * cloud.height != cloud.points.size ())
443  {
444  throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Number of points different than width * height!");
445  return (-1);
446  }
447 
448  std::ofstream fs;
449  fs.open (file_name.c_str ()); // Open file
450 
451  if (!fs.is_open () || fs.fail ())
452  {
453  throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Could not open file for writing!");
454  return (-1);
455  }
456 
457  // Mandatory lock file
458  boost::interprocess::file_lock file_lock;
459  setLockingPermissions (file_name, file_lock);
460 
461  fs.precision (precision);
462  fs.imbue (std::locale::classic ());
463 
464  const auto fields = pcl::getFields<PointT> ();
465 
466  // Write the header information
467  fs << generateHeader<PointT> (cloud) << "DATA ascii\n";
468 
469  std::ostringstream stream;
470  stream.precision (precision);
471  stream.imbue (std::locale::classic ());
472  // Iterate through the points
473  for (std::size_t i = 0; i < cloud.points.size (); ++i)
474  {
475  for (std::size_t d = 0; d < fields.size (); ++d)
476  {
477  // Ignore invalid padded dimensions that are inherited from binary data
478  if (fields[d].name == "_")
479  continue;
480 
481  int count = fields[d].count;
482  if (count == 0)
483  count = 1; // we simply cannot tolerate 0 counts (coming from older converter code)
484 
485  for (int c = 0; c < count; ++c)
486  {
487  switch (fields[d].datatype)
488  {
490  {
491  std::int8_t value;
492  memcpy (&value, reinterpret_cast<const char*> (&cloud.points[i]) + fields[d].offset + c * sizeof (std::int8_t), sizeof (std::int8_t));
493  stream << boost::numeric_cast<std::int32_t>(value);
494  break;
495  }
497  {
498  std::uint8_t value;
499  memcpy (&value, reinterpret_cast<const char*> (&cloud.points[i]) + fields[d].offset + c * sizeof (std::uint8_t), sizeof (std::uint8_t));
500  stream << boost::numeric_cast<std::uint32_t>(value);
501  break;
502  }
504  {
505  std::int16_t value;
506  memcpy (&value, reinterpret_cast<const char*> (&cloud.points[i]) + fields[d].offset + c * sizeof (std::int16_t), sizeof (std::int16_t));
507  stream << boost::numeric_cast<std::int16_t>(value);
508  break;
509  }
511  {
512  std::uint16_t value;
513  memcpy (&value, reinterpret_cast<const char*> (&cloud.points[i]) + fields[d].offset + c * sizeof (std::uint16_t), sizeof (std::uint16_t));
514  stream << boost::numeric_cast<std::uint16_t>(value);
515  break;
516  }
518  {
519  std::int32_t value;
520  memcpy (&value, reinterpret_cast<const char*> (&cloud.points[i]) + fields[d].offset + c * sizeof (std::int32_t), sizeof (std::int32_t));
521  stream << boost::numeric_cast<std::int32_t>(value);
522  break;
523  }
525  {
526  std::uint32_t value;
527  memcpy (&value, reinterpret_cast<const char*> (&cloud.points[i]) + fields[d].offset + c * sizeof (std::uint32_t), sizeof (std::uint32_t));
528  stream << boost::numeric_cast<std::uint32_t>(value);
529  break;
530  }
532  {
533  /*
534  * Despite the float type, store the rgb field as uint32
535  * because several fully opaque color values are mapped to
536  * nan.
537  */
538  if ("rgb" == fields[d].name)
539  {
540  std::uint32_t value;
541  memcpy (&value, reinterpret_cast<const char*> (&cloud.points[i]) + fields[d].offset + c * sizeof (float), sizeof (float));
542  stream << boost::numeric_cast<std::uint32_t>(value);
543  break;
544  }
545  float value;
546  memcpy (&value, reinterpret_cast<const char*> (&cloud.points[i]) + fields[d].offset + c * sizeof (float), sizeof (float));
547  if (std::isnan (value))
548  stream << "nan";
549  else
550  stream << boost::numeric_cast<float>(value);
551  break;
552  }
554  {
555  double value;
556  memcpy (&value, reinterpret_cast<const char*> (&cloud.points[i]) + fields[d].offset + c * sizeof (double), sizeof (double));
557  if (std::isnan (value))
558  stream << "nan";
559  else
560  stream << boost::numeric_cast<double>(value);
561  break;
562  }
563  default:
564  PCL_WARN ("[pcl::PCDWriter::writeASCII] Incorrect field data type specified (%d)!\n", fields[d].datatype);
565  break;
566  }
567 
568  if (d < fields.size () - 1 || c < static_cast<int> (fields[d].count - 1))
569  stream << " ";
570  }
571  }
572  // Copy the stream, trim it, and write it to disk
573  std::string result = stream.str ();
574  boost::trim (result);
575  stream.str ("");
576  fs << result << "\n";
577  }
578  fs.close (); // Close file
579  resetLockingPermissions (file_name, file_lock);
580  return (0);
581 }
582 
583 
584 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
585 template <typename PointT> int
586 pcl::PCDWriter::writeBinary (const std::string &file_name,
587  const pcl::PointCloud<PointT> &cloud,
588  const std::vector<int> &indices)
589 {
590  if (cloud.points.empty () || indices.empty ())
591  {
592  throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Input point cloud has no data or empty indices given!");
593  return (-1);
594  }
595  int data_idx = 0;
596  std::ostringstream oss;
597  oss << generateHeader<PointT> (cloud, static_cast<int> (indices.size ())) << "DATA binary\n";
598  oss.flush ();
599  data_idx = static_cast<int> (oss.tellp ());
600 
601 #if _WIN32
602  HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
603  if (h_native_file == INVALID_HANDLE_VALUE)
604  {
605  throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during CreateFile!");
606  return (-1);
607  }
608 #else
609  int fd = io::raw_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
610  if (fd < 0)
611  {
612  throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during open!");
613  return (-1);
614  }
615 #endif
616  // Mandatory lock file
617  boost::interprocess::file_lock file_lock;
618  setLockingPermissions (file_name, file_lock);
619 
620  auto fields = pcl::getFields<PointT> ();
621  std::vector<int> fields_sizes;
622  std::size_t fsize = 0;
623  std::size_t data_size = 0;
624  std::size_t nri = 0;
625  // Compute the total size of the fields
626  for (const auto &field : fields)
627  {
628  if (field.name == "_")
629  continue;
630 
631  int fs = field.count * getFieldSize (field.datatype);
632  fsize += fs;
633  fields_sizes.push_back (fs);
634  fields[nri++] = field;
635  }
636  fields.resize (nri);
637 
638  data_size = indices.size () * fsize;
639 
640  // Prepare the map
641 #if _WIN32
642  HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, data_idx + data_size, NULL);
643  char *map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, data_idx + data_size));
644  CloseHandle (fm);
645 
646 #else
647  // Allocate disk space for the entire file to prevent bus errors.
648  if (io::raw_fallocate (fd, data_idx + data_size) != 0)
649  {
650  io::raw_close (fd);
651  resetLockingPermissions (file_name, file_lock);
652  PCL_ERROR ("[pcl::PCDWriter::writeBinary] posix_fallocate errno: %d strerror: %s\n", errno, strerror (errno));
653 
654  throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during posix_fallocate ()!");
655  return (-1);
656  }
657 
658  char *map = static_cast<char*> (::mmap (nullptr, data_idx + data_size, PROT_WRITE, MAP_SHARED, fd, 0));
659  if (map == reinterpret_cast<char*> (-1)) //MAP_FAILED)
660  {
661  io::raw_close (fd);
662  resetLockingPermissions (file_name, file_lock);
663  throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during mmap ()!");
664  return (-1);
665  }
666 #endif
667 
668  // Copy the header
669  memcpy (&map[0], oss.str ().c_str (), data_idx);
670 
671  char *out = &map[0] + data_idx;
672  // Copy the data
673  for (const int &index : indices)
674  {
675  int nrj = 0;
676  for (const auto &field : fields)
677  {
678  memcpy (out, reinterpret_cast<const char*> (&cloud.points[index]) + field.offset, fields_sizes[nrj]);
679  out += fields_sizes[nrj++];
680  }
681  }
682 
683 #if !_WIN32
684  // If the user set the synchronization flag on, call msync
685  if (map_synchronization_)
686  msync (map, data_idx + data_size, MS_SYNC);
687 #endif
688 
689  // Unmap the pages of memory
690 #if _WIN32
691  UnmapViewOfFile (map);
692 #else
693  if (::munmap (map, (data_idx + data_size)) == -1)
694  {
695  io::raw_close (fd);
696  resetLockingPermissions (file_name, file_lock);
697  throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during munmap ()!");
698  return (-1);
699  }
700 #endif
701  // Close file
702 #if _WIN32
703  CloseHandle(h_native_file);
704 #else
705  io::raw_close (fd);
706 #endif
707 
708  resetLockingPermissions (file_name, file_lock);
709  return (0);
710 }
711 
712 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
713 template <typename PointT> int
714 pcl::PCDWriter::writeASCII (const std::string &file_name,
715  const pcl::PointCloud<PointT> &cloud,
716  const std::vector<int> &indices,
717  const int precision)
718 {
719  if (cloud.points.empty () || indices.empty ())
720  {
721  throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Input point cloud has no data or empty indices given!");
722  return (-1);
723  }
724 
725  if (cloud.width * cloud.height != cloud.points.size ())
726  {
727  throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Number of points different than width * height!");
728  return (-1);
729  }
730 
731  std::ofstream fs;
732  fs.open (file_name.c_str ()); // Open file
733  if (!fs.is_open () || fs.fail ())
734  {
735  throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Could not open file for writing!");
736  return (-1);
737  }
738 
739  // Mandatory lock file
740  boost::interprocess::file_lock file_lock;
741  setLockingPermissions (file_name, file_lock);
742 
743  fs.precision (precision);
744  fs.imbue (std::locale::classic ());
745 
746  const auto fields = pcl::getFields<PointT> ();
747 
748  // Write the header information
749  fs << generateHeader<PointT> (cloud, static_cast<int> (indices.size ())) << "DATA ascii\n";
750 
751  std::ostringstream stream;
752  stream.precision (precision);
753  stream.imbue (std::locale::classic ());
754 
755  // Iterate through the points
756  for (const int &index : indices)
757  {
758  for (std::size_t d = 0; d < fields.size (); ++d)
759  {
760  // Ignore invalid padded dimensions that are inherited from binary data
761  if (fields[d].name == "_")
762  continue;
763 
764  int count = fields[d].count;
765  if (count == 0)
766  count = 1; // we simply cannot tolerate 0 counts (coming from older converter code)
767 
768  for (int c = 0; c < count; ++c)
769  {
770  switch (fields[d].datatype)
771  {
773  {
774  std::int8_t value;
775  memcpy (&value, reinterpret_cast<const char*> (&cloud.points[index]) + fields[d].offset + c * sizeof (std::int8_t), sizeof (std::int8_t));
776  stream << boost::numeric_cast<std::int32_t>(value);
777  break;
778  }
780  {
781  std::uint8_t value;
782  memcpy (&value, reinterpret_cast<const char*> (&cloud.points[index]) + fields[d].offset + c * sizeof (std::uint8_t), sizeof (std::uint8_t));
783  stream << boost::numeric_cast<std::uint32_t>(value);
784  break;
785  }
787  {
788  std::int16_t value;
789  memcpy (&value, reinterpret_cast<const char*> (&cloud.points[index]) + fields[d].offset + c * sizeof (std::int16_t), sizeof (std::int16_t));
790  stream << boost::numeric_cast<std::int16_t>(value);
791  break;
792  }
794  {
795  std::uint16_t value;
796  memcpy (&value, reinterpret_cast<const char*> (&cloud.points[index]) + fields[d].offset + c * sizeof (std::uint16_t), sizeof (std::uint16_t));
797  stream << boost::numeric_cast<std::uint16_t>(value);
798  break;
799  }
801  {
802  std::int32_t value;
803  memcpy (&value, reinterpret_cast<const char*> (&cloud.points[index]) + fields[d].offset + c * sizeof (std::int32_t), sizeof (std::int32_t));
804  stream << boost::numeric_cast<std::int32_t>(value);
805  break;
806  }
808  {
809  std::uint32_t value;
810  memcpy (&value, reinterpret_cast<const char*> (&cloud.points[index]) + fields[d].offset + c * sizeof (std::uint32_t), sizeof (std::uint32_t));
811  stream << boost::numeric_cast<std::uint32_t>(value);
812  break;
813  }
815  {
816  /*
817  * Despite the float type, store the rgb field as uint32
818  * because several fully opaque color values are mapped to
819  * nan.
820  */
821  if ("rgb" == fields[d].name)
822  {
823  std::uint32_t value;
824  memcpy (&value, reinterpret_cast<const char*> (&cloud.points[index]) + fields[d].offset + c * sizeof (float), sizeof (float));
825  stream << boost::numeric_cast<std::uint32_t>(value);
826  }
827  else
828  {
829  float value;
830  memcpy (&value, reinterpret_cast<const char*> (&cloud.points[index]) + fields[d].offset + c * sizeof (float), sizeof (float));
831  if (std::isnan (value))
832  stream << "nan";
833  else
834  stream << boost::numeric_cast<float>(value);
835  }
836  break;
837  }
839  {
840  double value;
841  memcpy (&value, reinterpret_cast<const char*> (&cloud.points[index]) + fields[d].offset + c * sizeof (double), sizeof (double));
842  if (std::isnan (value))
843  stream << "nan";
844  else
845  stream << boost::numeric_cast<double>(value);
846  break;
847  }
848  default:
849  PCL_WARN ("[pcl::PCDWriter::writeASCII] Incorrect field data type specified (%d)!\n", fields[d].datatype);
850  break;
851  }
852 
853  if (d < fields.size () - 1 || c < static_cast<int> (fields[d].count - 1))
854  stream << " ";
855  }
856  }
857  // Copy the stream, trim it, and write it to disk
858  std::string result = stream.str ();
859  boost::trim (result);
860  stream.str ("");
861  fs << result << "\n";
862  }
863  fs.close (); // Close file
864 
865  resetLockingPermissions (file_name, file_lock);
866 
867  return (0);
868 }
869 
870 #endif //#ifndef PCL_IO_PCD_IO_H_
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:394
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.
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:56
int raw_open(const char *pathname, int flags, int mode)
Definition: low_level_io.h:112
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.
int raw_close(int fd)
Definition: low_level_io.h:122
int writeBinary(const std::string &file_name, const pcl::PCLPointCloud2 &cloud, const Eigen::Vector4f &origin=Eigen::Vector4f::Zero(), const Eigen::Quaternionf &orientation=Eigen::Quaternionf::Identity())
Save point cloud data to a PCD file containing n-D points, in BINARY format.
int getFieldType(const int size, char type)
Obtains the type of the PCLPointField from a specific size and type.
Definition: io.h:191
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:397
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:405
int raw_fallocate(int fd, off_t length)
Definition: low_level_io.h:168
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:407
void setLockingPermissions(const std::string &file_name, boost::interprocess::file_lock &lock)
Set permissions for file locking (Boost 1.49+).
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:399
int getFieldSize(const int datatype)
Obtains the size of a specific field data type in bytes.
Definition: io.h:152
An exception that is thrown during an IO error (typical read/write errors)
Definition: exceptions.h:179
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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...
void resetLockingPermissions(const std::string &file_name, boost::interprocess::file_lock &lock)
Reset permissions for file locking (Boost 1.49+).
bool empty() const
Definition: point_cloud.h:434