edit_octree.cpp
Go to the documentation of this file.
1 /*
2  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
3  * http://octomap.github.com/
4  *
5  * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
6  * All rights reserved.
7  * License: New BSD
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above copyright
15  * notice, this list of conditions and the following disclaimer in the
16  * documentation and/or other materials provided with the distribution.
17  * * Neither the name of the University of Freiburg nor the names of its
18  * contributors may be used to endorse or promote products derived from
19  * this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
25  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
26  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
27  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
28  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
29  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
30  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  */
33 
34 #include <string>
35 #include <fstream>
36 #include <iostream>
37 #include <octomap/octomap.h>
38 #include <cstdlib>
39 #include <cstring>
40 
41 using namespace std;
42 using namespace octomap;
43 
44 typedef unsigned char byte;
45 
46 int main(int argc, char **argv)
47 {
48 
49  //bool rotate = false; // Fix orientation of webots-exported files
50  bool show_help = false;
51  string outputFilename("");
52  string inputFilename("");
53 // double minX = 0.0;
54 // double minY = 0.0;
55 // double minZ = 0.0;
56 // double maxX = 0.0;
57 // double maxY = 0.0;
58 // double maxZ = 0.0;
59 // bool applyBBX = false;
60 // bool applyOffset = false;
61  octomap::point3d offset(0.0, 0.0, 0.0);
62 
63  double scale = 1.0;
64  double res = -1.0;
65 
66  if(argc == 1) show_help = true;
67  for(int i = 1; i < argc && !show_help; i++) {
68  if(strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-help") == 0 ||
69  strcmp(argv[i], "--usage") == 0 || strcmp(argv[i], "-usage") == 0 ||
70  strcmp(argv[i], "-h") == 0
71  )
72  show_help = true;
73  }
74 
75  if(show_help) {
76  cout << "Usage: "<<argv[0]<<" [OPTIONS] <filename.bt>" << endl;
77  cout << "\tOPTIONS:" << endl;
78  cout << "\t -o <file> Output filename (default: first input filename + .bt)" << endl;
79 // cout << "\t --mark-free Mark not occupied cells as 'free' (default: unknown)" << endl;
80 // cout << "\t --rotate Rotate left by 90 deg. to fix the coordinate system when exported from Webots" << endl;
81  cout << "\t --offset <x> <y> <z>: add an offset to the octree coordinates (translation)\n";
82 // cout << "\t --bb <minx> <miny> <minz> <maxx> <maxy> <maxz>: force bounding box for OcTree" << endl;
83  cout << "\t --res <resolution>: set ressolution of OcTree to new value\n";
84  cout << "\t --scale <scale>: scale octree resolution by a value\n";
85  exit(0);
86  }
87 
88  for(int i = 1; i < argc; i++) {
89  // Parse command line arguments
90  if(strcmp(argv[i], "--rotate") == 0) {
91  OCTOMAP_WARNING_STR(argv[i] << " not yet implemented!\n");
92  //rotate = true;
93  continue;
94  } else if(strcmp(argv[i], "-o") == 0 && i < argc - 1) {
95  i++;
96  outputFilename = argv[i];
97  continue;
98  } else if (strcmp(argv[i], "--bb") == 0 && i < argc - 7) {
99  OCTOMAP_WARNING_STR(argv[i] << " not yet implemented!\n");
100  i++;
101  //minX = atof(argv[i]);
102  i++;
103  //minY = atof(argv[i]);
104  i++;
105  //minZ = atof(argv[i]);
106  i++;
107  //maxX = atof(argv[i]);
108  i++;
109  //maxY = atof(argv[i]);
110  i++;
111  //maxZ = atof(argv[i]);
112 
113  //applyBBX = true;
114 
115  continue;
116  } else if (strcmp(argv[i], "--res") == 0 && i < argc - 1) {
117  i++;
118  res = atof(argv[i]);
119 
120  continue;
121  } else if (strcmp(argv[i], "--scale") == 0 && i < argc - 1) {
122  i++;
123  scale = atof(argv[i]);
124 
125  continue;
126  } else if (strcmp(argv[i], "--offset") == 0 && i < argc - 4) {
127  OCTOMAP_WARNING_STR(argv[i] << " not yet implemented!\n");
128  i++;
129  offset(0) = (float) atof(argv[i]);
130  i++;
131  offset(1) = (float) atof(argv[i]);
132  i++;
133  offset(2) = (float) atof(argv[i]);
134 
135  //applyOffset = true;
136 
137  continue;
138  } else if (i == argc-1){
139  inputFilename = string(argv[i]);
140  }
141  }
142 
143  if (outputFilename == ""){
144  outputFilename = inputFilename + ".edit.bt";
145  }
146 
147  OcTree* tree = new OcTree(0.1);
148  if (!tree->readBinary(inputFilename)){
149  OCTOMAP_ERROR("Could not open file, exiting.\n");
150  exit(1);
151  }
152 
153  // apply scale / resolution setting:
154 
155  if (scale != 1.0){
156  res = tree->getResolution() * scale;
157  }
158 
159  if (res > 0.0){
160  std::cout << "Setting new tree resolution: " << res << std::endl;
161  tree->setResolution(res);
162  }
163 
164  // TODO: implement rest (move, bounding box, rotation...)
165 
166 
167 
168 // size = width * height * depth;
169 // double res = double(scale)/double(depth);
170 //
171 // if(!tree) {
172 // cout << "Generate labeled octree with leaf size " << res << endl << endl;
173 // tree = new OcTree(res);
174 // }
175 //
176 // if (applyBBX){
177 // cout << "Bounding box for Octree: [" << minX << ","<< minY << "," << minZ << " - "
178 // << maxX << ","<< maxY << "," << maxZ << "]\n";
179 //
180 // }
181 // if (applyOffset){
182 // std::cout << "Offset on final map: "<< offset << std::endl;
183 //
184 // }
185 //
186 //
187 
188 // if(rotate) {
189 // endpoint.rotate_IP(M_PI_2, 0.0, 0.0);
190 // }
191 // if (applyOffset)
192 // endpoint += offset;
193 //
194 // if (!applyBBX || (endpoint(0) <= maxX && endpoint(0) >= minX
195 // && endpoint(1) <= maxY && endpoint(1) >= minY
196 // && endpoint(2) <= maxZ && endpoint(2) >= minZ)){
197 //
198 // // mark cell in octree as free or occupied
199 // if(mark_free || value == 1) {
200 // tree->updateNode(endpoint, value == 1);
201 // }
202 // } else{
203 // nr_voxels_out ++;
204 // }
205 // }
206 
207 
208 //
209 // // prune octree
210 // cout << "Prune octree" << endl << endl;
211 // tree->prune();
212 
213  // write octree to file
214 
215  cout << "Writing octree to " << outputFilename << endl;
216 
217  if (!tree->writeBinary(outputFilename)){
218  OCTOMAP_ERROR("Error writing tree to %s\n", outputFilename.c_str());
219  exit(1);
220  }
221 
222  cout << "done" << endl << endl;
223  delete tree;
224  return 0;
225 }
void setResolution(double r)
unsigned char byte
Definition: edit_octree.cpp:44
int main(int argc, char **argv)
Definition: edit_octree.cpp:46
bool writeBinary(const std::string &filename)
This class represents a three-dimensional vector.
Definition: Vector3.h:50
#define OCTOMAP_WARNING_STR(args)
Definition: octomap_types.h:76
double getResolution() const
#define OCTOMAP_ERROR(...)
Definition: octomap_types.h:77


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Wed Jun 5 2019 19:26:27