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