template <typename PointT> int
pcl::PCDWriter::writeASCII (const std::string &file_name,
const pcl::PointCloud<PointT> &cloud,
const std::vector<int> &indices,
const int precision)
{
if (cloud.points.empty () || indices.empty ())
{
throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Input point cloud has no data or empty indices given!");
return (-1);
}
if (cloud.width * cloud.height != cloud.points.size ())
{
throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Number of points different than width * height!");
return (-1);
}
std::ofstream fs;
fs.open (file_name.c_str ()); // Open file
if (!fs.is_open () || fs.fail ())
{
throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Could not open file for writing!");
return (-1);
}
// Mandatory lock file
boost::interprocess::file_lock file_lock;
setLockingPermissions (file_name, file_lock);
fs.precision (precision);
fs.imbue (std::locale::classic ());
std::vector<pcl::PCLPointField> fields;
pcl::getFields (cloud, fields);
// Write the header information
fs << generateHeader<PointT> (cloud, static_cast<int> (indices.size ())) << "DATA ascii\n";
std::ostringstream stream;
stream.precision (precision);
stream.imbue (std::locale::classic ());
// Iterate through the points
for (size_t i = 0; i < indices.size (); ++i)
{
for (size_t d = 0; d < fields.size (); ++d)
{
// Ignore invalid padded dimensions that are inherited from binary data
if (fields[d].name == "_")
continue;
int count = fields[d].count;
if (count == 0)
count = 1; // we simply cannot tolerate 0 counts (coming from older converter code)
for (int c = 0; c < count; ++c)
{
switch (fields[d].datatype)
{
case pcl::PCLPointField::INT8:
{
int8_t value;
memcpy (&value, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[d].offset + c * sizeof (int8_t), sizeof (int8_t));
if (pcl_isnan (value))
stream << "nan";
else
stream << boost::numeric_cast<uint32_t>(value);
break;
}
case pcl::PCLPointField::UINT8:
{
uint8_t value;
memcpy (&value, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[d].offset + c * sizeof (uint8_t), sizeof (uint8_t));
if (pcl_isnan (value))
stream << "nan";
else
stream << boost::numeric_cast<uint32_t>(value);
break;
}
case pcl::PCLPointField::INT16:
{
int16_t value;
memcpy (&value, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[d].offset + c * sizeof (int16_t), sizeof (int16_t));
if (pcl_isnan (value))
stream << "nan";
else
stream << boost::numeric_cast<int16_t>(value);
break;
}
case pcl::PCLPointField::UINT16:
{
uint16_t value;
memcpy (&value, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[d].offset + c * sizeof (uint16_t), sizeof (uint16_t));
if (pcl_isnan (value))
stream << "nan";
else
stream << boost::numeric_cast<uint16_t>(value);
break;
}
case pcl::PCLPointField::INT32:
{
int32_t value;
memcpy (&value, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[d].offset + c * sizeof (int32_t), sizeof (int32_t));
if (pcl_isnan (value))
stream << "nan";
else
stream << boost::numeric_cast<int32_t>(value);
break;
}
case pcl::PCLPointField::UINT32:
{
uint32_t value;
memcpy (&value, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[d].offset + c * sizeof (uint32_t), sizeof (uint32_t));
if (pcl_isnan (value))
stream << "nan";
else
stream << boost::numeric_cast<uint32_t>(value);
break;
}
case pcl::PCLPointField::FLOAT32:
{
/*
* Despite the float type, store the rgb field as uint32
* because several fully opaque color values are mapped to
* nan.
*/
if ("rgb" == fields[d].name)
{
uint32_t value;
memcpy (&value, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[d].offset + c * sizeof (float), sizeof (float));
if (pcl_isnan (value))
stream << "nan";
else
stream << boost::numeric_cast<uint32_t>(value);
}
else
{
float value;
memcpy (&value, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[d].offset + c * sizeof (float), sizeof (float));
if (pcl_isnan (value))
stream << "nan";
else
stream << boost::numeric_cast<float>(value);
}
break;
}
case pcl::PCLPointField::FLOAT64:
{
double value;
memcpy (&value, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[d].offset + c * sizeof (double), sizeof (double));
if (pcl_isnan (value))
stream << "nan";
else
stream << boost::numeric_cast<double>(value);
break;
}
default:
PCL_WARN ("[pcl::PCDWriter::writeASCII] Incorrect field data type specified (%d)!\n", fields[d].datatype);
break;
}
if (d < fields.size () - 1 || c < static_cast<int> (fields[d].count - 1))
stream << " ";
}
}
// Copy the stream, trim it, and write it to disk
std::string result = stream.str ();
boost::trim (result);
stream.str ("");
fs << result << "\n";
}
fs.close (); // Close file
resetLockingPermissions (file_name, file_lock);
return (0);
}
template <typename PointT> int
pcl::PCDWriter::writeBinaryCompressed (const std::string &file_name,
const pcl::PointCloud<PointT> &cloud)
{
if (cloud.points.empty ())
{
throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Input point cloud has no data!");
return (-1);
}
int data_idx = 0;
std::ostringstream oss;
oss << generateHeader<PointT> (cloud) << "DATA binary_compressed\n";
oss.flush ();
data_idx = static_cast<int> (oss.tellp ());
#if _WIN32
HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
if (h_native_file == INVALID_HANDLE_VALUE)
{
throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during CreateFile!");
return (-1);
}
#else
int fd = pcl_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
if (fd < 0)
{
throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during open!");
return (-1);
}
#endif
// Mandatory lock file
boost::interprocess::file_lock file_lock;
setLockingPermissions (file_name, file_lock);
std::vector<pcl::PCLPointField> fields;
size_t fsize = 0;
size_t data_size = 0;
size_t nri = 0;
pcl::getFields (cloud, fields);
std::vector<int> fields_sizes (fields.size ());
// Compute the total size of the fields
for (size_t i = 0; i < fields.size (); ++i)
{
if (fields[i].name == "_")
continue;
fields_sizes[nri] = fields[i].count * pcl::getFieldSize (fields[i].datatype);
fsize += fields_sizes[nri];
fields[nri] = fields[i];
++nri;
}
fields_sizes.resize (nri);
fields.resize (nri);
// Compute the size of data
data_size = cloud.points.size () * fsize;
//////////////////////////////////////////////////////////////////////
// Empty array holding only the valid data
// data_size = nr_points * point_size
// = nr_points * (sizeof_field_1 + sizeof_field_2 + ... sizeof_field_n)
// = sizeof_field_1 * nr_points + sizeof_field_2 * nr_points + ... sizeof_field_n * nr_points
char *only_valid_data = static_cast<char*> (malloc (data_size));
// Convert the XYZRGBXYZRGB structure to XXYYZZRGBRGB to aid compression. For
// this, we need a vector of fields.size () (4 in this case), which points to
// each individual plane:
// pters[0] = &only_valid_data[offset_of_plane_x];
// pters[1] = &only_valid_data[offset_of_plane_y];
// pters[2] = &only_valid_data[offset_of_plane_z];
// pters[3] = &only_valid_data[offset_of_plane_RGB];
//
std::vector<char*> pters (fields.size ());
int toff = 0;
for (size_t i = 0; i < pters.size (); ++i)
{
pters[i] = &only_valid_data[toff];
toff += fields_sizes[i] * static_cast<int> (cloud.points.size ());
}
// Go over all the points, and copy the data in the appropriate places
for (size_t i = 0; i < cloud.points.size (); ++i)
{
for (size_t j = 0; j < fields.size (); ++j)
{
memcpy (pters[j], reinterpret_cast<const char*> (&cloud.points[i]) + fields[j].offset, fields_sizes[j]);
// Increment the pointer
pters[j] += fields_sizes[j];
}
}
char* temp_buf = static_cast<char*> (malloc (static_cast<size_t> (static_cast<float> (data_size) * 1.5f + 8.0f)));
// Compress the valid data
unsigned int compressed_size = pcl::lzfCompress (only_valid_data,
static_cast<uint32_t> (data_size),
&temp_buf[8],
static_cast<uint32_t> (static_cast<float>(data_size) * 1.5f));
unsigned int compressed_final_size = 0;
// Was the compression successful?
if (compressed_size)
{
char *header = &temp_buf[0];
memcpy (&header[0], &compressed_size, sizeof (unsigned int));
memcpy (&header[4], &data_size, sizeof (unsigned int));
data_size = compressed_size + 8;
compressed_final_size = static_cast<uint32_t> (data_size) + data_idx;
}
else
{
#if !_WIN32
pcl_close (fd);
#endif
resetLockingPermissions (file_name, file_lock);
throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during compression!");
return (-1);
}
#if !_WIN32
// Stretch the file size to the size of the data
off_t result = pcl_lseek (fd, getpagesize () + data_size - 1, SEEK_SET);
if (result < 0)
{
pcl_close (fd);
resetLockingPermissions (file_name, file_lock);
PCL_ERROR ("[pcl::PCDWriter::writeBinary] lseek errno: %d strerror: %s\n", errno, strerror (errno));
throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during lseek ()!");
return (-1);
}
// Write a bogus entry so that the new file size comes in effect
result = static_cast<int> (::write (fd, "", 1));
if (result != 1)
{
pcl_close (fd);
resetLockingPermissions (file_name, file_lock);
throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during write ()!");
return (-1);
}
#endif
// Prepare the map
#if _WIN32
HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, compressed_final_size, NULL);
char *map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, compressed_final_size));
CloseHandle (fm);
#else
char *map = static_cast<char*> (mmap (0, compressed_final_size, PROT_WRITE, MAP_SHARED, fd, 0));
if (map == reinterpret_cast<char*> (-1)) //MAP_FAILED)
{
pcl_close (fd);
resetLockingPermissions (file_name, file_lock);
throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during mmap ()!");
return (-1);
}
#endif
// Copy the header
memcpy (&map[0], oss.str ().c_str (), data_idx);
// Copy the compressed data
memcpy (&map[data_idx], temp_buf, data_size);
#if !_WIN32
// If the user set the synchronization flag on, call msync
if (map_synchronization_)
msync (map, compressed_final_size, MS_SYNC);
#endif
// Unmap the pages of memory
#if _WIN32
UnmapViewOfFile (map);
#else
if (munmap (map, (compressed_final_size)) == -1)
{
pcl_close (fd);
resetLockingPermissions (file_name, file_lock);
throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during munmap ()!");
return (-1);
}
#endif
// Close file
#if _WIN32
CloseHandle (h_native_file);
#else
pcl_close (fd);
#endif
resetLockingPermissions (file_name, file_lock);
free (only_valid_data);
free (temp_buf);
return (0);
}
template <typename PointT> int
pcl::PCDWriter::writeBinary (const std::string &file_name,
const pcl::PointCloud<PointT> &cloud,
const std::vector<int> &indices)
{
if (cloud.points.empty () || indices.empty ())
{
throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Input point cloud has no data or empty indices given!");
return (-1);
}
int data_idx = 0;
std::ostringstream oss;
oss << generateHeader<PointT> (cloud, static_cast<int> (indices.size ())) << "DATA binary\n";
oss.flush ();
data_idx = static_cast<int> (oss.tellp ());
#if _WIN32
HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
if (h_native_file == INVALID_HANDLE_VALUE)
{
throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during CreateFile!");
return (-1);
}
#else
int fd = pcl_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
if (fd < 0)
{
throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during open!");
return (-1);
}
#endif
// Mandatory lock file
boost::interprocess::file_lock file_lock;
setLockingPermissions (file_name, file_lock);
std::vector<pcl::PCLPointField> fields;
std::vector<int> fields_sizes;
size_t fsize = 0;
size_t data_size = 0;
size_t nri = 0;
pcl::getFields (cloud, fields);
// Compute the total size of the fields
for (size_t i = 0; i < fields.size (); ++i)
{
if (fields[i].name == "_")
continue;
int fs = fields[i].count * getFieldSize (fields[i].datatype);
fsize += fs;
fields_sizes.push_back (fs);
fields[nri++] = fields[i];
}
fields.resize (nri);
data_size = indices.size () * fsize;
// Prepare the map
#if _WIN32
HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, data_idx + data_size, NULL);
char *map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, data_idx + data_size));
CloseHandle (fm);
#else
// Stretch the file size to the size of the data
off_t result = pcl_lseek (fd, getpagesize () + data_size - 1, SEEK_SET);
if (result < 0)
{
pcl_close (fd);
resetLockingPermissions (file_name, file_lock);
PCL_ERROR ("[pcl::PCDWriter::writeBinary] lseek errno: %d strerror: %s\n", errno, strerror (errno));
throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during lseek ()!");
return (-1);
}
// Write a bogus entry so that the new file size comes in effect
result = static_cast<int> (::write (fd, "", 1));
if (result != 1)
{
pcl_close (fd);
resetLockingPermissions (file_name, file_lock);
throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during write ()!");
return (-1);
}
char *map = static_cast<char*> (mmap (0, data_idx + data_size, PROT_WRITE, MAP_SHARED, fd, 0));
if (map == reinterpret_cast<char*> (-1)) //MAP_FAILED)
{
pcl_close (fd);
resetLockingPermissions (file_name, file_lock);
throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during mmap ()!");
return (-1);
}
#endif
// Copy the header
memcpy (&map[0], oss.str ().c_str (), data_idx);
char *out = &map[0] + data_idx;
// Copy the data
for (size_t i = 0; i < indices.size (); ++i)
{
int nrj = 0;
for (size_t j = 0; j < fields.size (); ++j)
{
memcpy (out, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[j].offset, fields_sizes[nrj]);
out += fields_sizes[nrj++];
}
}
#if !_WIN32
// If the user set the synchronization flag on, call msync
if (map_synchronization_)
msync (map, data_idx + data_size, MS_SYNC);
#endif
// Unmap the pages of memory
#if _WIN32
UnmapViewOfFile (map);
#else
if (munmap (map, (data_idx + data_size)) == -1)
{
pcl_close (fd);
resetLockingPermissions (file_name, file_lock);
throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during munmap ()!");
return (-1);
}
#endif
// Close file
#if _WIN32
CloseHandle(h_native_file);
#else
pcl_close (fd);
#endif
resetLockingPermissions (file_name, file_lock);
return (0);
}
template<typename PointT> int
pcl::PCDWriter::appendBinary(const std::string &file_name,
const pcl::PointCloud<PointT> &cloud)
{
if(cloud.empty())
{
throw pcl::IOException("[pcl::PCDWriter::appendBinary] Input point cloud has no data!");
return -1;
}
if(!boost::filesystem::exists(file_name))
return writeBinary(file_name, cloud);
std::ifstream file_istream;
file_istream.open(file_name.c_str(), std::ifstream::binary);
if(!file_istream.good())
{
throw pcl::IOException("[pcl::PCDWriter::appendBinary] Error opening file for reading");
return -1;
}
file_istream.seekg(0, std::ios_base::end);
size_t file_size = file_istream.tellg();
file_istream.close();
pcl::PCLPointCloud2 tmp_cloud;
PCDReader reader;
if(reader.readHeader(file_name, tmp_cloud) != 0)
{
throw pcl::IOException("[pcl::PCDWriter::appendBinary] Failed reading header");
return -1;
}
if(tmp_cloud.height != 1 || cloud.height != 1)
{
throw pcl::IOException("[pcl::PCDWriter::appendBinary] can't use appendBinary with a point cloud that "
"has height different than 1!");
return -1;
}
tmp_cloud.width += cloud.width;
std::ostringstream oss;
pcl::PointCloud<PointT> tmp_cloud2;
// copy the header values:
tmp_cloud2.header = tmp_cloud.header;
tmp_cloud2.width = tmp_cloud.width;
tmp_cloud2.height = tmp_cloud.height;
tmp_cloud2.is_dense = tmp_cloud.is_dense;
oss << PCDWriter::generateHeader(tmp_cloud2, tmp_cloud2.width) << "DATA binary\n";
size_t data_idx = oss.tellp();
#if _WIN32
HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL);
if (h_native_file == INVALID_HANDLE_VALUE)
{
throw pcl::IOException ("[pcl::PCDWriter::appendBinary] Error during CreateFile!");
return (-1);
}
#else
int fd = pcl_open (file_name.c_str (), O_RDWR | O_CREAT | O_APPEND, static_cast<mode_t> (0600));
if (fd < 0)
{
throw pcl::IOException ("[pcl::PCDWriter::appendBinary] Error during open!");
return (-1);
}
#endif
// Mandatory lock file
boost::interprocess::file_lock file_lock;
setLockingPermissions (file_name, file_lock);
std::vector<pcl::PCLPointField> fields;
std::vector<int> fields_sizes;
size_t fsize = 0;
size_t data_size = 0;
size_t nri = 0;
pcl::getFields (cloud, fields);
// Compute the total size of the fields
for (size_t i = 0; i < fields.size (); ++i)
{
if (fields[i].name == "_")
continue;
int fs = fields[i].count * getFieldSize (fields[i].datatype);
fsize += fs;
fields_sizes.push_back (fs);
fields[nri++] = fields[i];
}
fields.resize (nri);
data_size = cloud.points.size () * fsize;
data_idx += (tmp_cloud.width - cloud.width) * fsize;
if (data_idx != file_size)
{
const char *msg = "[pcl::PCDWriter::appendBinary] The expected data size and the current data size are different!";
PCL_WARN(msg);
throw pcl::IOException (msg);
return -1;
}
// Prepare the map
#if _WIN32
HANDLE fm = CreateFileMappingA (h_native_file, NULL, PAGE_READWRITE, 0, (DWORD) (data_idx + data_size), NULL);
char *map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, data_idx + data_size));
CloseHandle (fm);
#else
// Stretch the file size to the size of the data
off_t result = pcl_lseek (fd, getpagesize () + data_size - 1, SEEK_SET);
if (result < 0)
{
pcl_close (fd);
resetLockingPermissions (file_name, file_lock);
PCL_ERROR ("[pcl::PCDWriter::appendBinary] lseek errno: %d strerror: %s\n", errno, strerror (errno));
throw pcl::IOException ("[pcl::PCDWriter::appendBinary] Error during lseek ()!");
return (-1);
}
// Write a bogus entry so that the new file size comes in effect
result = static_cast<int> (::write (fd, "", 1));
if (result != 1)
{
pcl_close (fd);
resetLockingPermissions (file_name, file_lock);
throw pcl::IOException ("[pcl::PCDWriter::appendBinary] Error during write ()!");
return (-1);
}
char *map = static_cast<char*> (mmap (0, data_idx + data_size, PROT_WRITE, MAP_SHARED, fd, 0));
if (map == reinterpret_cast<char*> (-1)) //MAP_FAILED)
{
pcl_close (fd);
resetLockingPermissions (file_name, file_lock);
throw pcl::IOException ("[pcl::PCDWriter::appendBinary] Error during mmap ()!");
return (-1);
}
#endif
char* out = &map[0] + data_idx;
// Copy the data
for (size_t i = 0; i < cloud.points.size (); ++i)
{
int nrj = 0;
for (size_t j = 0; j < fields.size (); ++j)
{
memcpy (out, reinterpret_cast<const char*> (&cloud.points[i]) + fields[j].offset, fields_sizes[nrj]);
out += fields_sizes[nrj++];
}
}
// write the new header:
std::string header(oss.str());
memcpy(map, header.c_str(), header.size());
// If the user set the synchronization flag on, call msync
#if !_WIN32
if (map_synchronization_)
msync (map, data_idx + data_size, MS_SYNC);
#endif
// Unmap the pages of memory
#if _WIN32
UnmapViewOfFile (map);
#else
if (munmap (map, (data_idx + data_size)) == -1)
{
pcl_close (fd);
resetLockingPermissions (file_name, file_lock);
throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during munmap ()!");
return (-1);
}
#endif
// Close file
#if _WIN32
CloseHandle (h_native_file);
#else
pcl_close (fd);
#endif
resetLockingPermissions (file_name, file_lock);
return 0;
}
