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
00045 #ifndef M_PI_2
00046 #define M_PI_2 1.5707963267948966192E0
00047 #endif
00048
00049 #include <string>
00050 #include <fstream>
00051 #include <iostream>
00052 #include <octomap/octomap.h>
00053 #include <cstdlib>
00054 #include <cstring>
00055
00056 using namespace std;
00057 using namespace octomap;
00058
00059 typedef unsigned char byte;
00060
00061 int main(int argc, char **argv)
00062 {
00063 int version;
00064 int depth, height, width;
00065 int size;
00066 float tx, ty, tz;
00067 float scale;
00068 bool mark_free = false;
00069 bool rotate = false;
00070 bool show_help = false;
00071 string output_filename;
00072 double minX = 0.0;
00073 double minY = 0.0;
00074 double minZ = 0.0;
00075 double maxX = 0.0;
00076 double maxY = 0.0;
00077 double maxZ = 0.0;
00078 bool applyBBX = false;
00079 bool applyOffset = false;
00080 octomap::point3d offset(0.0, 0.0, 0.0);
00081 OcTree *tree = 0;
00082
00083 if(argc == 1) show_help = true;
00084 for(int i = 1; i < argc && !show_help; i++) {
00085 if(strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-help") == 0 ||
00086 strcmp(argv[i], "--usage") == 0 || strcmp(argv[i], "-usage") == 0 ||
00087 strcmp(argv[i], "-h") == 0
00088 )
00089 show_help = true;
00090 }
00091
00092 if(show_help) {
00093 cout << "Usage: "<<argv[0]<<" [OPTIONS] <binvox filenames>" << endl;
00094 cout << "\tOPTIONS:" << endl;
00095 cout << "\t -o <file> Output filename (default: first input filename + .bt)\n";
00096 cout << "\t --mark-free Mark not occupied cells as 'free' (default: unknown)\n";
00097 cout << "\t --rotate Rotate left by 90 deg. to fix the coordinate system when exported from Webots\n";
00098 cout << "\t --bb <minx> <miny> <minz> <maxx> <maxy> <maxz>: force bounding box for OcTree\n";
00099 cout << "\t --offset <x> <y> <z>: add an offset to the final coordinates\n";
00100 cout << "If more than one binvox file is given, the models are composed to a single bonsai tree->\n";
00101 cout << "All options apply to the subsequent input files.\n\n";
00102 exit(0);
00103 }
00104
00105 for(int i = 1; i < argc; i++) {
00106
00107 if(strcmp(argv[i], "--mark-free") == 0) {
00108 mark_free = true;
00109 continue;
00110 } else if(strcmp(argv[i], "--no-mark-free") == 0) {
00111 mark_free = false;
00112 continue;
00113 }else if(strcmp(argv[i], "--rotate") == 0) {
00114 rotate = true;
00115 continue;
00116 } else if(strcmp(argv[i], "-o") == 0 && i < argc - 1) {
00117 i++;
00118 output_filename = argv[i];
00119
00120 continue;
00121 } else if (strcmp(argv[i], "--bb") == 0 && i < argc - 7) {
00122 i++;
00123 minX = atof(argv[i]);
00124 i++;
00125 minY = atof(argv[i]);
00126 i++;
00127 minZ = atof(argv[i]);
00128 i++;
00129 maxX = atof(argv[i]);
00130 i++;
00131 maxY = atof(argv[i]);
00132 i++;
00133 maxZ = atof(argv[i]);
00134
00135 applyBBX = true;
00136
00137 continue;
00138 } else if (strcmp(argv[i], "--offset") == 0 && i < argc - 4) {
00139 i++;
00140 offset(0) = (float) atof(argv[i]);
00141 i++;
00142 offset(1) = (float) atof(argv[i]);
00143 i++;
00144 offset(2) = (float) atof(argv[i]);
00145
00146 applyOffset = true;
00147
00148 continue;
00149 }
00150
00151
00152
00153 ifstream *input = new ifstream(argv[i], ios::in | ios::binary);
00154 if(!input->good()) {
00155 cerr << "Error: Could not open input file " << argv[i] << "!" << endl;
00156 exit(1);
00157 } else {
00158 cout << "Reading binvox file " << argv[i] << "." << endl;
00159 if(output_filename.empty()) {
00160 output_filename = string(argv[i]).append(".bt");
00161 }
00162 }
00163
00164
00165 string line;
00166 *input >> line;
00167 if (line.compare("#binvox") != 0) {
00168 cout << "Error: first line reads [" << line << "] instead of [#binvox]" << endl;
00169 delete input;
00170 return 0;
00171 }
00172 *input >> version;
00173 cout << "reading binvox version " << version << endl;
00174
00175 depth = -1;
00176 int done = 0;
00177 while(input->good() && !done) {
00178 *input >> line;
00179 if (line.compare("data") == 0) done = 1;
00180 else if (line.compare("dim") == 0) {
00181 *input >> depth >> height >> width;
00182 }
00183 else if (line.compare("translate") == 0) {
00184 *input >> tx >> ty >> tz;
00185 }
00186 else if (line.compare("scale") == 0) {
00187 *input >> scale;
00188 }
00189 else {
00190 cout << " unrecognized keyword [" << line << "], skipping" << endl;
00191 char c;
00192 do {
00193 c = input->get();
00194 } while(input->good() && (c != '\n'));
00195
00196 }
00197 }
00198 if (!done) {
00199 cout << " error reading header" << endl;
00200 return 0;
00201 }
00202 if (depth == -1) {
00203 cout << " missing dimensions in header" << endl;
00204 return 0;
00205 }
00206
00207 size = width * height * depth;
00208 int maxSide = std::max(std::max(width, height), depth);
00209 double res = double(scale)/double(maxSide);
00210
00211 if(!tree) {
00212 cout << "Generating octree with leaf size " << res << endl << endl;
00213 tree = new OcTree(res);
00214 }
00215
00216 if (applyBBX){
00217 cout << "Bounding box for Octree: [" << minX << ","<< minY << "," << minZ << " - "
00218 << maxX << ","<< maxY << "," << maxZ << "]\n";
00219
00220 }
00221 if (applyOffset){
00222 std::cout << "Offset on final map: "<< offset << std::endl;
00223
00224 }
00225
00226 cout << "Read data: ";
00227 cout.flush();
00228
00229
00230 byte value;
00231 byte count;
00232 int index = 0;
00233 int end_index = 0;
00234 unsigned nr_voxels = 0;
00235 unsigned nr_voxels_out = 0;
00236
00237 input->unsetf(ios::skipws);
00238 *input >> value;
00239
00240 while((end_index < size) && input->good()) {
00241 *input >> value >> count;
00242
00243 if (input->good()) {
00244 end_index = index + count;
00245 if (end_index > size) return 0;
00246 for(int i=index; i < end_index; i++) {
00247
00248 if(i % (size / 20) == 0) {
00249 cout << ".";
00250 cout.flush();
00251 }
00252
00253 int y = i % width;
00254 int z = (i / width) % height;
00255 int x = i / (width * height);
00256
00257
00258 point3d endpoint((float) ((double) x*res + tx + 0.000001),
00259 (float) ((double) y*res + ty + 0.000001),
00260 (float) ((double) z*res + tz + 0.000001));
00261
00262 if(rotate) {
00263 endpoint.rotate_IP(M_PI_2, 0.0, 0.0);
00264 }
00265 if (applyOffset)
00266 endpoint += offset;
00267
00268 if (!applyBBX || (endpoint(0) <= maxX && endpoint(0) >= minX
00269 && endpoint(1) <= maxY && endpoint(1) >= minY
00270 && endpoint(2) <= maxZ && endpoint(2) >= minZ)){
00271
00272
00273 if(mark_free || value == 1) {
00274 tree->updateNode(endpoint, value == 1, true);
00275 }
00276 } else{
00277 nr_voxels_out ++;
00278 }
00279 }
00280
00281 if (value) nr_voxels += count;
00282 index = end_index;
00283 }
00284
00285 }
00286
00287 cout << endl << endl;
00288
00289 input->close();
00290 cout << " read " << nr_voxels << " voxels, skipped "<<nr_voxels_out << " (out of bounding box)\n\n";
00291 }
00292
00293
00294 cout << "Pruning octree" << endl << endl;
00295 tree->updateInnerOccupancy();
00296 tree->prune();
00297
00298
00299 if(output_filename.empty()) {
00300 cerr << "Error: No input files found." << endl << endl;
00301 exit(1);
00302 }
00303
00304 cout << "Writing octree to " << output_filename << endl << endl;
00305
00306 tree->writeBinary(output_filename.c_str());
00307
00308 cout << "done" << endl << endl;
00309 return 0;
00310 }