40 #ifndef PCL_IO_PCD_IO_IMPL_H_
41 #define PCL_IO_PCD_IO_IMPL_H_
47 #include <pcl/io/boost.h>
48 #include <pcl/console/print.h>
49 #include <pcl/io/pcd_io.h>
53 # ifndef WIN32_LEAN_AND_MEAN
54 # define WIN32_LEAN_AND_MEAN
55 # endif // WIN32_LEAN_AND_MEAN
60 # define pcl_open _open
61 # define pcl_close(fd) _close(fd)
62 # define pcl_lseek(fd,offset,origin) _lseek(fd,offset,origin)
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)
70 #include <pcl/io/lzf.h>
73 template <
typename Po
intT> std::string
76 std::ostringstream oss;
77 oss.imbue (std::locale::classic ());
79 oss <<
"# .PCD v0.7 - Point Cloud Data file format"
83 std::vector<pcl::PCLPointField> fields;
86 std::stringstream field_names, field_types, field_sizes, field_counts;
87 for (
size_t i = 0; i < fields.size (); ++i)
89 if (fields[i].name ==
"_")
92 field_names <<
" " << fields[i].name;
95 int count = abs (static_cast<int> (fields[i].count));
96 if (count == 0) count = 1;
97 field_counts <<
" " << count;
99 oss << field_names.str ();
100 oss <<
"\nSIZE" << field_sizes.str ()
101 <<
"\nTYPE" << field_types.str ()
102 <<
"\nCOUNT" << field_counts.str ();
104 if (nr_points != std::numeric_limits<int>::max ())
105 oss <<
"\nWIDTH " << nr_points <<
"\nHEIGHT " << 1 <<
"\n";
107 oss <<
"\nWIDTH " << cloud.
width <<
"\nHEIGHT " << cloud.
height <<
"\n";
116 if (nr_points != std::numeric_limits<int>::max ())
117 oss <<
"POINTS " << nr_points <<
"\n";
119 oss <<
"POINTS " << cloud.
points.size () <<
"\n";
125 template <
typename Po
intT>
int
131 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Input point cloud has no data!");
135 std::ostringstream oss;
136 oss << generateHeader<PointT> (cloud) <<
"DATA binary\n";
138 data_idx =
static_cast<int> (oss.tellp ());
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)
144 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during CreateFile!");
148 int fd = pcl_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC,
static_cast<mode_t
> (0600));
151 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during open!");
156 boost::interprocess::file_lock file_lock;
157 setLockingPermissions (file_name, file_lock);
159 std::vector<pcl::PCLPointField> fields;
160 std::vector<int> fields_sizes;
162 size_t data_size = 0;
166 for (
size_t i = 0; i < fields.size (); ++i)
168 if (fields[i].name ==
"_")
171 int fs = fields[i].count *
getFieldSize (fields[i].datatype);
173 fields_sizes.push_back (fs);
174 fields[nri++] = fields[i];
178 data_size = cloud.
points.size () * fsize;
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));
188 off_t result = pcl_lseek (fd, getpagesize () + data_size - 1, SEEK_SET);
193 resetLockingPermissions (file_name, file_lock);
194 PCL_ERROR (
"[pcl::PCDWriter::writeBinary] lseek errno: %d strerror: %s\n", errno, strerror (errno));
196 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during lseek ()!");
200 result =
static_cast<int> (
::write (fd,
"", 1));
204 resetLockingPermissions (file_name, file_lock);
205 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during write ()!");
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))
213 resetLockingPermissions (file_name, file_lock);
214 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during mmap ()!");
220 memcpy (&map[0], oss.str ().c_str (), data_idx);
223 char *out = &map[0] + data_idx;
224 for (
size_t i = 0; i < cloud.
points.size (); ++i)
227 for (
size_t j = 0; j < fields.size (); ++j)
229 memcpy (out, reinterpret_cast<const char*> (&cloud.
points[i]) + fields[j].offset, fields_sizes[nrj]);
230 out += fields_sizes[nrj++];
236 if (map_synchronization_)
237 msync (map, data_idx + data_size, MS_SYNC);
242 UnmapViewOfFile (map);
244 if (munmap (map, (data_idx + data_size)) == -1)
247 resetLockingPermissions (file_name, file_lock);
248 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during munmap ()!");
254 CloseHandle (h_native_file);
258 resetLockingPermissions (file_name, file_lock);
263 template <
typename Po
intT>
int
267 if (cloud.
points.empty ())
269 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Input point cloud has no data!");
273 std::ostringstream oss;
274 oss << generateHeader<PointT> (cloud) <<
"DATA binary_compressed\n";
276 data_idx =
static_cast<int> (oss.tellp ());
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)
282 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during CreateFile!");
286 int fd = pcl_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC,
static_cast<mode_t
> (0600));
289 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during open!");
295 boost::interprocess::file_lock file_lock;
296 setLockingPermissions (file_name, file_lock);
298 std::vector<pcl::PCLPointField> fields;
300 size_t data_size = 0;
303 std::vector<int> fields_sizes (fields.size ());
305 for (
size_t i = 0; i < fields.size (); ++i)
307 if (fields[i].name ==
"_")
311 fsize += fields_sizes[nri];
312 fields[nri] = fields[i];
315 fields_sizes.resize (nri);
319 data_size = cloud.
points.size () * fsize;
326 char *only_valid_data =
static_cast<char*
> (malloc (data_size));
336 std::vector<char*> pters (fields.size ());
338 for (
size_t i = 0; i < pters.size (); ++i)
340 pters[i] = &only_valid_data[toff];
341 toff += fields_sizes[i] *
static_cast<int> (cloud.
points.size ());
345 for (
size_t i = 0; i < cloud.
points.size (); ++i)
347 for (
size_t j = 0; j < fields.size (); ++j)
349 memcpy (pters[j], reinterpret_cast<const char*> (&cloud.
points[i]) + fields[j].offset, fields_sizes[j]);
351 pters[j] += fields_sizes[j];
355 char* temp_buf =
static_cast<char*
> (malloc (static_cast<size_t> (static_cast<float> (data_size) * 1.5f + 8.0f)));
358 static_cast<uint32_t> (data_size),
360 static_cast<uint32_t> (static_cast<float>(data_size) * 1.5f));
361 unsigned int compressed_final_size = 0;
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;
376 resetLockingPermissions (file_name, file_lock);
377 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during compression!");
383 off_t result = pcl_lseek (fd, getpagesize () + data_size - 1, SEEK_SET);
387 resetLockingPermissions (file_name, file_lock);
388 PCL_ERROR (
"[pcl::PCDWriter::writeBinary] lseek errno: %d strerror: %s\n", errno, strerror (errno));
390 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during lseek ()!");
394 result =
static_cast<int> (
::write (fd,
"", 1));
398 resetLockingPermissions (file_name, file_lock);
399 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during write ()!");
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));
411 char *map =
static_cast<char*
> (mmap (0, compressed_final_size, PROT_WRITE, MAP_SHARED, fd, 0));
412 if (map == reinterpret_cast<char*> (-1))
415 resetLockingPermissions (file_name, file_lock);
416 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during mmap ()!");
422 memcpy (&map[0], oss.str ().c_str (), data_idx);
424 memcpy (&map[data_idx], temp_buf, data_size);
428 if (map_synchronization_)
429 msync (map, compressed_final_size, MS_SYNC);
434 UnmapViewOfFile (map);
436 if (munmap (map, (compressed_final_size)) == -1)
439 resetLockingPermissions (file_name, file_lock);
440 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during munmap ()!");
447 CloseHandle (h_native_file);
451 resetLockingPermissions (file_name, file_lock);
453 free (only_valid_data);
459 template <
typename Po
intT>
int
465 throw pcl::IOException (
"[pcl::PCDWriter::writeASCII] Input point cloud has no data!");
471 throw pcl::IOException (
"[pcl::PCDWriter::writeASCII] Number of points different than width * height!");
476 fs.open (file_name.c_str ());
478 if (!fs.is_open () || fs.fail ())
480 throw pcl::IOException (
"[pcl::PCDWriter::writeASCII] Could not open file for writing!");
485 boost::interprocess::file_lock file_lock;
486 setLockingPermissions (file_name, file_lock);
488 fs.precision (precision);
489 fs.imbue (std::locale::classic ());
491 std::vector<pcl::PCLPointField> fields;
495 fs << generateHeader<PointT> (cloud) <<
"DATA ascii\n";
497 std::ostringstream stream;
498 stream.precision (precision);
499 stream.imbue (std::locale::classic ());
501 for (
size_t i = 0; i < cloud.
points.size (); ++i)
503 for (
size_t d = 0; d < fields.size (); ++d)
506 if (fields[d].name ==
"_")
509 int count = fields[d].count;
513 for (
int c = 0; c < count; ++c)
515 switch (fields[d].datatype)
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))
524 stream << boost::numeric_cast<uint32_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))
534 stream << boost::numeric_cast<uint32_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))
544 stream << boost::numeric_cast<int16_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))
554 stream << boost::numeric_cast<uint16_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))
564 stream << boost::numeric_cast<int32_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))
574 stream << boost::numeric_cast<uint32_t>(value);
580 memcpy (&value, reinterpret_cast<const char*> (&cloud.
points[i]) + fields[d].offset + c * sizeof (
float),
sizeof (
float));
581 if (pcl_isnan (value))
584 stream << boost::numeric_cast<float>(value);
590 memcpy (&value, reinterpret_cast<const char*> (&cloud.
points[i]) + fields[d].offset + c * sizeof (
double),
sizeof (
double));
591 if (pcl_isnan (value))
594 stream << boost::numeric_cast<double>(value);
598 PCL_WARN (
"[pcl::PCDWriter::writeASCII] Incorrect field data type specified (%d)!\n", fields[d].datatype);
602 if (d < fields.size () - 1 || c < static_cast<int> (fields[d].count - 1))
607 std::string result = stream.str ();
608 boost::trim (result);
610 fs << result <<
"\n";
613 resetLockingPermissions (file_name, file_lock);
619 template <
typename Po
intT>
int
622 const std::vector<int> &indices)
624 if (cloud.
points.empty () || indices.empty ())
626 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Input point cloud has no data or empty indices given!");
630 std::ostringstream oss;
631 oss << generateHeader<PointT> (cloud,
static_cast<int> (indices.size ())) <<
"DATA binary\n";
633 data_idx =
static_cast<int> (oss.tellp ());
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)
639 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during CreateFile!");
643 int fd = pcl_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC,
static_cast<mode_t
> (0600));
646 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during open!");
651 boost::interprocess::file_lock file_lock;
652 setLockingPermissions (file_name, file_lock);
654 std::vector<pcl::PCLPointField> fields;
655 std::vector<int> fields_sizes;
657 size_t data_size = 0;
661 for (
size_t i = 0; i < fields.size (); ++i)
663 if (fields[i].name ==
"_")
666 int fs = fields[i].count *
getFieldSize (fields[i].datatype);
668 fields_sizes.push_back (fs);
669 fields[nri++] = fields[i];
673 data_size = indices.size () * fsize;
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));
683 off_t result = pcl_lseek (fd, getpagesize () + data_size - 1, SEEK_SET);
687 resetLockingPermissions (file_name, file_lock);
688 PCL_ERROR (
"[pcl::PCDWriter::writeBinary] lseek errno: %d strerror: %s\n", errno, strerror (errno));
690 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during lseek ()!");
694 result =
static_cast<int> (
::write (fd,
"", 1));
698 resetLockingPermissions (file_name, file_lock);
699 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during write ()!");
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))
707 resetLockingPermissions (file_name, file_lock);
708 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during mmap ()!");
714 memcpy (&map[0], oss.str ().c_str (), data_idx);
716 char *out = &map[0] + data_idx;
718 for (
size_t i = 0; i < indices.size (); ++i)
721 for (
size_t j = 0; j < fields.size (); ++j)
723 memcpy (out, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[j].offset, fields_sizes[nrj]);
724 out += fields_sizes[nrj++];
730 if (map_synchronization_)
731 msync (map, data_idx + data_size, MS_SYNC);
736 UnmapViewOfFile (map);
738 if (munmap (map, (data_idx + data_size)) == -1)
741 resetLockingPermissions (file_name, file_lock);
742 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during munmap ()!");
748 CloseHandle(h_native_file);
753 resetLockingPermissions (file_name, file_lock);
758 template <
typename Po
intT>
int
761 const std::vector<int> &indices,
764 if (cloud.
points.empty () || indices.empty ())
766 throw pcl::IOException (
"[pcl::PCDWriter::writeASCII] Input point cloud has no data or empty indices given!");
772 throw pcl::IOException (
"[pcl::PCDWriter::writeASCII] Number of points different than width * height!");
777 fs.open (file_name.c_str ());
778 if (!fs.is_open () || fs.fail ())
780 throw pcl::IOException (
"[pcl::PCDWriter::writeASCII] Could not open file for writing!");
785 boost::interprocess::file_lock file_lock;
786 setLockingPermissions (file_name, file_lock);
788 fs.precision (precision);
789 fs.imbue (std::locale::classic ());
791 std::vector<pcl::PCLPointField> fields;
795 fs << generateHeader<PointT> (cloud,
static_cast<int> (indices.size ())) <<
"DATA ascii\n";
797 std::ostringstream stream;
798 stream.precision (precision);
799 stream.imbue (std::locale::classic ());
802 for (
size_t i = 0; i < indices.size (); ++i)
804 for (
size_t d = 0; d < fields.size (); ++d)
807 if (fields[d].name ==
"_")
810 int count = fields[d].count;
814 for (
int c = 0; c < count; ++c)
816 switch (fields[d].datatype)
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))
825 stream << boost::numeric_cast<uint32_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))
835 stream << boost::numeric_cast<uint32_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))
845 stream << boost::numeric_cast<int16_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))
855 stream << boost::numeric_cast<uint16_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))
865 stream << boost::numeric_cast<int32_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))
875 stream << boost::numeric_cast<uint32_t>(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))
885 stream << boost::numeric_cast<float>(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))
895 stream << boost::numeric_cast<double>(value);
899 PCL_WARN (
"[pcl::PCDWriter::writeASCII] Incorrect field data type specified (%d)!\n", fields[d].datatype);
903 if (d < fields.size () - 1 || c < static_cast<int> (fields[d].count - 1))
908 std::string result = stream.str ();
909 boost::trim (result);
911 fs << result <<
"\n";
915 resetLockingPermissions (file_name, file_lock);
920 #endif //#ifndef PCL_IO_PCD_IO_H_
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 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 getFieldType(const int size, char type)
Obtains the type of the PCLPointField from a specific size and type.
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
uint32_t width
The point cloud width (if organized as an image-structure).
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.
void getFields(const pcl::PointCloud< PointT > &cloud, std::vector< pcl::PCLPointField > &fields)
Get the list of available fields (i.e., dimension/channel)
uint32_t height
The point cloud height (if organized as an image-structure).
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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...
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
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)
void write(std::ostream &stream, Type value)
Function for writing data to a stream.