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