edit_octree.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 
00034 #include <string>
00035 #include <fstream>
00036 #include <iostream>
00037 #include <octomap/octomap.h>
00038 #include <cstdlib>
00039 #include <cstring>
00040 
00041 using namespace std;
00042 using namespace octomap;
00043 
00044 typedef unsigned char byte;
00045 
00046 int main(int argc, char **argv)
00047 {
00048 
00049     //bool rotate = false;       // Fix orientation of webots-exported files
00050     bool show_help = false;
00051     string outputFilename("");
00052     string inputFilename("");
00053 //    double minX = 0.0;
00054 //    double minY = 0.0;
00055 //    double minZ = 0.0;
00056 //    double maxX = 0.0;
00057 //    double maxY = 0.0;
00058 //    double maxZ = 0.0;
00059 //    bool applyBBX = false;
00060 //    bool applyOffset = false;
00061     octomap::point3d offset(0.0, 0.0, 0.0);
00062 
00063     double scale = 1.0;
00064     double res = -1.0;
00065 
00066     if(argc == 1) show_help = true;
00067     for(int i = 1; i < argc && !show_help; i++) {
00068         if(strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-help") == 0 ||
00069            strcmp(argv[i], "--usage") == 0 || strcmp(argv[i], "-usage") == 0 ||
00070            strcmp(argv[i], "-h") == 0
00071           )
00072                show_help = true;
00073     }
00074     
00075     if(show_help) {
00076         cout << "Usage: "<<argv[0]<<" [OPTIONS] <filename.bt>" << endl;
00077         cout << "\tOPTIONS:" << endl;
00078         cout << "\t -o <file>        Output filename (default: first input filename + .bt)" << endl;
00079 //        cout << "\t --mark-free      Mark not occupied cells as 'free' (default: unknown)" << endl;
00080 //        cout << "\t --rotate         Rotate left by 90 deg. to fix the coordinate system when exported from Webots" << endl;
00081         cout << "\t --offset <x> <y> <z>: add an offset to the octree coordinates (translation)\n";
00082 //        cout << "\t --bb <minx> <miny> <minz> <maxx> <maxy> <maxz>: force bounding box for OcTree" << endl;
00083         cout << "\t --res <resolution>: set ressolution of OcTree to new value\n";
00084         cout << "\t --scale <scale>: scale  octree resolution by a value\n";
00085         exit(0);
00086     }
00087     
00088     for(int i = 1; i < argc; i++) {
00089       // Parse command line arguments
00090       if(strcmp(argv[i], "--rotate") == 0) {
00091         OCTOMAP_WARNING_STR(argv[i] << " not yet implemented!\n");
00092         //rotate = true;
00093         continue;
00094       } else if(strcmp(argv[i], "-o") == 0 && i < argc - 1) {
00095         i++;
00096         outputFilename = argv[i];
00097         continue;
00098       } else if (strcmp(argv[i], "--bb") == 0 && i < argc - 7) {
00099         OCTOMAP_WARNING_STR(argv[i] << " not yet implemented!\n");
00100         i++;
00101         //minX = atof(argv[i]);
00102         i++;
00103         //minY = atof(argv[i]);
00104         i++;
00105         //minZ = atof(argv[i]);
00106         i++;
00107         //maxX = atof(argv[i]);
00108         i++;
00109         //maxY = atof(argv[i]);
00110         i++;
00111         //maxZ = atof(argv[i]);
00112 
00113         //applyBBX = true;
00114 
00115         continue;
00116       } else if (strcmp(argv[i], "--res") == 0 && i < argc - 1) {
00117         i++;
00118         res = atof(argv[i]);
00119 
00120         continue;
00121       } else if (strcmp(argv[i], "--scale") == 0 && i < argc - 1) {
00122         i++;
00123         scale = atof(argv[i]);
00124 
00125         continue;
00126       } else if (strcmp(argv[i], "--offset") == 0 && i < argc - 4) {
00127         OCTOMAP_WARNING_STR(argv[i] << " not yet implemented!\n");
00128         i++;
00129         offset(0) = (float) atof(argv[i]);
00130         i++;
00131         offset(1) = (float) atof(argv[i]);
00132         i++;
00133         offset(2) = (float) atof(argv[i]);
00134 
00135         //applyOffset = true;
00136 
00137         continue;
00138       } else if (i == argc-1){
00139         inputFilename = string(argv[i]);
00140       }
00141     }
00142 
00143     if (outputFilename == ""){
00144       outputFilename = inputFilename + ".edit.bt";
00145     }
00146 
00147     OcTree* tree = new OcTree(0.1);
00148     if (!tree->readBinary(inputFilename)){
00149       OCTOMAP_ERROR("Could not open file, exiting.\n");
00150       exit(1);
00151     }
00152 
00153     // apply scale / resolution setting:
00154 
00155     if (scale != 1.0){
00156       res = tree->getResolution() * scale;
00157     }
00158 
00159     if (res > 0.0){
00160       std::cout << "Setting new tree resolution: " << res << std::endl;
00161       tree->setResolution(res);
00162     }
00163 
00164     // TODO: implement rest (move, bounding box, rotation...)
00165 
00166 
00167 
00168 //      size = width * height * depth;
00169 //      double res = double(scale)/double(depth);
00170 //
00171 //      if(!tree) {
00172 //        cout << "Generate labeled octree with leaf size " << res << endl << endl;
00173 //        tree = new OcTree(res);
00174 //      }
00175 //
00176 //      if (applyBBX){
00177 //        cout << "Bounding box for Octree: [" << minX << ","<< minY << "," << minZ << " - "
00178 //            << maxX << ","<< maxY << "," << maxZ << "]\n";
00179 //
00180 //      }
00181 //      if (applyOffset){
00182 //        std::cout << "Offset on final map: "<< offset << std::endl;
00183 //
00184 //      }
00185 //
00186 //
00187 
00188 //            if(rotate) {
00189 //              endpoint.rotate_IP(M_PI_2, 0.0, 0.0);
00190 //            }
00191 //            if (applyOffset)
00192 //              endpoint += offset;
00193 //
00194 //            if (!applyBBX  || (endpoint(0) <= maxX && endpoint(0) >= minX
00195 //                && endpoint(1) <= maxY && endpoint(1) >= minY
00196 //                && endpoint(2) <= maxZ && endpoint(2) >= minZ)){
00197 //
00198 //              // mark cell in octree as free or occupied
00199 //              if(mark_free || value == 1) {
00200 //                tree->updateNode(endpoint, value == 1);
00201 //              }
00202 //            } else{
00203 //              nr_voxels_out ++;
00204 //            }
00205 //          }
00206 
00207 
00208 //
00209 //    // prune octree
00210 //    cout << "Prune octree" << endl << endl;
00211 //    tree->prune();
00212  
00213     // write octree to file  
00214 
00215     cout << "Writing octree to " << outputFilename << endl;
00216    
00217     if (!tree->writeBinary(outputFilename)){
00218       OCTOMAP_ERROR("Error writing tree to %s\n", outputFilename.c_str());
00219       exit(1);
00220     }
00221 
00222     cout << "done" << endl << endl;
00223     delete tree;
00224     return 0;
00225 }


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