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