binvox2bt.cpp
Go to the documentation of this file.
00001 /*
00002  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
00003  * http://octomap.github.com/
00004  *
00005  * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
00006  * All rights reserved.
00007  * License: New BSD
00008  *
00009  * Redistribution and use in source and binary forms, with or without
00010  * modification, are permitted provided that the following conditions are met:
00011  *
00012  *     * Redistributions of source code must retain the above copyright
00013  *       notice, this list of conditions and the following disclaimer.
00014  *     * Redistributions in binary form must reproduce the above copyright
00015  *       notice, this list of conditions and the following disclaimer in the
00016  *       documentation and/or other materials provided with the distribution.
00017  *     * Neither the name of the University of Freiburg nor the names of its
00018  *       contributors may be used to endorse or promote products derived from
00019  *       this software without specific prior written permission.
00020  *
00021  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00022  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00023  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00024  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00025  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00026  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00027  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00028  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00029  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00030  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031  * POSSIBILITY OF SUCH DAMAGE.
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;               // binvox file format version (should be 1)
00064     int depth, height, width;  // dimensions of the voxel grid
00065     int size;                  // number of grid cells (height * width * depth)
00066     float tx, ty, tz;          // Translation
00067     float scale;               // Scaling factor
00068     bool mark_free = false;    // Mark free cells (false = cells remain "unknown")    
00069     bool rotate = false;       // Fix orientation of webots-exported files
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         // Parse command line arguments    
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         // Open input file
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         // read header
00165         string line;
00166         *input >> line;    // #binvox
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 {    // skip until end of line
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         // read voxel data
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);    // need to read every byte now (!)
00238         *input >> value;    // read the linefeed char
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                     // Output progress dots
00248                     if(i % (size / 20) == 0) {
00249                         cout << ".";            
00250                         cout.flush();
00251                     }
00252                     // voxel index --> voxel coordinates 
00253                     int y = i % width;
00254                     int z = (i / width) % height;
00255                     int x = i / (width * height);
00256                     
00257                     // voxel coordinates --> world coordinates
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                       // mark cell in octree as free or occupied
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             }    // if file still ok
00284             
00285         }    // while
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     // prune octree
00294     cout << "Pruning octree" << endl << endl;
00295     tree->updateInnerOccupancy();
00296     tree->prune();
00297  
00298     // write octree to file  
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 }


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Aug 27 2015 14:13:14