PCL AppendBinary

// 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;
}