binvox2bt.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 
45 #ifndef M_PI_2
46  #define M_PI_2 1.5707963267948966192E0
47 #endif
48 
49 #include <string>
50 #include <fstream>
51 #include <iostream>
52 #include <octomap/octomap.h>
53 #include <cstdlib>
54 #include <cstring>
55 
56 using namespace std;
57 using namespace octomap;
58 
59 typedef unsigned char byte;
60 
61 int main(int argc, char **argv)
62 {
63  int version; // binvox file format version (should be 1)
64  int depth, height, width; // dimensions of the voxel grid
65  int size; // number of grid cells (height * width * depth)
66  float tx, ty, tz; // Translation
67  float scale; // Scaling factor
68  bool mark_free = false; // Mark free cells (false = cells remain "unknown")
69  bool rotate = false; // Fix orientation of webots-exported files
70  bool show_help = false;
71  string output_filename;
72  double minX = 0.0;
73  double minY = 0.0;
74  double minZ = 0.0;
75  double maxX = 0.0;
76  double maxY = 0.0;
77  double maxZ = 0.0;
78  bool applyBBX = false;
79  bool applyOffset = false;
80  octomap::point3d offset(0.0, 0.0, 0.0);
81  OcTree *tree = 0;
82 
83  if(argc == 1) show_help = true;
84  for(int i = 1; i < argc && !show_help; i++) {
85  if(strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-help") == 0 ||
86  strcmp(argv[i], "--usage") == 0 || strcmp(argv[i], "-usage") == 0 ||
87  strcmp(argv[i], "-h") == 0
88  )
89  show_help = true;
90  }
91 
92  if(show_help) {
93  cout << "Usage: "<<argv[0]<<" [OPTIONS] <binvox filenames>" << endl;
94  cout << "\tOPTIONS:" << endl;
95  cout << "\t -o <file> Output filename (default: first input filename + .bt)\n";
96  cout << "\t --mark-free Mark not occupied cells as 'free' (default: unknown)\n";
97  cout << "\t --rotate Rotate left by 90 deg. to fix the coordinate system when exported from Webots\n";
98  cout << "\t --bb <minx> <miny> <minz> <maxx> <maxy> <maxz>: force bounding box for OcTree\n";
99  cout << "\t --offset <x> <y> <z>: add an offset to the final coordinates\n";
100  cout << "If more than one binvox file is given, the models are composed to a single bonsai tree->\n";
101  cout << "All options apply to the subsequent input files.\n\n";
102  exit(0);
103  }
104 
105  for(int i = 1; i < argc; i++) {
106  // Parse command line arguments
107  if(strcmp(argv[i], "--mark-free") == 0) {
108  mark_free = true;
109  continue;
110  } else if(strcmp(argv[i], "--no-mark-free") == 0) {
111  mark_free = false;
112  continue;
113  }else if(strcmp(argv[i], "--rotate") == 0) {
114  rotate = true;
115  continue;
116  } else if(strcmp(argv[i], "-o") == 0 && i < argc - 1) {
117  i++;
118  output_filename = argv[i];
119 
120  continue;
121  } else if (strcmp(argv[i], "--bb") == 0 && i < argc - 7) {
122  i++;
123  minX = atof(argv[i]);
124  i++;
125  minY = atof(argv[i]);
126  i++;
127  minZ = atof(argv[i]);
128  i++;
129  maxX = atof(argv[i]);
130  i++;
131  maxY = atof(argv[i]);
132  i++;
133  maxZ = atof(argv[i]);
134 
135  applyBBX = true;
136 
137  continue;
138  } else if (strcmp(argv[i], "--offset") == 0 && i < argc - 4) {
139  i++;
140  offset(0) = (float) atof(argv[i]);
141  i++;
142  offset(1) = (float) atof(argv[i]);
143  i++;
144  offset(2) = (float) atof(argv[i]);
145 
146  applyOffset = true;
147 
148  continue;
149  }
150 
151 
152  // Open input file
153  ifstream *input = new ifstream(argv[i], ios::in | ios::binary);
154  if(!input->good()) {
155  cerr << "Error: Could not open input file " << argv[i] << "!" << endl;
156  exit(1);
157  } else {
158  cout << "Reading binvox file " << argv[i] << "." << endl;
159  if(output_filename.empty()) {
160  output_filename = string(argv[i]).append(".bt");
161  }
162  }
163 
164  // read header
165  string line;
166  *input >> line; // #binvox
167  if (line.compare("#binvox") != 0) {
168  cout << "Error: first line reads [" << line << "] instead of [#binvox]" << endl;
169  delete input;
170  return 0;
171  }
172  *input >> version;
173  cout << "reading binvox version " << version << endl;
174 
175  depth = -1;
176  int done = 0;
177  while(input->good() && !done) {
178  *input >> line;
179  if (line.compare("data") == 0) done = 1;
180  else if (line.compare("dim") == 0) {
181  *input >> depth >> height >> width;
182  }
183  else if (line.compare("translate") == 0) {
184  *input >> tx >> ty >> tz;
185  }
186  else if (line.compare("scale") == 0) {
187  *input >> scale;
188  }
189  else {
190  cout << " unrecognized keyword [" << line << "], skipping" << endl;
191  char c;
192  do { // skip until end of line
193  c = input->get();
194  } while(input->good() && (c != '\n'));
195 
196  }
197  }
198  if (!done) {
199  cout << " error reading header" << endl;
200  return 0;
201  }
202  if (depth == -1) {
203  cout << " missing dimensions in header" << endl;
204  return 0;
205  }
206 
207  size = width * height * depth;
208  int maxSide = std::max(std::max(width, height), depth);
209  double res = double(scale)/double(maxSide);
210 
211  if(!tree) {
212  cout << "Generating octree with leaf size " << res << endl << endl;
213  tree = new OcTree(res);
214  }
215 
216  if (applyBBX){
217  cout << "Bounding box for Octree: [" << minX << ","<< minY << "," << minZ << " - "
218  << maxX << ","<< maxY << "," << maxZ << "]\n";
219 
220  }
221  if (applyOffset){
222  std::cout << "Offset on final map: "<< offset << std::endl;
223 
224  }
225 
226  cout << "Read data: ";
227  cout.flush();
228 
229  // read voxel data
230  byte value;
231  byte count;
232  int index = 0;
233  int end_index = 0;
234  unsigned nr_voxels = 0;
235  unsigned nr_voxels_out = 0;
236 
237  input->unsetf(ios::skipws); // need to read every byte now (!)
238  *input >> value; // read the linefeed char
239 
240  while((end_index < size) && input->good()) {
241  *input >> value >> count;
242 
243  if (input->good()) {
244  end_index = index + count;
245  if (end_index > size) return 0;
246  for(int i=index; i < end_index; i++) {
247  // Output progress dots
248  if(i % (size / 20) == 0) {
249  cout << ".";
250  cout.flush();
251  }
252  // voxel index --> voxel coordinates
253  int y = i % width;
254  int z = (i / width) % height;
255  int x = i / (width * height);
256 
257  // voxel coordinates --> world coordinates
258  point3d endpoint((float) ((double) x*res + tx + 0.000001),
259  (float) ((double) y*res + ty + 0.000001),
260  (float) ((double) z*res + tz + 0.000001));
261 
262  if(rotate) {
263  endpoint.rotate_IP(M_PI_2, 0.0, 0.0);
264  }
265  if (applyOffset)
266  endpoint += offset;
267 
268  if (!applyBBX || (endpoint(0) <= maxX && endpoint(0) >= minX
269  && endpoint(1) <= maxY && endpoint(1) >= minY
270  && endpoint(2) <= maxZ && endpoint(2) >= minZ)){
271 
272  // mark cell in octree as free or occupied
273  if(mark_free || value == 1) {
274  tree->updateNode(endpoint, value == 1, true);
275  }
276  } else{
277  nr_voxels_out ++;
278  }
279  }
280 
281  if (value) nr_voxels += count;
282  index = end_index;
283  } // if file still ok
284 
285  } // while
286 
287  cout << endl << endl;
288 
289  input->close();
290  cout << " read " << nr_voxels << " voxels, skipped "<<nr_voxels_out << " (out of bounding box)\n\n";
291  }
292 
293  // prune octree
294  cout << "Pruning octree" << endl << endl;
295  tree->updateInnerOccupancy();
296  tree->prune();
297 
298  // write octree to file
299  if(output_filename.empty()) {
300  cerr << "Error: No input files found." << endl << endl;
301  exit(1);
302  }
303 
304  cout << "Writing octree to " << output_filename << endl << endl;
305 
306  tree->writeBinary(output_filename.c_str());
307 
308  cout << "done" << endl << endl;
309  return 0;
310 }
int main(int argc, char **argv)
Definition: binvox2bt.cpp:61
virtual NODE * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
Vector3 & rotate_IP(double roll, double pitch, double yaw)
Definition: Vector3.cpp:41
#define M_PI_2
Definition: binvox2bt.cpp:46
bool writeBinary(const std::string &filename)
This class represents a three-dimensional vector.
Definition: Vector3.h:50
unsigned char byte
Definition: binvox2bt.cpp:59
virtual void prune()


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Mon Feb 28 2022 22:58:06