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