46 #define M_PI_2 1.5707963267948966192E0 59 typedef unsigned char byte;
61 int main(
int argc,
char **argv)
64 int depth, height, width;
68 bool mark_free =
false;
70 bool show_help =
false;
71 string output_filename;
78 bool applyBBX =
false;
79 bool applyOffset =
false;
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
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";
105 for(
int i = 1; i < argc; i++) {
107 if(strcmp(argv[i],
"--mark-free") == 0) {
110 }
else if(strcmp(argv[i],
"--no-mark-free") == 0) {
113 }
else if(strcmp(argv[i],
"--rotate") == 0) {
116 }
else if(strcmp(argv[i],
"-o") == 0 && i < argc - 1) {
118 output_filename = argv[i];
121 }
else if (strcmp(argv[i],
"--bb") == 0 && i < argc - 7) {
123 minX = atof(argv[i]);
125 minY = atof(argv[i]);
127 minZ = atof(argv[i]);
129 maxX = atof(argv[i]);
131 maxY = atof(argv[i]);
133 maxZ = atof(argv[i]);
138 }
else if (strcmp(argv[i],
"--offset") == 0 && i < argc - 4) {
140 offset(0) = (float) atof(argv[i]);
142 offset(1) = (float) atof(argv[i]);
144 offset(2) = (float) atof(argv[i]);
153 ifstream *input =
new ifstream(argv[i], ios::in | ios::binary);
155 cerr <<
"Error: Could not open input file " << argv[i] <<
"!" << endl;
158 cout <<
"Reading binvox file " << argv[i] <<
"." << endl;
159 if(output_filename.empty()) {
160 output_filename = string(argv[i]).append(
".bt");
167 if (line.compare(
"#binvox") != 0) {
168 cout <<
"Error: first line reads [" << line <<
"] instead of [#binvox]" << endl;
173 cout <<
"reading binvox version " << version << endl;
177 while(input->good() && !done) {
179 if (line.compare(
"data") == 0) done = 1;
180 else if (line.compare(
"dim") == 0) {
181 *input >> depth >> height >> width;
183 else if (line.compare(
"translate") == 0) {
184 *input >> tx >> ty >> tz;
186 else if (line.compare(
"scale") == 0) {
190 cout <<
" unrecognized keyword [" << line <<
"], skipping" << endl;
194 }
while(input->good() && (c !=
'\n'));
199 cout <<
" error reading header" << endl;
203 cout <<
" missing dimensions in header" << endl;
207 size = width * height * depth;
208 int maxSide = std::max(std::max(width, height), depth);
209 double res = double(scale)/double(maxSide);
212 cout <<
"Generating octree with leaf size " << res << endl << endl;
217 cout <<
"Bounding box for Octree: [" << minX <<
","<< minY <<
"," << minZ <<
" - " 218 << maxX <<
","<< maxY <<
"," << maxZ <<
"]\n";
222 std::cout <<
"Offset on final map: "<< offset << std::endl;
226 cout <<
"Read data: ";
234 unsigned nr_voxels = 0;
235 unsigned nr_voxels_out = 0;
237 input->unsetf(ios::skipws);
240 while((end_index < size) && input->good()) {
241 *input >> value >> count;
244 end_index = index + count;
245 if (end_index > size)
return 0;
246 for(
int i=index; i < end_index; i++) {
248 if(i % (size / 20) == 0) {
254 int z = (i / width) % height;
255 int x = i / (width * height);
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));
268 if (!applyBBX || (endpoint(0) <= maxX && endpoint(0) >= minX
269 && endpoint(1) <= maxY && endpoint(1) >= minY
270 && endpoint(2) <= maxZ && endpoint(2) >= minZ)){
273 if(mark_free || value == 1) {
281 if (value) nr_voxels += count;
287 cout << endl << endl;
290 cout <<
" read " << nr_voxels <<
" voxels, skipped "<<nr_voxels_out <<
" (out of bounding box)\n\n";
294 cout <<
"Pruning octree" << endl << endl;
299 if(output_filename.empty()) {
300 cerr <<
"Error: No input files found." << endl << endl;
304 cout <<
"Writing octree to " << output_filename << endl << endl;
308 cout <<
"done" << endl << endl;
void updateInnerOccupancy()
int main(int argc, char **argv)
virtual NODE * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
Vector3 & rotate_IP(double roll, double pitch, double yaw)
bool writeBinary(const std::string &filename)
This class represents a three-dimensional vector.