40 #ifndef PCL_IO_PCD_IO_IMPL_H_
41 #define PCL_IO_PCD_IO_IMPL_H_
47 #include <pcl/console/print.h>
48 #include <pcl/io/boost.h>
49 #include <pcl/io/low_level_io.h>
50 #include <pcl/io/pcd_io.h>
52 #include <pcl/io/lzf.h>
55 template <
typename Po
intT> std::string
58 std::ostringstream oss;
59 oss.imbue (std::locale::classic ());
61 oss <<
"# .PCD v0.7 - Point Cloud Data file format"
65 const auto fields = pcl::getFields<PointT> ();
67 std::stringstream field_names, field_types, field_sizes, field_counts;
68 for (
const auto &field : fields)
70 if (field.name ==
"_")
73 field_names <<
" " << field.name;
75 if (
"rgb" == field.name)
76 field_types <<
" " <<
"U";
79 int count = std::abs (static_cast<int> (field.count));
80 if (count == 0) count = 1;
81 field_counts <<
" " << count;
83 oss << field_names.str ();
84 oss <<
"\nSIZE" << field_sizes.str ()
85 <<
"\nTYPE" << field_types.str ()
86 <<
"\nCOUNT" << field_counts.str ();
88 if (nr_points != std::numeric_limits<int>::max ())
89 oss <<
"\nWIDTH " << nr_points <<
"\nHEIGHT " << 1 <<
"\n";
91 oss <<
"\nWIDTH " << cloud.
width <<
"\nHEIGHT " << cloud.
height <<
"\n";
100 if (nr_points != std::numeric_limits<int>::max ())
101 oss <<
"POINTS " << nr_points <<
"\n";
103 oss <<
"POINTS " << cloud.
points.size () <<
"\n";
109 template <
typename Po
intT>
int
115 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Input point cloud has no data!");
119 std::ostringstream oss;
120 oss << generateHeader<PointT> (cloud) <<
"DATA binary\n";
122 data_idx =
static_cast<int> (oss.tellp ());
125 HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
126 if (h_native_file == INVALID_HANDLE_VALUE)
128 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during CreateFile!");
132 int fd =
io::raw_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
135 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during open!");
140 boost::interprocess::file_lock file_lock;
141 setLockingPermissions (file_name, file_lock);
143 auto fields = pcl::getFields<PointT> ();
144 std::vector<int> fields_sizes;
145 std::size_t fsize = 0;
146 std::size_t data_size = 0;
149 for (
const auto &field : fields)
151 if (field.name ==
"_")
156 fields_sizes.push_back (fs);
157 fields[nri++] = field;
161 data_size = cloud.
points.size () * fsize;
165 HANDLE fm = CreateFileMappingA (h_native_file, NULL, PAGE_READWRITE, 0, (DWORD) (data_idx + data_size), NULL);
168 throw pcl::IOException(
"[pcl::PCDWriter::writeBinary] Error during memory map creation ()!");
171 char *map =
static_cast<char*
>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, data_idx + data_size));
179 resetLockingPermissions (file_name, file_lock);
180 PCL_ERROR (
"[pcl::PCDWriter::writeBinary] posix_fallocate errno: %d strerror: %s\n", errno, strerror (errno));
182 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during posix_fallocate ()!");
186 char *map =
static_cast<char*
> (::mmap (
nullptr, data_idx + data_size, PROT_WRITE, MAP_SHARED, fd, 0));
187 if (map == reinterpret_cast<char*> (-1))
190 resetLockingPermissions (file_name, file_lock);
191 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during mmap ()!");
197 memcpy (&map[0], oss.str ().c_str (), data_idx);
200 char *out = &map[0] + data_idx;
201 for (std::size_t i = 0; i < cloud.
points.size (); ++i)
204 for (
const auto &field : fields)
206 memcpy (out, reinterpret_cast<const char*> (&cloud.
points[i]) + field.offset, fields_sizes[nrj]);
207 out += fields_sizes[nrj++];
213 if (map_synchronization_)
214 msync (map, data_idx + data_size, MS_SYNC);
219 UnmapViewOfFile (map);
221 if (::munmap (map, (data_idx + data_size)) == -1)
224 resetLockingPermissions (file_name, file_lock);
225 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during munmap ()!");
231 CloseHandle (h_native_file);
235 resetLockingPermissions (file_name, file_lock);
240 template <
typename Po
intT>
int
244 if (cloud.
points.empty ())
246 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Input point cloud has no data!");
250 std::ostringstream oss;
251 oss << generateHeader<PointT> (cloud) <<
"DATA binary_compressed\n";
253 data_idx =
static_cast<int> (oss.tellp ());
256 HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
257 if (h_native_file == INVALID_HANDLE_VALUE)
259 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during CreateFile!");
263 int fd =
io::raw_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
266 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during open!");
272 boost::interprocess::file_lock file_lock;
273 setLockingPermissions (file_name, file_lock);
275 auto fields = pcl::getFields<PointT> ();
276 std::size_t fsize = 0;
277 std::size_t data_size = 0;
279 std::vector<int> fields_sizes (fields.size ());
281 for (
const auto &field : fields)
283 if (field.name ==
"_")
287 fsize += fields_sizes[nri];
291 fields_sizes.resize (nri);
295 data_size = cloud.
points.size () * fsize;
299 if (data_size * 3 / 2 > std::numeric_limits<std::uint32_t>::max ())
301 PCL_ERROR (
"[pcl::PCDWriter::writeBinaryCompressed] The input data exceeds the maximum size for compressed version 0.7 pcds of %l bytes.\n",
302 static_cast<std::size_t> (std::numeric_limits<std::uint32_t>::max ()) * 2 / 3);
311 char *only_valid_data =
static_cast<char*
> (malloc (data_size));
321 std::vector<char*> pters (fields.size ());
322 std::size_t toff = 0;
323 for (std::size_t i = 0; i < pters.size (); ++i)
325 pters[i] = &only_valid_data[toff];
326 toff +=
static_cast<std::size_t
>(fields_sizes[i]) * cloud.
points.size();
330 for (std::size_t i = 0; i < cloud.
points.size (); ++i)
332 for (std::size_t j = 0; j < fields.size (); ++j)
334 memcpy (pters[j], reinterpret_cast<const char*> (&cloud.
points[i]) + fields[j].offset, fields_sizes[j]);
336 pters[j] += fields_sizes[j];
340 char* temp_buf =
static_cast<char*
> (malloc (static_cast<std::size_t> (static_cast<float> (data_size) * 1.5f + 8.0f)));
343 static_cast<std::uint32_t> (data_size),
345 static_cast<std::uint32_t> (static_cast<float>(data_size) * 1.5f));
346 unsigned int compressed_final_size = 0;
350 char *header = &temp_buf[0];
351 memcpy (&header[0], &compressed_size,
sizeof (
unsigned int));
352 memcpy (&header[4], &data_size,
sizeof (
unsigned int));
353 data_size = compressed_size + 8;
354 compressed_final_size =
static_cast<std::uint32_t
> (data_size) + data_idx;
361 resetLockingPermissions (file_name, file_lock);
362 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during compression!");
368 HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, compressed_final_size, NULL);
369 char *map =
static_cast<char*
>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, compressed_final_size));
377 resetLockingPermissions (file_name, file_lock);
378 PCL_ERROR (
"[pcl::PCDWriter::writeBinaryCompressed] posix_fallocate errno: %d strerror: %s\n", errno, strerror (errno));
380 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during posix_fallocate ()!");
384 char *map =
static_cast<char*
> (::mmap (
nullptr, compressed_final_size, PROT_WRITE, MAP_SHARED, fd, 0));
385 if (map == reinterpret_cast<char*> (-1))
388 resetLockingPermissions (file_name, file_lock);
389 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during mmap ()!");
395 memcpy (&map[0], oss.str ().c_str (), data_idx);
397 memcpy (&map[data_idx], temp_buf, data_size);
401 if (map_synchronization_)
402 msync (map, compressed_final_size, MS_SYNC);
407 UnmapViewOfFile (map);
409 if (::munmap (map, (compressed_final_size)) == -1)
412 resetLockingPermissions (file_name, file_lock);
413 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during munmap ()!");
420 CloseHandle (h_native_file);
424 resetLockingPermissions (file_name, file_lock);
426 free (only_valid_data);
432 template <
typename Po
intT>
int
438 throw pcl::IOException (
"[pcl::PCDWriter::writeASCII] Input point cloud has no data!");
444 throw pcl::IOException (
"[pcl::PCDWriter::writeASCII] Number of points different than width * height!");
449 fs.open (file_name.c_str ());
451 if (!fs.is_open () || fs.fail ())
453 throw pcl::IOException (
"[pcl::PCDWriter::writeASCII] Could not open file for writing!");
458 boost::interprocess::file_lock file_lock;
459 setLockingPermissions (file_name, file_lock);
461 fs.precision (precision);
462 fs.imbue (std::locale::classic ());
464 const auto fields = pcl::getFields<PointT> ();
467 fs << generateHeader<PointT> (cloud) <<
"DATA ascii\n";
469 std::ostringstream stream;
470 stream.precision (precision);
471 stream.imbue (std::locale::classic ());
473 for (std::size_t i = 0; i < cloud.
points.size (); ++i)
475 for (std::size_t d = 0; d < fields.size (); ++d)
478 if (fields[d].name ==
"_")
481 int count = fields[d].count;
485 for (
int c = 0; c < count; ++c)
487 switch (fields[d].datatype)
492 memcpy (&value, reinterpret_cast<const char*> (&cloud.
points[i]) + fields[d].offset + c * sizeof (std::int8_t),
sizeof (std::int8_t));
493 stream << boost::numeric_cast<std::int32_t>(value);
499 memcpy (&value, reinterpret_cast<const char*> (&cloud.
points[i]) + fields[d].offset + c * sizeof (std::uint8_t),
sizeof (std::uint8_t));
500 stream << boost::numeric_cast<std::uint32_t>(value);
506 memcpy (&value, reinterpret_cast<const char*> (&cloud.
points[i]) + fields[d].offset + c * sizeof (std::int16_t),
sizeof (std::int16_t));
507 stream << boost::numeric_cast<std::int16_t>(value);
513 memcpy (&value, reinterpret_cast<const char*> (&cloud.
points[i]) + fields[d].offset + c * sizeof (std::uint16_t),
sizeof (std::uint16_t));
514 stream << boost::numeric_cast<std::uint16_t>(value);
520 memcpy (&value, reinterpret_cast<const char*> (&cloud.
points[i]) + fields[d].offset + c * sizeof (std::int32_t),
sizeof (std::int32_t));
521 stream << boost::numeric_cast<std::int32_t>(value);
527 memcpy (&value, reinterpret_cast<const char*> (&cloud.
points[i]) + fields[d].offset + c * sizeof (std::uint32_t),
sizeof (std::uint32_t));
528 stream << boost::numeric_cast<std::uint32_t>(value);
538 if (
"rgb" == fields[d].name)
541 memcpy (&value, reinterpret_cast<const char*> (&cloud.
points[i]) + fields[d].offset + c * sizeof (
float),
sizeof (
float));
542 stream << boost::numeric_cast<std::uint32_t>(value);
546 memcpy (&value, reinterpret_cast<const char*> (&cloud.
points[i]) + fields[d].offset + c * sizeof (
float),
sizeof (
float));
547 if (std::isnan (value))
550 stream << boost::numeric_cast<float>(value);
556 memcpy (&value, reinterpret_cast<const char*> (&cloud.
points[i]) + fields[d].offset + c * sizeof (
double),
sizeof (
double));
557 if (std::isnan (value))
560 stream << boost::numeric_cast<double>(value);
564 PCL_WARN (
"[pcl::PCDWriter::writeASCII] Incorrect field data type specified (%d)!\n", fields[d].datatype);
568 if (d < fields.size () - 1 || c < static_cast<int> (fields[d].count - 1))
573 std::string result = stream.str ();
574 boost::trim (result);
576 fs << result <<
"\n";
579 resetLockingPermissions (file_name, file_lock);
585 template <
typename Po
intT>
int
588 const std::vector<int> &indices)
590 if (cloud.
points.empty () || indices.empty ())
592 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Input point cloud has no data or empty indices given!");
596 std::ostringstream oss;
597 oss << generateHeader<PointT> (cloud,
static_cast<int> (indices.size ())) <<
"DATA binary\n";
599 data_idx =
static_cast<int> (oss.tellp ());
602 HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
603 if (h_native_file == INVALID_HANDLE_VALUE)
605 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during CreateFile!");
609 int fd =
io::raw_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
612 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during open!");
617 boost::interprocess::file_lock file_lock;
618 setLockingPermissions (file_name, file_lock);
620 auto fields = pcl::getFields<PointT> ();
621 std::vector<int> fields_sizes;
622 std::size_t fsize = 0;
623 std::size_t data_size = 0;
626 for (
const auto &field : fields)
628 if (field.name ==
"_")
633 fields_sizes.push_back (fs);
634 fields[nri++] = field;
638 data_size = indices.size () * fsize;
642 HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, data_idx + data_size, NULL);
643 char *map =
static_cast<char*
>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, data_idx + data_size));
651 resetLockingPermissions (file_name, file_lock);
652 PCL_ERROR (
"[pcl::PCDWriter::writeBinary] posix_fallocate errno: %d strerror: %s\n", errno, strerror (errno));
654 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during posix_fallocate ()!");
658 char *map =
static_cast<char*
> (::mmap (
nullptr, data_idx + data_size, PROT_WRITE, MAP_SHARED, fd, 0));
659 if (map == reinterpret_cast<char*> (-1))
662 resetLockingPermissions (file_name, file_lock);
663 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during mmap ()!");
669 memcpy (&map[0], oss.str ().c_str (), data_idx);
671 char *out = &map[0] + data_idx;
673 for (
const int &index : indices)
676 for (
const auto &field : fields)
678 memcpy (out, reinterpret_cast<const char*> (&cloud.points[index]) + field.offset, fields_sizes[nrj]);
679 out += fields_sizes[nrj++];
685 if (map_synchronization_)
686 msync (map, data_idx + data_size, MS_SYNC);
691 UnmapViewOfFile (map);
693 if (::munmap (map, (data_idx + data_size)) == -1)
696 resetLockingPermissions (file_name, file_lock);
697 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during munmap ()!");
703 CloseHandle(h_native_file);
708 resetLockingPermissions (file_name, file_lock);
713 template <
typename Po
intT>
int
716 const std::vector<int> &indices,
719 if (cloud.
points.empty () || indices.empty ())
721 throw pcl::IOException (
"[pcl::PCDWriter::writeASCII] Input point cloud has no data or empty indices given!");
727 throw pcl::IOException (
"[pcl::PCDWriter::writeASCII] Number of points different than width * height!");
732 fs.open (file_name.c_str ());
733 if (!fs.is_open () || fs.fail ())
735 throw pcl::IOException (
"[pcl::PCDWriter::writeASCII] Could not open file for writing!");
740 boost::interprocess::file_lock file_lock;
741 setLockingPermissions (file_name, file_lock);
743 fs.precision (precision);
744 fs.imbue (std::locale::classic ());
746 const auto fields = pcl::getFields<PointT> ();
749 fs << generateHeader<PointT> (cloud,
static_cast<int> (indices.size ())) <<
"DATA ascii\n";
751 std::ostringstream stream;
752 stream.precision (precision);
753 stream.imbue (std::locale::classic ());
756 for (
const int &index : indices)
758 for (std::size_t d = 0; d < fields.size (); ++d)
761 if (fields[d].name ==
"_")
764 int count = fields[d].count;
768 for (
int c = 0; c < count; ++c)
770 switch (fields[d].datatype)
775 memcpy (&value, reinterpret_cast<const char*> (&cloud.points[index]) + fields[d].offset + c * sizeof (std::int8_t),
sizeof (std::int8_t));
776 stream << boost::numeric_cast<std::int32_t>(value);
782 memcpy (&value, reinterpret_cast<const char*> (&cloud.points[index]) + fields[d].offset + c * sizeof (std::uint8_t),
sizeof (std::uint8_t));
783 stream << boost::numeric_cast<std::uint32_t>(value);
789 memcpy (&value, reinterpret_cast<const char*> (&cloud.points[index]) + fields[d].offset + c * sizeof (std::int16_t),
sizeof (std::int16_t));
790 stream << boost::numeric_cast<std::int16_t>(value);
796 memcpy (&value, reinterpret_cast<const char*> (&cloud.points[index]) + fields[d].offset + c * sizeof (std::uint16_t),
sizeof (std::uint16_t));
797 stream << boost::numeric_cast<std::uint16_t>(value);
803 memcpy (&value, reinterpret_cast<const char*> (&cloud.points[index]) + fields[d].offset + c * sizeof (std::int32_t),
sizeof (std::int32_t));
804 stream << boost::numeric_cast<std::int32_t>(value);
810 memcpy (&value, reinterpret_cast<const char*> (&cloud.points[index]) + fields[d].offset + c * sizeof (std::uint32_t),
sizeof (std::uint32_t));
811 stream << boost::numeric_cast<std::uint32_t>(value);
821 if (
"rgb" == fields[d].name)
824 memcpy (&value, reinterpret_cast<const char*> (&cloud.points[index]) + fields[d].offset + c * sizeof (
float),
sizeof (
float));
825 stream << boost::numeric_cast<std::uint32_t>(value);
830 memcpy (&value, reinterpret_cast<const char*> (&cloud.points[index]) + fields[d].offset + c * sizeof (
float),
sizeof (
float));
831 if (std::isnan (value))
834 stream << boost::numeric_cast<float>(value);
841 memcpy (&value, reinterpret_cast<const char*> (&cloud.points[index]) + fields[d].offset + c * sizeof (
double),
sizeof (
double));
842 if (std::isnan (value))
845 stream << boost::numeric_cast<double>(value);
849 PCL_WARN (
"[pcl::PCDWriter::writeASCII] Incorrect field data type specified (%d)!\n", fields[d].datatype);
853 if (d < fields.size () - 1 || c < static_cast<int> (fields[d].count - 1))
858 std::string result = stream.str ();
859 boost::trim (result);
861 fs << result <<
"\n";
865 resetLockingPermissions (file_name, file_lock);
870 #endif //#ifndef PCL_IO_PCD_IO_H_
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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.
int raw_open(const char *pathname, int flags, int mode)
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.
std::uint32_t width
The point cloud width (if organized as an image-structure).
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
std::uint32_t height
The point cloud height (if organized as an image-structure).
int getFieldSize(const int datatype)
Obtains the size of a specific field data type in bytes.
An exception that is thrown during an IO error (typical read/write errors)
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...
int raw_fallocate(int fd, long length)