bool AbstractOccupancyOcTree::readBinaryLegacyHeader(std::istream &s, unsigned int& size, double& res) {
if (!s.good()){
OCTOMAP_WARNING_STR("Input filestream not \"good\" in OcTree::readBinary");
}
int tree_type = -1;
s.read((char*)&tree_type, sizeof(tree_type));
if (tree_type == 3){
this->clear();
s.read((char*)&res, sizeof(res));
if (res <= 0.0){
OCTOMAP_ERROR("Invalid tree resolution: %f", res);
return false;
}
s.read((char*)&size, sizeof(size));
return true;
}
else {
OCTOMAP_ERROR_STR("Binary file does not contain an OcTree!");
return false;
}
}
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;
}
bool AbstractOccupancyOcTree::readBinary(std::istream &s) {
if (!s.good()){
OCTOMAP_WARNING_STR("Input filestream not \"good\" in OcTree::readBinary");
}
// check if first line valid:
std::string line;
std::istream::pos_type streampos = s.tellg();
std::getline(s, line);
unsigned size;
double res;
if (line.compare(0,AbstractOccupancyOcTree::binaryFileHeader.length(), AbstractOccupancyOcTree::binaryFileHeader) ==0){
std::string id;
if (!AbstractOcTree::readHeader(s, id, size, res))
return false;
OCTOMAP_DEBUG_STR("Reading binary octree type "<< id);
} else{ // try to read old binary format:
s.clear(); // clear eofbit of istream
s.seekg(streampos);
if (readBinaryLegacyHeader(s, size, res)){
OCTOMAP_WARNING_STR("You are using an outdated binary tree file format.");
OCTOMAP_WARNING_STR("Please convert your .bt files with convert_octree.");
}
else {
OCTOMAP_ERROR_STR("First line of OcTree file header does not start with \""<< AbstractOccupancyOcTree::binaryFileHeader<<"\"");
return false;
}
}
// otherwise: values are valid, stream is now at binary data!
this->clear();
this->setResolution(res);
this->readBinaryData(s);
if (size != this->size()){
OCTOMAP_ERROR("Tree size mismatch: # read nodes (%zu) != # expected nodes (%d)\n",this->size(), size);
return false;
}
return true;
}
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;
}