40#ifndef PCL_IO_PCD_IO_IMPL_H_
41#define PCL_IO_PCD_IO_IMPL_H_
43#include <boost/algorithm/string/trim.hpp>
48#include <pcl/common/io.h>
49#include <pcl/console/print.h>
50#include <pcl/io/low_level_io.h>
51#include <pcl/io/pcd_io.h>
53#include <pcl/io/lzf.h>
56template <
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"
69 for (
const auto &
field : fields)
71 if (
field.name ==
"_")
76 if (
"rgb" ==
field.name)
80 int count = std::abs (
static_cast<int> (
field.count));
81 if (count == 0) count = 1;
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";
94 oss <<
"VIEWPOINT " << cloud.sensor_origin_[0] <<
" " << cloud.sensor_origin_[1] <<
" " << cloud.sensor_origin_[2] <<
" " <<
95 cloud.sensor_orientation_.w () <<
" " <<
96 cloud.sensor_orientation_.x () <<
" " <<
97 cloud.sensor_orientation_.y () <<
" " <<
98 cloud.sensor_orientation_.z () <<
"\n";
101 if (nr_points != std::numeric_limits<int>::max ())
102 oss <<
"POINTS " << nr_points <<
"\n";
104 oss <<
"POINTS " << cloud.
size () <<
"\n";
110template <
typename Po
intT>
int
116 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Input point cloud has no data!");
120 std::ostringstream
oss;
129 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during CreateFile!");
136 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during open!");
141 boost::interprocess::file_lock
file_lock;
142 setLockingPermissions (file_name,
file_lock);
146 std::size_t
fsize = 0;
150 for (
const auto &
field : fields)
152 if (
field.name ==
"_")
169 throw pcl::IOException(
"[pcl::PCDWriter::writeBinary] Error during memory map creation ()!");
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 ()!");
188 if (map ==
reinterpret_cast<char*
> (-1))
191 resetLockingPermissions (file_name,
file_lock);
192 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during mmap ()!");
202 for (
const auto& point: cloud)
205 for (
const auto &
field : fields)
214 if (map_synchronization_)
225 resetLockingPermissions (file_name,
file_lock);
226 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during munmap ()!");
236 resetLockingPermissions (file_name,
file_lock);
241template <
typename Po
intT>
int
247 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Input point cloud has no data!");
251 std::ostringstream
oss;
260 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during CreateFile!");
267 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during open!");
273 boost::interprocess::file_lock
file_lock;
274 setLockingPermissions (file_name,
file_lock);
277 std::size_t
fsize = 0;
282 for (
const auto &
field : fields)
284 if (
field.name ==
"_")
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);
322 std::vector<char*>
pters (fields.size ());
323 std::size_t
toff = 0;
324 for (std::size_t i = 0; i <
pters.
size (); ++i)
331 for (
const auto& point: cloud)
333 for (std::size_t j = 0; j < fields.size (); ++j)
341 char*
temp_buf =
static_cast<char*
> (
malloc (
static_cast<std::size_t
> (
static_cast<float> (
data_size) * 1.5f + 8.0f)));
346 static_cast<std::uint32_t
> (
static_cast<float>(
data_size) * 1.5f));
362 resetLockingPermissions (file_name,
file_lock);
363 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during compression!");
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 ()!");
386 if (map ==
reinterpret_cast<char*
> (-1))
389 resetLockingPermissions (file_name,
file_lock);
390 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during mmap ()!");
402 if (map_synchronization_)
413 resetLockingPermissions (file_name,
file_lock);
414 throw pcl::IOException (
"[pcl::PCDWriter::writeBinaryCompressed] Error during munmap ()!");
425 resetLockingPermissions (file_name,
file_lock);
433template <
typename Po
intT>
int
439 throw pcl::IOException (
"[pcl::PCDWriter::writeASCII] Input point cloud has no data!");
443 if (cloud.width * cloud.height != cloud.
size ())
445 throw pcl::IOException (
"[pcl::PCDWriter::writeASCII] Number of points different than width * height!");
450 fs.open (file_name.c_str (), std::ios::binary);
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);
463 fs.imbue (std::locale::classic ());
470 std::ostringstream
stream;
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));
500 memcpy (&value,
reinterpret_cast<const char*
> (&point) + fields[d].offset +
c *
sizeof (std::uint8_t),
sizeof (std::uint8_t));
507 memcpy (&value,
reinterpret_cast<const char*
> (&point) + fields[d].offset +
c *
sizeof (std::int16_t),
sizeof (std::int16_t));
514 memcpy (&value,
reinterpret_cast<const char*
> (&point) + fields[d].offset +
c *
sizeof (std::uint16_t),
sizeof (std::uint16_t));
521 memcpy (&value,
reinterpret_cast<const char*
> (&point) + fields[d].offset +
c *
sizeof (std::int32_t),
sizeof (std::int32_t));
528 memcpy (&value,
reinterpret_cast<const char*
> (&point) + fields[d].offset +
c *
sizeof (std::uint32_t),
sizeof (std::uint32_t));
539 if (
"rgb" == fields[d].name)
542 memcpy (&value,
reinterpret_cast<const char*
> (&point) + fields[d].offset +
c *
sizeof (
float),
sizeof (
float));
547 memcpy (&value,
reinterpret_cast<const char*
> (&point) + fields[d].offset +
c *
sizeof (
float),
sizeof (
float));
548 if (std::isnan (value))
557 memcpy (&value,
reinterpret_cast<const char*
> (&point) + fields[d].offset +
c *
sizeof (
double),
sizeof (
double));
558 if (std::isnan (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))
580 resetLockingPermissions (file_name,
file_lock);
586template <
typename Po
intT>
int
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;
606 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during CreateFile!");
613 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during open!");
618 boost::interprocess::file_lock
file_lock;
619 setLockingPermissions (file_name,
file_lock);
623 std::size_t
fsize = 0;
627 for (
const auto &
field : fields)
629 if (
field.name ==
"_")
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 ()!");
660 if (map ==
reinterpret_cast<char*
> (-1))
663 resetLockingPermissions (file_name,
file_lock);
664 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during mmap ()!");
674 for (
const auto &index : indices)
677 for (
const auto &
field : fields)
686 if (map_synchronization_)
697 resetLockingPermissions (file_name,
file_lock);
698 throw pcl::IOException (
"[pcl::PCDWriter::writeBinary] Error during munmap ()!");
709 resetLockingPermissions (file_name,
file_lock);
714template <
typename Po
intT>
int
720 if (cloud.empty () || indices.empty ())
722 throw pcl::IOException (
"[pcl::PCDWriter::writeASCII] Input point cloud has no data or empty indices given!");
726 if (cloud.width * cloud.height != cloud.
size ())
728 throw pcl::IOException (
"[pcl::PCDWriter::writeASCII] Number of points different than width * height!");
733 fs.open (file_name.c_str (), std::ios::binary);
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);
745 fs.imbue (std::locale::classic ());
752 std::ostringstream
stream;
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));
783 memcpy (&value,
reinterpret_cast<const char*
> (&cloud[index]) + fields[d].offset +
c *
sizeof (std::uint8_t),
sizeof (std::uint8_t));
790 memcpy (&value,
reinterpret_cast<const char*
> (&cloud[index]) + fields[d].offset +
c *
sizeof (std::int16_t),
sizeof (std::int16_t));
797 memcpy (&value,
reinterpret_cast<const char*
> (&cloud[index]) + fields[d].offset +
c *
sizeof (std::uint16_t),
sizeof (std::uint16_t));
804 memcpy (&value,
reinterpret_cast<const char*
> (&cloud[index]) + fields[d].offset +
c *
sizeof (std::int32_t),
sizeof (std::int32_t));
811 memcpy (&value,
reinterpret_cast<const char*
> (&cloud[index]) + fields[d].offset +
c *
sizeof (std::uint32_t),
sizeof (std::uint32_t));
822 if (
"rgb" == fields[d].name)
825 memcpy (&value,
reinterpret_cast<const char*
> (&cloud[index]) + fields[d].offset +
c *
sizeof (
float),
sizeof (
float));
831 memcpy (&value,
reinterpret_cast<const char*
> (&cloud[index]) + fields[d].offset +
c *
sizeof (
float),
sizeof (
float));
832 if (std::isnan (value))
842 memcpy (&value,
reinterpret_cast<const char*
> (&cloud[index]) + fields[d].offset +
c *
sizeof (
double),
sizeof (
double));
843 if (std::isnan (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))
866 resetLockingPermissions (file_name,
file_lock);
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
An exception that is thrown during an IO error (typical read/write errors)
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 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.
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 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.
int getFieldSize(const int datatype)
Obtains the size of a specific field data type in bytes.
int getFieldType(const int size, char type)
Obtains the type of the PCLPointField from a specific size and type.
int raw_fallocate(int fd, long length)
int raw_open(const char *pathname, int flags, int mode)
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...
IndicesAllocator<> Indices
Type used for indices in PCL.