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