void Pointcloud::writeVrml(std::string filename){ std::ofstream outfile (filename.c_str()); outfile << "#VRML V2.0 utf8" << std::endl; outfile << "Transform {" << std::endl; outfile << "translation 0 0 0" << std::endl; outfile << "rotation 0 0 0 0" << std::endl; outfile << " children [" << std::endl; outfile << " Shape{" << std::endl; outfile << " geometry PointSet {" << std::endl; outfile << " coord Coordinate {" << std::endl; outfile << " point [" << std::endl; OCTOMAP_DEBUG_STR("PointCloud::writeVrml writing " << points.size() << " points to " << filename.c_str() << "."); for (unsigned int i = 0; i < (points.size()); i++){ outfile << "\t\t" << (points[i])(0) << " " << (points[i])(1) << " " << (points[i])(2) << "\n"; } outfile << " ]" << std::endl; outfile << " }" << std::endl; outfile << " color Color{" << std::endl; outfile << " color [" << std::endl; for (unsigned int i = 0; i < points.size(); i++){ outfile << "\t\t 1.0 1.0 1.0 \n"; } outfile << " ]" << std::endl; outfile << " }" << std::endl; outfile << " }" << std::endl; outfile << " }" << std::endl; outfile << " ]" << std::endl; outfile << "}" << std::endl; }
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; }