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