edit_octree.cpp
Go to the documentation of this file.
1 /*
2  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
3  * https://octomap.github.io/
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 
45 int main(int argc, char **argv)
46 {
47 
48  //bool rotate = false; // Fix orientation of webots-exported files
49  bool show_help = false;
50  string outputFilename("");
51  string inputFilename("");
52 // double minX = 0.0;
53 // double minY = 0.0;
54 // double minZ = 0.0;
55 // double maxX = 0.0;
56 // double maxY = 0.0;
57 // double maxZ = 0.0;
58 // bool applyBBX = false;
59 // bool applyOffset = false;
60  octomap::point3d offset(0.0, 0.0, 0.0);
61 
62  double scale = 1.0;
63  double res = -1.0;
64 
65  if(argc == 1) show_help = true;
66  for(int i = 1; i < argc && !show_help; i++) {
67  if(strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-help") == 0 ||
68  strcmp(argv[i], "--usage") == 0 || strcmp(argv[i], "-usage") == 0 ||
69  strcmp(argv[i], "-h") == 0
70  )
71  show_help = true;
72  }
73 
74  if(show_help) {
75  cout << "Usage: "<<argv[0]<<" [OPTIONS] <filename.bt>" << endl;
76  cout << "\tOPTIONS:" << endl;
77  cout << "\t -o <file> Output filename (default: first input filename + .bt)" << endl;
78 // cout << "\t --mark-free Mark not occupied cells as 'free' (default: unknown)" << endl;
79 // cout << "\t --rotate Rotate left by 90 deg. to fix the coordinate system when exported from Webots" << endl;
80  cout << "\t --offset <x> <y> <z>: add an offset to the octree coordinates (translation)\n";
81 // cout << "\t --bb <minx> <miny> <minz> <maxx> <maxy> <maxz>: force bounding box for OcTree" << endl;
82  cout << "\t --res <resolution>: set ressolution of OcTree to new value\n";
83  cout << "\t --scale <scale>: scale octree resolution by a value\n";
84  exit(0);
85  }
86 
87  for(int i = 1; i < argc; i++) {
88  // Parse command line arguments
89  if(strcmp(argv[i], "--rotate") == 0) {
90  OCTOMAP_WARNING_STR(argv[i] << " not yet implemented!\n");
91  //rotate = true;
92  continue;
93  } else if(strcmp(argv[i], "-o") == 0 && i < argc - 1) {
94  i++;
95  outputFilename = argv[i];
96  continue;
97  } else if (strcmp(argv[i], "--bb") == 0 && i < argc - 7) {
98  OCTOMAP_WARNING_STR(argv[i] << " not yet implemented!\n");
99  i++;
100  //minX = atof(argv[i]);
101  i++;
102  //minY = atof(argv[i]);
103  i++;
104  //minZ = atof(argv[i]);
105  i++;
106  //maxX = atof(argv[i]);
107  i++;
108  //maxY = atof(argv[i]);
109  i++;
110  //maxZ = atof(argv[i]);
111 
112  //applyBBX = true;
113 
114  continue;
115  } else if (strcmp(argv[i], "--res") == 0 && i < argc - 1) {
116  i++;
117  res = atof(argv[i]);
118 
119  continue;
120  } else if (strcmp(argv[i], "--scale") == 0 && i < argc - 1) {
121  i++;
122  scale = atof(argv[i]);
123 
124  continue;
125  } else if (strcmp(argv[i], "--offset") == 0 && i < argc - 4) {
126  OCTOMAP_WARNING_STR(argv[i] << " not yet implemented!\n");
127  i++;
128  offset(0) = (float) atof(argv[i]);
129  i++;
130  offset(1) = (float) atof(argv[i]);
131  i++;
132  offset(2) = (float) atof(argv[i]);
133 
134  //applyOffset = true;
135 
136  continue;
137  } else if (i == argc-1){
138  inputFilename = string(argv[i]);
139  }
140  }
141 
142  if (outputFilename == ""){
143  outputFilename = inputFilename + ".edit.bt";
144  }
145 
146  OcTree* tree = new OcTree(0.1);
147  if (!tree->readBinary(inputFilename)){
148  OCTOMAP_ERROR("Could not open file, exiting.\n");
149  exit(1);
150  }
151 
152  // apply scale / resolution setting:
153 
154  if (scale != 1.0){
155  res = tree->getResolution() * scale;
156  }
157 
158  if (res > 0.0){
159  std::cout << "Setting new tree resolution: " << res << std::endl;
160  tree->setResolution(res);
161  }
162 
163  // TODO: implement rest (move, bounding box, rotation...)
164 
165 
166 
167 // size = width * height * depth;
168 // double res = double(scale)/double(depth);
169 //
170 // if(!tree) {
171 // cout << "Generate labeled octree with leaf size " << res << endl << endl;
172 // tree = new OcTree(res);
173 // }
174 //
175 // if (applyBBX){
176 // cout << "Bounding box for Octree: [" << minX << ","<< minY << "," << minZ << " - "
177 // << maxX << ","<< maxY << "," << maxZ << "]\n";
178 //
179 // }
180 // if (applyOffset){
181 // std::cout << "Offset on final map: "<< offset << std::endl;
182 //
183 // }
184 //
185 //
186 
187 // if(rotate) {
188 // endpoint.rotate_IP(M_PI_2, 0.0, 0.0);
189 // }
190 // if (applyOffset)
191 // endpoint += offset;
192 //
193 // if (!applyBBX || (endpoint(0) <= maxX && endpoint(0) >= minX
194 // && endpoint(1) <= maxY && endpoint(1) >= minY
195 // && endpoint(2) <= maxZ && endpoint(2) >= minZ)){
196 //
197 // // mark cell in octree as free or occupied
198 // if(mark_free || value == 1) {
199 // tree->updateNode(endpoint, value == 1);
200 // }
201 // } else{
202 // nr_voxels_out ++;
203 // }
204 // }
205 
206 
207 //
208 // // prune octree
209 // cout << "Prune octree" << endl << endl;
210 // tree->prune();
211 
212  // write octree to file
213 
214  cout << "Writing octree to " << outputFilename << endl;
215 
216  if (!tree->writeBinary(outputFilename)){
217  OCTOMAP_ERROR("Error writing tree to %s\n", outputFilename.c_str());
218  exit(1);
219  }
220 
221  cout << "done" << endl << endl;
222  delete tree;
223  return 0;
224 }
OCTOMAP_WARNING_STR
#define OCTOMAP_WARNING_STR(args)
Definition: octomap_types.h:77
octomap::AbstractOccupancyOcTree::readBinary
bool readBinary(std::istream &s)
Definition: AbstractOccupancyOcTree.cpp:135
octomath::Vector3
This class represents a three-dimensional vector.
Definition: Vector3.h:50
octomap::OcTree
Definition: OcTree.h:49
octomap::OcTreeBaseImpl::getResolution
double getResolution() const
Definition: OcTreeBaseImpl.h:109
OCTOMAP_ERROR
#define OCTOMAP_ERROR(...)
Definition: octomap_types.h:78
octomap::OcTreeBaseImpl::setResolution
void setResolution(double r)
octomap.h
main
int main(int argc, char **argv)
Definition: edit_octree.cpp:45
octomap
octomap::AbstractOccupancyOcTree::writeBinary
bool writeBinary(const std::string &filename)
Definition: AbstractOccupancyOcTree.cpp:50


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Tue Dec 12 2023 03:39:40