3dpcp/.svn/pristine/c1/c11dbf818969f3d9a0181fdff445e1945d052d61.svn-base
2012-09-16 14:33:11 +02:00

149 lines
3.4 KiB
Text

/*
* toGlobal implementation
*
* Copyright (C) Jan Elseberg, Dorit Borrmann
*
* Released under the GPL version 3.
*
*/
#include <fstream>
#include <iostream>
using std::cout;
using std::cerr;
using std::endl;
using std::ifstream;
using std::ofstream;
#include "slam6d/point.h"
#include "slam6d/globals.icc"
#include <string.h>
#include <exception>
using std::exception;
#ifndef _MSC_VER
#include <unistd.h>
#endif
#ifdef _MSC_VER
#include "XGetopt.h"
#else
#include <getopt.h>
#endif
#if WIN32
#define snprintf sprintf_s
#endif
int parseArgs(int argc,char **argv, char dir[255], int& start, int& end){
start = 0;
end = -1; // -1 indicates no limitation
int c;
// from unistd.h
extern char *optarg;
extern int optind;
cout << endl;
while ((c = getopt (argc, argv, "s:e:")) != -1)
switch (c)
{
case 's':
start = atoi(optarg);
if (start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); }
break;
case 'e':
end = atoi(optarg);
if (end < 0) { cerr << "Error: Cannot end at a negative scan number.\n"; exit(1); }
if (end < start) { cerr << "Error: <end> cannot be smaller than <start>.\n"; exit(1); }
break;
}
if (optind != argc-1) {
cerr << "\n*** Directory missing ***" << endl;
exit(0);
}
strncpy(dir,argv[optind],255);
#ifndef _MSC_VER
if (dir[strlen(dir)-1] != '/') strcat(dir,"/");
#else
if (dir[strlen(dir)-1] != '\\') strcat(dir,"\\");
#endif
return 0;
}
int main(int argc, char **argv)
{
int start = 0, end = -1;
char dir[255];
parseArgs(argc, argv, dir, start, end);
int fileCounter = start;
char frameFileName[255];
char scanFileName[255];
ifstream frame_in;
ifstream scan_in;
for (;;) {
if (end > -1 && fileCounter > end) break; // 'nuf read
snprintf(frameFileName,255,"%sscan%.3d.frames",dir,fileCounter);
snprintf(scanFileName,255,"%sscan%.3d.3d",dir,fileCounter++);
scan_in.open(scanFileName);
frame_in.open(frameFileName);
double tmp;
// read 3D scan
if (!frame_in.good()) break; // no more files in the directory
cerr << "Reading frame " << frameFileName << "..." << endl;
int frameCounter = 0;
double transMat[16];
//double transMatOpenGL[16];
while (frame_in.good()) {
frameCounter++;
try {
frame_in >> transMat;
frame_in >> tmp;
} catch (const exception &e) {
break;
}
}
Point p;
// double range, theta, phi, reflectance;
cout.precision(10);
while(scan_in.good()) {
/* scan_in >> p.z >> p.x >> p.y >> range >> theta >> phi >> reflectance;
*/ //scan_in >> p.z >> p.x >> p.y >> range >> theta >> phi >> reflectance;
scan_in >> p.x >> p.y >> p.z; // >> p.reflectance;
/* p.x *= -100;
p.y *= 100;
p.z *= 100;
*/
p.transform(transMat);
//Matrix4ToEuler(transMat,rPosTheta,rPos);
//cout << p.y << " " << p.z << " " << -p.x << " " << reflectance << endl;
//cout << p.y << " " << p.z << " " << -p.x << endl;
//cout << p.x*0.01 << " " << p.z*0.01 << " " << p.y*0.01 << " " << p.reflectance << endl;
//cout << (int)p.x << " " << (int)p.y << " " << (int)p.z << endl;// << " " << p.reflectance << endl;
cout << p.x << " " << p.y << " " << p.z << endl;// << " " << p.reflectance << endl;
}
scan_in.close();
scan_in.clear();
frame_in.close();
frame_in.clear();
cerr << " done." << endl;
}
}