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 std::vector<pcl::PCLPointField> fields;
68 std::stringstream field_names, field_types, field_sizes, field_counts;
69 for (
size_t i = 0; i < fields.size (); ++i)
71 if (fields[i].name ==
"_")
74 field_names <<
" " << fields[i].name;
76 if (
"rgb" == fields[i].name)
77 field_types <<
" " <<
"U";
80 int count = abs (static_cast<int> (fields[i].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.
points.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 std::vector<pcl::PCLPointField> fields;
145 std::vector<int> fields_sizes;
147 size_t data_size = 0;
151 for (
size_t i = 0; i < fields.size (); ++i)
153 if (fields[i].name ==
"_")
156 int fs = fields[i].count *
getFieldSize (fields[i].datatype);
158 fields_sizes.push_back (fs);
159 fields[nri++] = fields[i];
163 data_size = cloud.
points.size () * fsize;
167 HANDLE fm = CreateFileMappingA (h_native_file, NULL, PAGE_READWRITE, 0, (DWORD) (data_idx + data_size), NULL);
170 throw pcl::IOException(
"[pcl::PCDWriter::writeBinary] Error during memory map creation ()!");
173 char *map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, data_idx + data_size));
181 resetLockingPermissions (file_name, file_lock);
182 PCL_ERROR (
"[pcl::PCDWriter::writeBinary] posix_fallocate errno: %d strerror: %s\n", errno, strerror (errno));
184 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during posix_fallocate ()!");
188 char *map = static_cast<char*> (::mmap (0, data_idx + data_size, PROT_WRITE, MAP_SHARED, fd, 0));
189 if (map == reinterpret_cast<char*> (-1))
192 resetLockingPermissions (file_name, file_lock);
193 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during mmap ()!");
199 memcpy (&map[0], oss.str ().c_str (), data_idx);
202 char *out = &map[0] + data_idx;
203 for (
size_t i = 0; i < cloud.
points.size (); ++i)
206 for (
size_t j = 0; j < fields.size (); ++j)
208 memcpy (out, reinterpret_cast<const char*> (&cloud.
points[i]) + fields[j].offset, fields_sizes[nrj]);
209 out += fields_sizes[nrj++];
215 if (map_synchronization_)
216 msync (map, data_idx + data_size, MS_SYNC);
221 UnmapViewOfFile (map);
223 if (::munmap (map, (data_idx + data_size)) == -1)
226 resetLockingPermissions (file_name, file_lock);
227 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during munmap ()!");
233 CloseHandle (h_native_file);
237 resetLockingPermissions (file_name, file_lock);
242 template <
typename Po
intT>
int 246 if (cloud.
points.empty ())
248 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Input point cloud has no data!");
252 std::ostringstream oss;
253 oss << generateHeader<PointT> (cloud) <<
"DATA binary_compressed\n";
255 data_idx = static_cast<int> (oss.tellp ());
258 HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
259 if (h_native_file == INVALID_HANDLE_VALUE)
261 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during CreateFile!");
265 int fd =
io::raw_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
268 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during open!");
274 boost::interprocess::file_lock file_lock;
275 setLockingPermissions (file_name, file_lock);
277 std::vector<pcl::PCLPointField> fields;
279 size_t data_size = 0;
282 std::vector<int> fields_sizes (fields.size ());
284 for (
size_t i = 0; i < fields.size (); ++i)
286 if (fields[i].name ==
"_")
290 fsize += fields_sizes[nri];
291 fields[nri] = fields[i];
294 fields_sizes.resize (nri);
298 data_size = cloud.
points.size () * fsize;
302 if (data_size * 3 / 2 > std::numeric_limits<uint32_t>::max ())
304 PCL_ERROR (
"[pcl::PCDWriter::writeBinaryCompressed] The input data exceeds the maximum size for compressed version 0.7 pcds of %l bytes.\n",
305 static_cast<size_t> (std::numeric_limits<uint32_t>::max ()) * 2 / 3);
314 char *only_valid_data = static_cast<char*> (malloc (data_size));
324 std::vector<char*> pters (fields.size ());
326 for (
size_t i = 0; i < pters.size (); ++i)
328 pters[i] = &only_valid_data[toff];
329 toff += static_cast<size_t>(fields_sizes[i]) * cloud.
points.size();
333 for (
size_t i = 0; i < cloud.
points.size (); ++i)
335 for (
size_t j = 0; j < fields.size (); ++j)
337 memcpy (pters[j], reinterpret_cast<const char*> (&cloud.
points[i]) + fields[j].offset, fields_sizes[j]);
339 pters[j] += fields_sizes[j];
343 char* temp_buf = static_cast<char*> (malloc (static_cast<size_t> (static_cast<float> (data_size) * 1.5f + 8.0f)));
346 static_cast<uint32_t> (data_size),
348 static_cast<uint32_t> (static_cast<float>(data_size) * 1.5f));
349 unsigned int compressed_final_size = 0;
353 char *header = &temp_buf[0];
354 memcpy (&header[0], &compressed_size,
sizeof (
unsigned int));
355 memcpy (&header[4], &data_size,
sizeof (
unsigned int));
356 data_size = compressed_size + 8;
357 compressed_final_size = static_cast<uint32_t> (data_size) + data_idx;
364 resetLockingPermissions (file_name, file_lock);
365 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during compression!");
371 HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, compressed_final_size, NULL);
372 char *map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, compressed_final_size));
380 resetLockingPermissions (file_name, file_lock);
381 PCL_ERROR (
"[pcl::PCDWriter::writeBinaryCompressed] posix_fallocate errno: %d strerror: %s\n", errno, strerror (errno));
383 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during posix_fallocate ()!");
387 char *map = static_cast<char*> (::mmap (0, compressed_final_size, PROT_WRITE, MAP_SHARED, fd, 0));
388 if (map == reinterpret_cast<char*> (-1))
391 resetLockingPermissions (file_name, file_lock);
392 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during mmap ()!");
398 memcpy (&map[0], oss.str ().c_str (), data_idx);
400 memcpy (&map[data_idx], temp_buf, data_size);
404 if (map_synchronization_)
405 msync (map, compressed_final_size, MS_SYNC);
410 UnmapViewOfFile (map);
412 if (::munmap (map, (compressed_final_size)) == -1)
415 resetLockingPermissions (file_name, file_lock);
416 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during munmap ()!");
423 CloseHandle (h_native_file);
427 resetLockingPermissions (file_name, file_lock);
429 free (only_valid_data);
435 template <
typename Po
intT>
int 441 throw pcl::IOException (
"[pcl::PCDWriter::writeASCII] Input point cloud has no data!");
447 throw pcl::IOException (
"[pcl::PCDWriter::writeASCII] Number of points different than width * height!");
452 fs.open (file_name.c_str ());
454 if (!fs.is_open () || fs.fail ())
456 throw pcl::IOException (
"[pcl::PCDWriter::writeASCII] Could not open file for writing!");
461 boost::interprocess::file_lock file_lock;
462 setLockingPermissions (file_name, file_lock);
464 fs.precision (precision);
465 fs.imbue (std::locale::classic ());
467 std::vector<pcl::PCLPointField> fields;
471 fs << generateHeader<PointT> (cloud) <<
"DATA ascii\n";
473 std::ostringstream stream;
474 stream.precision (precision);
475 stream.imbue (std::locale::classic ());
477 for (
size_t i = 0; i < cloud.
points.size (); ++i)
479 for (
size_t d = 0; d < fields.size (); ++d)
482 if (fields[d].name ==
"_")
485 int count = fields[d].count;
489 for (
int c = 0; c < count; ++c)
491 switch (fields[d].datatype)
496 memcpy (&value, reinterpret_cast<const char*> (&cloud.
points[i]) + fields[d].offset + c * sizeof (int8_t),
sizeof (int8_t));
497 if (pcl_isnan (value))
500 stream << boost::numeric_cast<uint32_t>(value);
506 memcpy (&value, reinterpret_cast<const char*> (&cloud.
points[i]) + fields[d].offset + c * sizeof (uint8_t),
sizeof (uint8_t));
507 if (pcl_isnan (value))
510 stream << boost::numeric_cast<uint32_t>(value);
516 memcpy (&value, reinterpret_cast<const char*> (&cloud.
points[i]) + fields[d].offset + c * sizeof (int16_t),
sizeof (int16_t));
517 if (pcl_isnan (value))
520 stream << boost::numeric_cast<int16_t>(value);
526 memcpy (&value, reinterpret_cast<const char*> (&cloud.
points[i]) + fields[d].offset + c * sizeof (uint16_t),
sizeof (uint16_t));
527 if (pcl_isnan (value))
530 stream << boost::numeric_cast<uint16_t>(value);
536 memcpy (&value, reinterpret_cast<const char*> (&cloud.
points[i]) + fields[d].offset + c * sizeof (int32_t),
sizeof (int32_t));
537 if (pcl_isnan (value))
540 stream << boost::numeric_cast<int32_t>(value);
546 memcpy (&value, reinterpret_cast<const char*> (&cloud.
points[i]) + fields[d].offset + c * sizeof (uint32_t),
sizeof (uint32_t));
547 if (pcl_isnan (value))
550 stream << boost::numeric_cast<uint32_t>(value);
560 if (
"rgb" == fields[d].name)
563 memcpy (&value, reinterpret_cast<const char*> (&cloud.
points[i]) + fields[d].offset + c * sizeof (
float),
sizeof (
float));
564 if (pcl_isnan (value))
567 stream << boost::numeric_cast<uint32_t>(value);
573 memcpy (&value, reinterpret_cast<const char*> (&cloud.
points[i]) + fields[d].offset + c * sizeof (
float),
sizeof (
float));
574 if (pcl_isnan (value))
577 stream << boost::numeric_cast<float>(value);
584 memcpy (&value, reinterpret_cast<const char*> (&cloud.
points[i]) + fields[d].offset + c * sizeof (
double),
sizeof (
double));
585 if (pcl_isnan (value))
588 stream << boost::numeric_cast<double>(value);
592 PCL_WARN (
"[pcl::PCDWriter::writeASCII] Incorrect field data type specified (%d)!\n", fields[d].datatype);
596 if (d < fields.size () - 1 || c < static_cast<int> (fields[d].count - 1))
601 std::string result = stream.str ();
602 boost::trim (result);
604 fs << result <<
"\n";
607 resetLockingPermissions (file_name, file_lock);
613 template <
typename Po
intT>
int 616 const std::vector<int> &indices)
618 if (cloud.
points.empty () || indices.empty ())
620 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Input point cloud has no data or empty indices given!");
624 std::ostringstream oss;
625 oss << generateHeader<PointT> (cloud, static_cast<int> (indices.size ())) <<
"DATA binary\n";
627 data_idx = static_cast<int> (oss.tellp ());
630 HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
631 if (h_native_file == INVALID_HANDLE_VALUE)
633 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during CreateFile!");
637 int fd =
io::raw_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
640 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during open!");
645 boost::interprocess::file_lock file_lock;
646 setLockingPermissions (file_name, file_lock);
648 std::vector<pcl::PCLPointField> fields;
649 std::vector<int> fields_sizes;
651 size_t data_size = 0;
655 for (
size_t i = 0; i < fields.size (); ++i)
657 if (fields[i].name ==
"_")
660 int fs = fields[i].count *
getFieldSize (fields[i].datatype);
662 fields_sizes.push_back (fs);
663 fields[nri++] = fields[i];
667 data_size = indices.size () * fsize;
671 HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, data_idx + data_size, NULL);
672 char *map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, data_idx + data_size));
680 resetLockingPermissions (file_name, file_lock);
681 PCL_ERROR (
"[pcl::PCDWriter::writeBinary] posix_fallocate errno: %d strerror: %s\n", errno, strerror (errno));
683 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during posix_fallocate ()!");
687 char *map = static_cast<char*> (::mmap (0, data_idx + data_size, PROT_WRITE, MAP_SHARED, fd, 0));
688 if (map == reinterpret_cast<char*> (-1))
691 resetLockingPermissions (file_name, file_lock);
692 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during mmap ()!");
698 memcpy (&map[0], oss.str ().c_str (), data_idx);
700 char *out = &map[0] + data_idx;
702 for (
size_t i = 0; i < indices.size (); ++i)
705 for (
size_t j = 0; j < fields.size (); ++j)
707 memcpy (out, reinterpret_cast<const char*> (&cloud.
points[indices[i]]) + fields[j].offset, fields_sizes[nrj]);
708 out += fields_sizes[nrj++];
714 if (map_synchronization_)
715 msync (map, data_idx + data_size, MS_SYNC);
720 UnmapViewOfFile (map);
722 if (::munmap (map, (data_idx + data_size)) == -1)
725 resetLockingPermissions (file_name, file_lock);
726 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during munmap ()!");
732 CloseHandle(h_native_file);
737 resetLockingPermissions (file_name, file_lock);
742 template <
typename Po
intT>
int 745 const std::vector<int> &indices,
748 if (cloud.
points.empty () || indices.empty ())
750 throw pcl::IOException (
"[pcl::PCDWriter::writeASCII] Input point cloud has no data or empty indices given!");
756 throw pcl::IOException (
"[pcl::PCDWriter::writeASCII] Number of points different than width * height!");
761 fs.open (file_name.c_str ());
762 if (!fs.is_open () || fs.fail ())
764 throw pcl::IOException (
"[pcl::PCDWriter::writeASCII] Could not open file for writing!");
769 boost::interprocess::file_lock file_lock;
770 setLockingPermissions (file_name, file_lock);
772 fs.precision (precision);
773 fs.imbue (std::locale::classic ());
775 std::vector<pcl::PCLPointField> fields;
779 fs << generateHeader<PointT> (cloud, static_cast<int> (indices.size ())) <<
"DATA ascii\n";
781 std::ostringstream stream;
782 stream.precision (precision);
783 stream.imbue (std::locale::classic ());
786 for (
size_t i = 0; i < indices.size (); ++i)
788 for (
size_t d = 0; d < fields.size (); ++d)
791 if (fields[d].name ==
"_")
794 int count = fields[d].count;
798 for (
int c = 0; c < count; ++c)
800 switch (fields[d].datatype)
805 memcpy (&value, reinterpret_cast<const char*> (&cloud.
points[indices[i]]) + fields[d].offset + c * sizeof (int8_t),
sizeof (int8_t));
806 if (pcl_isnan (value))
809 stream << boost::numeric_cast<uint32_t>(value);
815 memcpy (&value, reinterpret_cast<const char*> (&cloud.
points[indices[i]]) + fields[d].offset + c * sizeof (uint8_t),
sizeof (uint8_t));
816 if (pcl_isnan (value))
819 stream << boost::numeric_cast<uint32_t>(value);
825 memcpy (&value, reinterpret_cast<const char*> (&cloud.
points[indices[i]]) + fields[d].offset + c * sizeof (int16_t),
sizeof (int16_t));
826 if (pcl_isnan (value))
829 stream << boost::numeric_cast<int16_t>(value);
835 memcpy (&value, reinterpret_cast<const char*> (&cloud.
points[indices[i]]) + fields[d].offset + c * sizeof (uint16_t),
sizeof (uint16_t));
836 if (pcl_isnan (value))
839 stream << boost::numeric_cast<uint16_t>(value);
845 memcpy (&value, reinterpret_cast<const char*> (&cloud.
points[indices[i]]) + fields[d].offset + c * sizeof (int32_t),
sizeof (int32_t));
846 if (pcl_isnan (value))
849 stream << boost::numeric_cast<int32_t>(value);
855 memcpy (&value, reinterpret_cast<const char*> (&cloud.
points[indices[i]]) + fields[d].offset + c * sizeof (uint32_t),
sizeof (uint32_t));
856 if (pcl_isnan (value))
859 stream << boost::numeric_cast<uint32_t>(value);
869 if (
"rgb" == fields[d].name)
872 memcpy (&value, reinterpret_cast<const char*> (&cloud.
points[indices[i]]) + fields[d].offset + c * sizeof (
float),
sizeof (
float));
873 if (pcl_isnan (value))
876 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);
892 memcpy (&value, reinterpret_cast<const char*> (&cloud.
points[indices[i]]) + fields[d].offset + c * sizeof (
double),
sizeof (
double));
893 if (pcl_isnan (value))
896 stream << boost::numeric_cast<double>(value);
900 PCL_WARN (
"[pcl::PCDWriter::writeASCII] Incorrect field data type specified (%d)!\n", fields[d].datatype);
904 if (d < fields.size () - 1 || c < static_cast<int> (fields[d].count - 1))
909 std::string result = stream.str ();
910 boost::trim (result);
912 fs << result <<
"\n";
916 resetLockingPermissions (file_name, file_lock);
921 #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.
uint32_t height
The point cloud height (if organized as an image-structure).
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
int raw_fallocate(int fd, off_t length)
uint32_t width
The point cloud width (if organized as an image-structure).
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)
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...
void getFields(const pcl::PointCloud< PointT > &cloud, std::vector< pcl::PCLPointField > &fields)
Get the list of available fields (i.e., dimension/channel)