std::istream& Pointcloud::readBinary(std::istream &s) {
uint32_t pc_size = 0;
s.read((char*)&pc_size, sizeof(pc_size));
OCTOMAP_DEBUG("Reading %d points from binary file...", pc_size);
if (pc_size > 0) {
this->points.reserve(pc_size);
point3d p;
for (uint32_t i=0; i<pc_size; i++) {
p.readBinary(s);
if (!s.fail()) {
this->push_back(p);
}
else {
OCTOMAP_ERROR("Pointcloud::readBinary: ERROR.\n" );
break;
}
}
}
assert(pc_size == this->size());
OCTOMAP_DEBUG("done.\n");
return s;
}
std::ostream& Pointcloud::writeBinary(std::ostream &s) const {
size_t pc_size = this->size();
OCTOMAP_DEBUG("Writing %d points to binary file...", pc_size);
s.write((char*)&pc_size, sizeof(pc_size));
for (Pointcloud::const_iterator it = this->begin(); it != this->end(); it++) {
it->writeBinary(s);
}
OCTOMAP_DEBUG("done.\n");
return s;
}
std::ostream& Pointcloud::writeBinary(std::ostream &s) const {
// check if written unsigned int can hold size
size_t orig_size = this->size();
if (orig_size > std::numeric_limits<uint32_t>::max()){
OCTOMAP_ERROR("Pointcloud::writeBinary ERROR: Point cloud too large to be written");
return s;
}
uint32_t pc_size = static_cast<uint32_t>(this->size());
OCTOMAP_DEBUG("Writing %u points to binary file...", pc_size);
s.write((char*)&pc_size, sizeof(pc_size));
for (Pointcloud::const_iterator it = this->begin(); it != this->end(); it++) {
it->writeBinary(s);
}
OCTOMAP_DEBUG("done.\n");
return s;
}
bool AbstractOccupancyOcTree::writeBinaryConst(std::ostream &s) const{
// write new header first:
s << binaryFileHeader <<"\n# (feel free to add / change comments, but leave the first line as it is!)\n#\n";
s << "id " << this->getTreeType() << std::endl;
s << "size "<< this->size() << std::endl;
s << "res " << this->getResolution() << std::endl;
s << "data" << std::endl;
writeBinaryData(s);
if (s.good()){
OCTOMAP_DEBUG(" done.\n");
return true;
} else {
OCTOMAP_WARNING_STR("Output stream not \"good\" after writing tree");
return false;
}
}
