40 #ifndef PCL_IO_PCD_IO_IMPL_H_
41 #define PCL_IO_PCD_IO_IMPL_H_
47 #include <pcl/common/io.h>
48 #include <pcl/console/print.h>
49 #include <pcl/io/boost.h>
50 #include <pcl/io/low_level_io.h>
51 #include <pcl/io/pcd_io.h>
53 #include <pcl/io/lzf.h>
56 template <
typename Po
intT> std::string
59 std::ostringstream oss;
60 oss.imbue (std::locale::classic ());
62 oss <<
"# .PCD v0.7 - Point Cloud Data file format"
66 const auto fields = pcl::getFields<PointT> ();
68 std::stringstream field_names, field_types, field_sizes, field_counts;
69 for (
const auto &field : fields)
71 if (field.name ==
"_")
74 field_names <<
" " << field.name;
76 if (
"rgb" == field.name)
77 field_types <<
" " <<
"U";
80 int count = std::abs (
static_cast<int> (field.count));
81 if (count == 0) count = 1;
82 field_counts <<
" " << count;
84 oss << field_names.str ();
85 oss <<
"\nSIZE" << field_sizes.str ()
86 <<
"\nTYPE" << field_types.str ()
87 <<
"\nCOUNT" << field_counts.str ();
89 if (nr_points != std::numeric_limits<int>::max ())
90 oss <<
"\nWIDTH " << nr_points <<
"\nHEIGHT " << 1 <<
"\n";
92 oss <<
"\nWIDTH " << cloud.
width <<
"\nHEIGHT " << cloud.
height <<
"\n";
101 if (nr_points != std::numeric_limits<int>::max ())
102 oss <<
"POINTS " << nr_points <<
"\n";
104 oss <<
"POINTS " << cloud.
size () <<
"\n";
110 template <
typename Po
intT>
int
116 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Input point cloud has no data!");
120 std::ostringstream oss;
121 oss << generateHeader<PointT> (cloud) <<
"DATA binary\n";
123 data_idx =
static_cast<int> (oss.tellp ());
126 HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
127 if (h_native_file == INVALID_HANDLE_VALUE)
129 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during CreateFile!");
133 int fd =
io::raw_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
136 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during open!");
141 boost::interprocess::file_lock file_lock;
142 setLockingPermissions (file_name, file_lock);
144 auto fields = pcl::getFields<PointT> ();
145 std::vector<int> fields_sizes;
146 std::size_t fsize = 0;
147 std::size_t data_size = 0;
150 for (
const auto &field : fields)
152 if (field.name ==
"_")
157 fields_sizes.push_back (fs);
158 fields[nri++] = field;
162 data_size = cloud.
size () * fsize;
166 HANDLE fm = CreateFileMappingA (h_native_file, NULL, PAGE_READWRITE, 0, (DWORD) (data_idx + data_size), NULL);
169 throw pcl::IOException(
"[pcl::PCDWriter::writeBinary] Error during memory map creation ()!");
172 char *map =
static_cast<char*
>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, data_idx + data_size));
180 resetLockingPermissions (file_name, file_lock);
181 PCL_ERROR (
"[pcl::PCDWriter::writeBinary] posix_fallocate errno: %d strerror: %s\n", errno, strerror (errno));
183 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during posix_fallocate ()!");
187 char *map =
static_cast<char*
> (::mmap (
nullptr, data_idx + data_size, PROT_WRITE, MAP_SHARED, fd, 0));
188 if (map ==
reinterpret_cast<char*
> (-1))
191 resetLockingPermissions (file_name, file_lock);
192 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during mmap ()!");
198 memcpy (&map[0], oss.str ().c_str (), data_idx);
201 char *out = &map[0] + data_idx;
202 for (
const auto& point: cloud)
205 for (
const auto &field : fields)
207 memcpy (out,
reinterpret_cast<const char*
> (&point) + field.offset, fields_sizes[nrj]);
208 out += fields_sizes[nrj++];
214 if (map_synchronization_)
215 msync (map, data_idx + data_size, MS_SYNC);
220 UnmapViewOfFile (map);
222 if (::munmap (map, (data_idx + data_size)) == -1)
225 resetLockingPermissions (file_name, file_lock);
226 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during munmap ()!");
232 CloseHandle (h_native_file);
236 resetLockingPermissions (file_name, file_lock);
241 template <
typename Po
intT>
int
247 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Input point cloud has no data!");
251 std::ostringstream oss;
252 oss << generateHeader<PointT> (cloud) <<
"DATA binary_compressed\n";
254 data_idx =
static_cast<int> (oss.tellp ());
257 HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
258 if (h_native_file == INVALID_HANDLE_VALUE)
260 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during CreateFile!");
264 int fd =
io::raw_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
267 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during open!");
273 boost::interprocess::file_lock file_lock;
274 setLockingPermissions (file_name, file_lock);
276 auto fields = pcl::getFields<PointT> ();
277 std::size_t fsize = 0;
278 std::size_t data_size = 0;
280 std::vector<int> fields_sizes (fields.size ());
282 for (
const auto &field : fields)
284 if (field.name ==
"_")
288 fsize += fields_sizes[nri];
292 fields_sizes.resize (nri);
296 data_size = cloud.
size () * fsize;
300 if (data_size * 3 / 2 > std::numeric_limits<std::uint32_t>::max ())
302 PCL_ERROR (
"[pcl::PCDWriter::writeBinaryCompressed] The input data exceeds the maximum size for compressed version 0.7 pcds of %l bytes.\n",
303 static_cast<std::size_t
> (std::numeric_limits<std::uint32_t>::max ()) * 2 / 3);
312 char *only_valid_data =
static_cast<char*
> (malloc (data_size));
322 std::vector<char*> pters (fields.size ());
323 std::size_t toff = 0;
324 for (std::size_t i = 0; i < pters.size (); ++i)
326 pters[i] = &only_valid_data[toff];
327 toff +=
static_cast<std::size_t
>(fields_sizes[i]) * cloud.
size();
331 for (
const auto& point: cloud)
333 for (std::size_t j = 0; j < fields.size (); ++j)
335 memcpy (pters[j],
reinterpret_cast<const char*
> (&point) + fields[j].offset, fields_sizes[j]);
337 pters[j] += fields_sizes[j];
341 char* temp_buf =
static_cast<char*
> (malloc (
static_cast<std::size_t
> (
static_cast<float> (data_size) * 1.5f + 8.0f)));
344 static_cast<std::uint32_t
> (data_size),
346 static_cast<std::uint32_t
> (
static_cast<float>(data_size) * 1.5f));
347 unsigned int compressed_final_size = 0;
351 char *header = &temp_buf[0];
352 memcpy (&header[0], &compressed_size,
sizeof (
unsigned int));
353 memcpy (&header[4], &data_size,
sizeof (
unsigned int));
354 data_size = compressed_size + 8;
355 compressed_final_size =
static_cast<std::uint32_t
> (data_size) + data_idx;
362 resetLockingPermissions (file_name, file_lock);
363 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during compression!");
369 HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, compressed_final_size, NULL);
370 char *map =
static_cast<char*
>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, compressed_final_size));
378 resetLockingPermissions (file_name, file_lock);
379 PCL_ERROR (
"[pcl::PCDWriter::writeBinaryCompressed] posix_fallocate errno: %d strerror: %s\n", errno, strerror (errno));
381 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during posix_fallocate ()!");
385 char *map =
static_cast<char*
> (::mmap (
nullptr, compressed_final_size, PROT_WRITE, MAP_SHARED, fd, 0));
386 if (map ==
reinterpret_cast<char*
> (-1))
389 resetLockingPermissions (file_name, file_lock);
390 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during mmap ()!");
396 memcpy (&map[0], oss.str ().c_str (), data_idx);
398 memcpy (&map[data_idx], temp_buf, data_size);
402 if (map_synchronization_)
403 msync (map, compressed_final_size, MS_SYNC);
408 UnmapViewOfFile (map);
410 if (::munmap (map, (compressed_final_size)) == -1)
413 resetLockingPermissions (file_name, file_lock);
414 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during munmap ()!");
421 CloseHandle (h_native_file);
425 resetLockingPermissions (file_name, file_lock);
427 free (only_valid_data);
433 template <
typename Po
intT>
int
439 throw pcl::IOException (
"[pcl::PCDWriter::writeASCII] Input point cloud has no data!");
445 throw pcl::IOException (
"[pcl::PCDWriter::writeASCII] Number of points different than width * height!");
450 fs.open (file_name.c_str ());
452 if (!fs.is_open () || fs.fail ())
454 throw pcl::IOException (
"[pcl::PCDWriter::writeASCII] Could not open file for writing!");
459 boost::interprocess::file_lock file_lock;
460 setLockingPermissions (file_name, file_lock);
462 fs.precision (precision);
463 fs.imbue (std::locale::classic ());
465 const auto fields = pcl::getFields<PointT> ();
468 fs << generateHeader<PointT> (cloud) <<
"DATA ascii\n";
470 std::ostringstream stream;
471 stream.precision (precision);
472 stream.imbue (std::locale::classic ());
474 for (
const auto& point: cloud)
476 for (std::size_t d = 0; d < fields.size (); ++d)
479 if (fields[d].name ==
"_")
482 int count = fields[d].count;
486 for (
int c = 0; c < count; ++c)
488 switch (fields[d].datatype)
493 memcpy (&value,
reinterpret_cast<const char*
> (&point) + fields[d].offset + c *
sizeof (std::int8_t),
sizeof (std::int8_t));
494 stream << boost::numeric_cast<std::int32_t>(value);
500 memcpy (&value,
reinterpret_cast<const char*
> (&point) + fields[d].offset + c *
sizeof (std::uint8_t),
sizeof (std::uint8_t));
501 stream << boost::numeric_cast<std::uint32_t>(value);
507 memcpy (&value,
reinterpret_cast<const char*
> (&point) + fields[d].offset + c *
sizeof (std::int16_t),
sizeof (std::int16_t));
508 stream << boost::numeric_cast<std::int16_t>(value);
514 memcpy (&value,
reinterpret_cast<const char*
> (&point) + fields[d].offset + c *
sizeof (std::uint16_t),
sizeof (std::uint16_t));
515 stream << boost::numeric_cast<std::uint16_t>(value);
521 memcpy (&value,
reinterpret_cast<const char*
> (&point) + fields[d].offset + c *
sizeof (std::int32_t),
sizeof (std::int32_t));
522 stream << boost::numeric_cast<std::int32_t>(value);
528 memcpy (&value,
reinterpret_cast<const char*
> (&point) + fields[d].offset + c *
sizeof (std::uint32_t),
sizeof (std::uint32_t));
529 stream << boost::numeric_cast<std::uint32_t>(value);
539 if (
"rgb" == fields[d].name)
542 memcpy (&value,
reinterpret_cast<const char*
> (&point) + fields[d].offset + c *
sizeof (
float),
sizeof (
float));
543 stream << boost::numeric_cast<std::uint32_t>(value);
547 memcpy (&value,
reinterpret_cast<const char*
> (&point) + fields[d].offset + c *
sizeof (
float),
sizeof (
float));
548 if (std::isnan (value))
551 stream << boost::numeric_cast<float>(value);
557 memcpy (&value,
reinterpret_cast<const char*
> (&point) + fields[d].offset + c *
sizeof (
double),
sizeof (
double));
558 if (std::isnan (value))
561 stream << boost::numeric_cast<double>(value);
565 PCL_WARN (
"[pcl::PCDWriter::writeASCII] Incorrect field data type specified (%d)!\n", fields[d].datatype);
569 if (d < fields.size () - 1 || c <
static_cast<int> (fields[d].count - 1))
574 std::string result = stream.str ();
575 boost::trim (result);
577 fs << result <<
"\n";
580 resetLockingPermissions (file_name, file_lock);
586 template <
typename Po
intT>
int
589 const std::vector<int> &indices)
591 if (cloud.
empty () || indices.empty ())
593 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Input point cloud has no data or empty indices given!");
597 std::ostringstream oss;
598 oss << generateHeader<PointT> (cloud,
static_cast<int> (indices.size ())) <<
"DATA binary\n";
600 data_idx =
static_cast<int> (oss.tellp ());
603 HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
604 if (h_native_file == INVALID_HANDLE_VALUE)
606 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during CreateFile!");
610 int fd =
io::raw_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
613 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during open!");
618 boost::interprocess::file_lock file_lock;
619 setLockingPermissions (file_name, file_lock);
621 auto fields = pcl::getFields<PointT> ();
622 std::vector<int> fields_sizes;
623 std::size_t fsize = 0;
624 std::size_t data_size = 0;
627 for (
const auto &field : fields)
629 if (field.name ==
"_")
634 fields_sizes.push_back (fs);
635 fields[nri++] = field;
639 data_size = indices.size () * fsize;
643 HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, data_idx + data_size, NULL);
644 char *map =
static_cast<char*
>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, data_idx + data_size));
652 resetLockingPermissions (file_name, file_lock);
653 PCL_ERROR (
"[pcl::PCDWriter::writeBinary] posix_fallocate errno: %d strerror: %s\n", errno, strerror (errno));
655 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during posix_fallocate ()!");
659 char *map =
static_cast<char*
> (::mmap (
nullptr, data_idx + data_size, PROT_WRITE, MAP_SHARED, fd, 0));
660 if (map ==
reinterpret_cast<char*
> (-1))
663 resetLockingPermissions (file_name, file_lock);
664 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during mmap ()!");
670 memcpy (&map[0], oss.str ().c_str (), data_idx);
672 char *out = &map[0] + data_idx;
674 for (
const auto &index : indices)
677 for (
const auto &field : fields)
679 memcpy (out,
reinterpret_cast<const char*
> (&cloud[index]) + field.offset, fields_sizes[nrj]);
680 out += fields_sizes[nrj++];
686 if (map_synchronization_)
687 msync (map, data_idx + data_size, MS_SYNC);
692 UnmapViewOfFile (map);
694 if (::munmap (map, (data_idx + data_size)) == -1)
697 resetLockingPermissions (file_name, file_lock);
698 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during munmap ()!");
704 CloseHandle(h_native_file);
709 resetLockingPermissions (file_name, file_lock);
714 template <
typename Po
intT>
int
717 const std::vector<int> &indices,
720 if (cloud.
empty () || indices.empty ())
722 throw pcl::IOException (
"[pcl::PCDWriter::writeASCII] Input point cloud has no data or empty indices given!");
728 throw pcl::IOException (
"[pcl::PCDWriter::writeASCII] Number of points different than width * height!");
733 fs.open (file_name.c_str ());
734 if (!fs.is_open () || fs.fail ())
736 throw pcl::IOException (
"[pcl::PCDWriter::writeASCII] Could not open file for writing!");
741 boost::interprocess::file_lock file_lock;
742 setLockingPermissions (file_name, file_lock);
744 fs.precision (precision);
745 fs.imbue (std::locale::classic ());
747 const auto fields = pcl::getFields<PointT> ();
750 fs << generateHeader<PointT> (cloud,
static_cast<int> (indices.size ())) <<
"DATA ascii\n";
752 std::ostringstream stream;
753 stream.precision (precision);
754 stream.imbue (std::locale::classic ());
757 for (
const auto &index : indices)
759 for (std::size_t d = 0; d < fields.size (); ++d)
762 if (fields[d].name ==
"_")
765 int count = fields[d].count;
769 for (
int c = 0; c < count; ++c)
771 switch (fields[d].datatype)
776 memcpy (&value,
reinterpret_cast<const char*
> (&cloud[index]) + fields[d].offset + c *
sizeof (std::int8_t),
sizeof (std::int8_t));
777 stream << boost::numeric_cast<std::int32_t>(value);
783 memcpy (&value,
reinterpret_cast<const char*
> (&cloud[index]) + fields[d].offset + c *
sizeof (std::uint8_t),
sizeof (std::uint8_t));
784 stream << boost::numeric_cast<std::uint32_t>(value);
790 memcpy (&value,
reinterpret_cast<const char*
> (&cloud[index]) + fields[d].offset + c *
sizeof (std::int16_t),
sizeof (std::int16_t));
791 stream << boost::numeric_cast<std::int16_t>(value);
797 memcpy (&value,
reinterpret_cast<const char*
> (&cloud[index]) + fields[d].offset + c *
sizeof (std::uint16_t),
sizeof (std::uint16_t));
798 stream << boost::numeric_cast<std::uint16_t>(value);
804 memcpy (&value,
reinterpret_cast<const char*
> (&cloud[index]) + fields[d].offset + c *
sizeof (std::int32_t),
sizeof (std::int32_t));
805 stream << boost::numeric_cast<std::int32_t>(value);
811 memcpy (&value,
reinterpret_cast<const char*
> (&cloud[index]) + fields[d].offset + c *
sizeof (std::uint32_t),
sizeof (std::uint32_t));
812 stream << boost::numeric_cast<std::uint32_t>(value);
822 if (
"rgb" == fields[d].name)
825 memcpy (&value,
reinterpret_cast<const char*
> (&cloud[index]) + fields[d].offset + c *
sizeof (
float),
sizeof (
float));
826 stream << boost::numeric_cast<std::uint32_t>(value);
831 memcpy (&value,
reinterpret_cast<const char*
> (&cloud[index]) + fields[d].offset + c *
sizeof (
float),
sizeof (
float));
832 if (std::isnan (value))
835 stream << boost::numeric_cast<float>(value);
842 memcpy (&value,
reinterpret_cast<const char*
> (&cloud[index]) + fields[d].offset + c *
sizeof (
double),
sizeof (
double));
843 if (std::isnan (value))
846 stream << boost::numeric_cast<double>(value);
850 PCL_WARN (
"[pcl::PCDWriter::writeASCII] Incorrect field data type specified (%d)!\n", fields[d].datatype);
854 if (d < fields.size () - 1 || c <
static_cast<int> (fields[d].count - 1))
859 std::string result = stream.str ();
860 boost::trim (result);
862 fs << result <<
"\n";
866 resetLockingPermissions (file_name, file_lock);
871 #endif //#ifndef PCL_IO_PCD_IO_H_