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/AbstractOcTree.h>
00035 #include <octomap/OcTree.h>
00036 #include <octomap/ColorOcTree.h>
00037 #include <fstream>
00038 #include <iostream>
00039 #include <string.h>
00040 #include <stdlib.h>
00041 #include <list>
00042
00043 using namespace std;
00044 using namespace octomap;
00045
00046 void printUsage(char* self){
00047 std::cerr << "\nUSAGE: " << self << " input.(ot|bt|cot) [output.ot]\n\n";
00048
00049 std::cerr << "This tool converts between OctoMap octree file formats, \n"
00050 "e.g. to convert old legacy files to the new .ot format or to convert \n"
00051 "between .bt and .ot files. The default output format is .ot.\n\n";
00052
00053 exit(0);
00054 }
00055
00056 int main(int argc, char** argv) {
00057 string inputFilename = "";
00058 string outputFilename = "";
00059
00060 if (argc < 2 || argc > 3 || (argc > 1 && strcmp(argv[1], "-h") == 0)){
00061 printUsage(argv[0]);
00062 }
00063
00064 inputFilename = std::string(argv[1]);
00065 if (argc == 3)
00066 outputFilename = std::string(argv[2]);
00067 else{
00068 outputFilename = inputFilename + ".ot";
00069 }
00070
00071
00072 cout << "\nReading OcTree file\n===========================\n";
00073 std::ifstream file(inputFilename.c_str(), std::ios_base::in |std::ios_base::binary);
00074
00075 if (!file.is_open()){
00076 OCTOMAP_ERROR_STR("Filestream to "<< inputFilename << " not open, nothing read.");
00077 exit(-1);
00078 }
00079
00080 std::istream::pos_type streampos = file.tellg();
00081 AbstractOcTree* tree;
00082
00083
00084 if (inputFilename.length() > 3 && (inputFilename.compare(inputFilename.length()-3, 3, ".bt") == 0)){
00085 OcTree* binaryTree = new OcTree(0.1);
00086
00087 if (binaryTree->readBinary(file) && binaryTree->size() > 1)
00088 tree = binaryTree;
00089 else {
00090 OCTOMAP_ERROR_STR("Could not detect binary OcTree format in file.");
00091 exit(-1);
00092
00093 }
00094 } else {
00095 tree = AbstractOcTree::read(file);
00096 if (!tree){
00097 OCTOMAP_WARNING_STR("Could not detect OcTree in file, trying legacy formats.");
00098
00099
00100 file.clear();
00101 file.seekg(streampos);
00102 ColorOcTree* colorTree = new ColorOcTree(0.1);
00103 colorTree->readData(file);
00104 if (colorTree->size() > 1 && file.good()){
00105 OCTOMAP_WARNING_STR("Detected Binary ColorOcTree to convert. \nPlease check and update the new file header (resolution will likely be wrong).");
00106 tree = colorTree;
00107 } else{
00108 delete colorTree;
00109 std::cerr << "Error reading from file " << inputFilename << std::endl;
00110 exit(-1);
00111 }
00112 }
00113
00114
00115 }
00116
00117
00118 file.close();
00119
00120
00121 if (outputFilename.length() > 3 && (outputFilename.compare(outputFilename.length()-3, 3, ".bt") == 0)){
00122 std::cerr << "Writing binary (BonsaiTree) file" << std::endl;
00123 AbstractOccupancyOcTree* octree = dynamic_cast<AbstractOccupancyOcTree*>(tree);
00124 if (octree){
00125 if (!octree->writeBinary(outputFilename)){
00126 std::cerr << "Error writing to " << outputFilename << std::endl;
00127 exit(-2);
00128 }
00129 } else {
00130 std::cerr << "Error: Writing to .bt is not supported for this tree type: " << tree->getTreeType() << std::endl;
00131 exit(-2);
00132 }
00133 } else{
00134 std::cerr << "Writing general OcTree file" << std::endl;
00135 if (!tree->write(outputFilename)){
00136 std::cerr << "Error writing to " << outputFilename << std::endl;
00137 exit(-2);
00138 }
00139 }
00140
00141
00142
00143
00144
00145 std::cout << "Finished writing to " << outputFilename << std::endl;
00146 exit(0);
00147
00148 }