Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 #include <octomap/octomap.h>
00035 #include <fstream>
00036 #include <iostream>
00037 #include <string.h>
00038 #include <stdlib.h>
00039 #include <list>
00040
00041 using namespace std;
00042 using namespace octomap;
00043
00044 void printUsage(char* self){
00045 std::cerr << "\nUSAGE: " << self << " input.bt\n\n";
00046
00047 std::cerr << "This tool will convert the occupied voxels of a binary OctoMap \n"
00048 "file input.bt to a VRML2.0 file input.bt.wrl.\n\n";
00049
00050 std::cerr << "WARNING: The output files will be quite large!\n\n";
00051
00052 exit(0);
00053 }
00054
00055 int main(int argc, char** argv) {
00056
00057 string vrmlFilename = "";
00058 string btFilename = "";
00059
00060 if (argc != 2 || (argc > 1 && strcmp(argv[1], "-h") == 0)){
00061 printUsage(argv[0]);
00062 }
00063
00064 btFilename = std::string(argv[1]);
00065 vrmlFilename = btFilename + ".wrl";
00066
00067
00068 cout << "\nReading OcTree file\n===========================\n";
00069
00070 OcTree* tree = new OcTree(btFilename);
00071
00072
00073 cout << "\nWriting occupied volumes to VRML\n===========================\n";
00074
00075 std::ofstream outfile (vrmlFilename.c_str());
00076
00077 outfile << "#VRML V2.0 utf8\n#\n";
00078 outfile << "# created from OctoMap file "<<btFilename<< " with bt2vrml\n";
00079
00080
00081 size_t count(0);
00082 for(OcTree::leaf_iterator it = tree->begin(), end=tree->end(); it!= end; ++it) {
00083 if(tree->isNodeOccupied(*it)){
00084 count++;
00085 double size = it.getSize();
00086 outfile << "Transform { translation "
00087 << it.getX() << " " << it.getY() << " " << it.getZ()
00088 << " \n children ["
00089 << " Shape { geometry Box { size "
00090 << size << " " << size << " " << size << "} } ]\n"
00091 << "}\n";
00092 }
00093 }
00094
00095 delete tree;
00096
00097 outfile.close();
00098
00099 std::cout << "Finished writing "<< count << " voxels to " << vrmlFilename << std::endl;
00100
00101 }