int
pcl::PCDGrabberBase::PCDGrabberImpl::openTARFile (const std::string &file_name)
{
tar_fd_ = pcl_open (file_name.c_str (), O_RDONLY);
if (tar_fd_ == -1)
return (-1);
return (0);
}
bool
pcl::io::LZFImageWriter::saveImageBlob (const char* data,
size_t data_size,
const std::string &filename)
{
#ifdef _WIN32
HANDLE h_native_file = CreateFile (filename.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
if (h_native_file == INVALID_HANDLE_VALUE)
return (false);
HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, data_size, NULL);
char *map = static_cast<char*> (MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, data_size));
CloseHandle (fm);
memcpy (&map[0], data, data_size);
UnmapViewOfFile (map);
CloseHandle (h_native_file);
#else
int fd = pcl_open (filename.c_str (), O_RDWR | O_CREAT | O_TRUNC, static_cast<mode_t> (0600));
if (fd < 0)
return (false);
// Stretch the file size to the size of the data
off_t result = pcl_lseek (fd, data_size - 1, SEEK_SET);
if (result < 0)
{
pcl_close (fd);
return (false);
}
// 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);
return (false);
}
char *map = static_cast<char*> (mmap (0, data_size, PROT_WRITE, MAP_SHARED, fd, 0));
if (map == reinterpret_cast<char*> (-1)) // MAP_FAILED
{
pcl_close (fd);
return (false);
}
// Copy the data
memcpy (&map[0], data, data_size);
if (munmap (map, (data_size)) == -1)
{
pcl_close (fd);
return (false);
}
pcl_close (fd);
#endif
return (true);
}
bool
pcl::io::LZFImageReader::loadImageBlob (const std::string &filename,
std::vector<char> &data,
uint32_t &uncompressed_size)
{
if (filename == "" || !boost::filesystem::exists (filename))
{
PCL_ERROR ("[pcl::io::LZFImageReader::loadImage] Could not find file '%s'.\n", filename.c_str ());
return (false);
}
// Open for reading
int fd = pcl_open (filename.c_str (), O_RDONLY);
if (fd == -1)
{
PCL_ERROR ("[pcl::io::LZFImageReader::loadImage] Failure to open file %s\n", filename.c_str () );
return (false);
}
// Seek to the end of file to get the filesize
off_t data_size = pcl_lseek (fd, 0, SEEK_END);
if (data_size < 0)
{
pcl_close (fd);
PCL_ERROR ("[pcl::io::LZFImageReader::loadImage] lseek errno: %d strerror: %s\n", errno, strerror (errno));
PCL_ERROR ("[pcl::io::LZFImageReader::loadImage] Error during lseek ()!\n");
return (false);
}
pcl_lseek (fd, 0, SEEK_SET);
#ifdef _WIN32
// As we don't know the real size of data (compressed or not),
// we set dwMaximumSizeHigh = dwMaximumSizeLow = 0 so as to map the whole file
HANDLE fm = CreateFileMapping ((HANDLE) _get_osfhandle (fd), NULL, PAGE_READONLY, 0, 0, NULL);
// As we don't know the real size of data (compressed or not),
// we set dwNumberOfBytesToMap = 0 so as to map the whole file
char *map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ, 0, 0, 0));
if (map == NULL)
{
CloseHandle (fm);
pcl_close (fd);
PCL_ERROR ("[pcl::io::LZFImageReader::loadImage] Error mapping view of file, %s\n", filename.c_str ());
return (false);
}
#else
char *map = static_cast<char*> (mmap (0, data_size, PROT_READ, MAP_SHARED, fd, 0));
if (map == reinterpret_cast<char*> (-1)) // MAP_FAILED
{
pcl_close (fd);
PCL_ERROR ("[pcl::io::LZFImageReader::loadImage] Error preparing mmap for PCLZF file.\n");
return (false);
}
#endif
// Check the header identifier here
char header_string[5];
memcpy (&header_string, &map[0], 5); // PCLZF
if (std::string (header_string).substr (0, 5) != "PCLZF")
{
PCL_ERROR ("[pcl::io::LZFImageReader::loadImage] Wrong signature header! Should be 'P'C'L'Z'F'.\n");
#ifdef _WIN32
UnmapViewOfFile (map);
CloseHandle (fm);
#else
munmap (map, data_size);
#endif
return (false);
}
memcpy (&width_, &map[5], sizeof (uint32_t));
memcpy (&height_, &map[9], sizeof (uint32_t));
char imgtype_string[16];
memcpy (&imgtype_string, &map[13], 16); // BAYER8, RGB24_, YUV422_, ...
image_type_identifier_ = std::string (imgtype_string).substr (0, 15);
image_type_identifier_.insert (image_type_identifier_.end (), 1, '\0');
static const int header_size = LZF_HEADER_SIZE;
uint32_t compressed_size;
memcpy (&compressed_size, &map[29], sizeof (uint32_t));
if (compressed_size + header_size != data_size)
{
PCL_ERROR ("[pcl::io::LZFImageReader::loadImage] Number of bytes to decompress written in file (%u) differs from what it should be (%u)!\n", compressed_size, data_size - header_size);
#ifdef _WIN32
UnmapViewOfFile (map);
CloseHandle (fm);
#else
munmap (map, data_size);
#endif
return (false);
}
memcpy (&uncompressed_size, &map[33], sizeof (uint32_t));
data.resize (compressed_size);
memcpy (&data[0], &map[header_size], compressed_size);
#ifdef _WIN32
UnmapViewOfFile (map);
CloseHandle (fm);
#else
if (munmap (map, data_size) == -1)
PCL_ERROR ("[pcl::io::LZFImageReader::loadImage] Munmap failure\n");
#endif
pcl_close (fd);
data_size = off_t (compressed_size); // We only care about this from here on
return (true);
}
template <typename PointXYZT, typename PointRGBT> bool
pcl::LineRGBD<PointXYZT, PointRGBT>::loadTemplates (const std::string &file_name, const size_t object_id)
{
// Open the file
int ltm_fd = pcl_open (file_name.c_str (), O_RDONLY);
if (ltm_fd == -1)
return (false);
int ltm_offset = 0;
pcl::io::TARHeader ltm_header;
PCDReader pcd_reader;
std::string pcd_ext (".pcd");
std::string sqmmt_ext (".sqmmt");
// While there still is an LTM header to be read
while (readLTMHeader (ltm_fd, ltm_header))
{
ltm_offset += 512;
// Search for extension
std::string chunk_name (ltm_header.file_name);
std::transform (chunk_name.begin (), chunk_name.end (), chunk_name.begin (), tolower);
std::string::size_type it;
if ((it = chunk_name.find (pcd_ext)) != std::string::npos &&
(pcd_ext.size () - (chunk_name.size () - it)) == 0)
{
PCL_DEBUG ("[pcl::LineRGBD::loadTemplates] Reading and parsing %s as a PCD file.\n", chunk_name.c_str ());
// Read the next PCD file
template_point_clouds_.resize (template_point_clouds_.size () + 1);
pcd_reader.read (file_name, template_point_clouds_[template_point_clouds_.size () - 1], ltm_offset);
// Increment the offset for the next file
ltm_offset += (ltm_header.getFileSize ()) + (512 - ltm_header.getFileSize () % 512);
}
else if ((it = chunk_name.find (sqmmt_ext)) != std::string::npos &&
(sqmmt_ext.size () - (chunk_name.size () - it)) == 0)
{
PCL_DEBUG ("[pcl::LineRGBD::loadTemplates] Reading and parsing %s as a SQMMT file.\n", chunk_name.c_str ());
unsigned int fsize = ltm_header.getFileSize ();
char *buffer = new char[fsize];
int result = static_cast<int> (::read (ltm_fd, reinterpret_cast<char*> (&buffer[0]), fsize));
if (result == -1)
{
delete [] buffer;
PCL_ERROR ("[pcl::LineRGBD::loadTemplates] Error reading SQMMT template from file!\n");
break;
}
// Read a SQMMT file
std::stringstream stream (std::stringstream::in | std::stringstream::out | std::stringstream::binary);
stream.write (buffer, fsize);
SparseQuantizedMultiModTemplate sqmmt;
sqmmt.deserialize (stream);
linemod_.addTemplate (sqmmt);
object_ids_.push_back (object_id);
// Increment the offset for the next file
ltm_offset += (ltm_header.getFileSize ()) + (512 - ltm_header.getFileSize () % 512);
delete [] buffer;
}
if (static_cast<int> (pcl_lseek (ltm_fd, ltm_offset, SEEK_SET)) < 0)
break;
}
// Close the file
pcl_close (ltm_fd);
// Compute 3D bounding boxes from the template point clouds
bounding_boxes_.resize (template_point_clouds_.size ());
for (size_t i = 0; i < template_point_clouds_.size (); ++i)
{
PointCloud<PointXYZRGBA> & template_point_cloud = template_point_clouds_[i];
BoundingBoxXYZ & bb = bounding_boxes_[i];
bb.x = bb.y = bb.z = std::numeric_limits<float>::max ();
bb.width = bb.height = bb.depth = 0.0f;
float center_x = 0.0f;
float center_y = 0.0f;
float center_z = 0.0f;
float min_x = std::numeric_limits<float>::max ();
float min_y = std::numeric_limits<float>::max ();
float min_z = std::numeric_limits<float>::max ();
float max_x = -std::numeric_limits<float>::max ();
float max_y = -std::numeric_limits<float>::max ();
float max_z = -std::numeric_limits<float>::max ();
size_t counter = 0;
for (size_t j = 0; j < template_point_cloud.size (); ++j)
{
const PointXYZRGBA & p = template_point_cloud.points[j];
if (!pcl_isfinite (p.x) || !pcl_isfinite (p.y) || !pcl_isfinite (p.z))
continue;
min_x = std::min (min_x, p.x);
min_y = std::min (min_y, p.y);
min_z = std::min (min_z, p.z);
max_x = std::max (max_x, p.x);
max_y = std::max (max_y, p.y);
max_z = std::max (max_z, p.z);
center_x += p.x;
center_y += p.y;
center_z += p.z;
++counter;
}
center_x /= static_cast<float> (counter);
center_y /= static_cast<float> (counter);
center_z /= static_cast<float> (counter);
bb.width = max_x - min_x;
bb.height = max_y - min_y;
bb.depth = max_z - min_z;
bb.x = (min_x + bb.width / 2.0f) - center_x - bb.width / 2.0f;
bb.y = (min_y + bb.height / 2.0f) - center_y - bb.height / 2.0f;
bb.z = (min_z + bb.depth / 2.0f) - center_z - bb.depth / 2.0f;
for (size_t j = 0; j < template_point_cloud.size (); ++j)
{
PointXYZRGBA p = template_point_cloud.points[j];
if (!pcl_isfinite (p.x) || !pcl_isfinite (p.y) || !pcl_isfinite (p.z))
continue;
p.x -= center_x;
p.y -= center_y;
p.z -= center_z;
template_point_cloud.points[j] = p;
}
}
return (true);
}
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, static_cast<mode_t> (0600));
if (fd < 0)
{
throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during open!");
return (-1);
}
#endif
std::vector<sensor_msgs::PointField> 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
int result = static_cast<int> (pcl_lseek (fd, getpagesize () + data_size - 1, SEEK_SET));
if (result < 0)
{
pcl_close (fd);
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);
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);
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);
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
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, static_cast<mode_t> (0600));
if (fd < 0)
{
throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during open!");
return (-1);
}
#endif
std::vector<sensor_msgs::PointField> 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
throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during compression!");
return (-1);
}
#if !_WIN32
// Stretch the file size to the size of the data
int result = static_cast<int> (pcl_lseek (fd, getpagesize () + data_size - 1, SEEK_SET));
if (result < 0)
{
pcl_close (fd);
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);
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);
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);
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
free (only_valid_data);
free (temp_buf);
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;
}
