/* ---[ */
int
main (int argc, char** argv)
{
// Parse the command line arguments for .pcd files
std::vector<int> p_file_indices;
p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
if (p_file_indices.empty ())
{
print_error (" Need at least an input PCD file (e.g. scene.pcd) to continue!\n\n");
print_info ("Ideally, need an input file, and three output PCD files, e.g., object.pcd, plane.pcd, rest.pcd\n");
return (-1);
}
string object_file = "object.pcd", plane_file = "plane.pcd", rest_file = "rest.pcd";
if (p_file_indices.size () >= 4)
rest_file = argv[p_file_indices[3]];
if (p_file_indices.size () >= 3)
plane_file = argv[p_file_indices[2]];
if (p_file_indices.size () >= 2)
object_file = argv[p_file_indices[1]];
PCDReader reader;
// Test the header
pcl::PCLPointCloud2 dummy;
reader.readHeader (argv[p_file_indices[0]], dummy);
if (dummy.height != 1 && getFieldIndex (dummy, "rgba") != -1)
{
print_highlight ("Enabling 2D image viewer mode.\n");
ObjectSelection<PointXYZRGBA> s;
if (!s.load (argv[p_file_indices[0]])) return (-1);
s.initGUI ();
s.compute ();
s.save (object_file, plane_file);
}
else
{
ObjectSelection<PointXYZ> s;
if (!s.load (argv[p_file_indices[0]])) return (-1);
s.initGUI ();
s.compute ();
s.save (object_file, plane_file);
}
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;
}
