Skip to content

Instantly share code, notes, and snippets.

@TaylorDale
Last active March 11, 2021 03:44
Show Gist options
  • Save TaylorDale/49ccff40d1591db2985e36cd14a003d1 to your computer and use it in GitHub Desktop.
Save TaylorDale/49ccff40d1591db2985e36cd14a003d1 to your computer and use it in GitHub Desktop.
// ApPCDWriter.h
#include <pcl/io/pcd_io.h>
template<typename PointT>
class ApPCDWriter: public pcl::PCDWriter
{
public:
int appendBinary(const std::string &file_name,
const pcl::PointCloud<PointT> &cloud);
};
// ApPCDWriter.cpp
#include "ApPCDWriter.h"
template class ApPCDWriter<pcl::PointXYZ>;
template<typename PointT>
int ApPCDWriter<PointT>::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;
pcl::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 * pcl::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)
{
// warn , deleteme: do more than just warn : is the file going to end up corrupted?
PCL_WARN("pcl::PCDWriter::appendBinary] The expected data size and the current data size are different!");
}
// 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
// NFS SYSTEM PROTECTION BELOW
// 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;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment