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