3dpcp/.svn/pristine/7d/7dee2b0f053dc73b42a1a05277f53ebf2642576d.svn-base

142 lines
3 KiB
Text
Raw Normal View History

2012-09-16 12:33:11 +00:00
/*
* frames_to_graph implementation
*
* Copyright (C) Jochen Sprickerhof
*
* Released under the GPL version 3.
*
*/
/* @file extracts the final poses from the frame files
* @author Jochen Sprickerhof
*/
#ifndef _MSC_VER
#include <getopt.h>
#else
#include "XGetopt.h"
#endif
#include <iostream>
using std::cout;
using std::cerr;
using std::endl;
#include <string>
using std::string;
#include <fstream>
using std::ifstream;
using std::ofstream;
#include <stdexcept>
using std::exception;
#include "slam6d/globals.icc"
void read_Frame(string frameFile, ofstream &out_stream)
{
double transMat[16];
int type;
double rPos[3];
double rPosTheta[3];
ifstream frame(frameFile.c_str());
while(frame) {
try {
frame >> transMat >> type;
}
catch(const exception &e) {
break;
}
}
Matrix4ToEuler(transMat, rPosTheta, rPos);
//out_stream << rPos[0] << " " << rPos[1] << " " << rPos[2] << " " << rPosTheta[0] << " " << rPosTheta[1] << " " << rPosTheta[2] << endl;
double quat[4];
Matrix4ToQuat(transMat, quat);
QuatToAA(quat);
out_stream << rPos[0] << " " << rPos[1] << " " << rPos[2] << " " << quat[0] << " " << quat[1] << " " << quat[2] << " " << quat[3] << endl;
frame.close();
}
void readFrames(string dir, int start, int end, ofstream &out_stream)
{
for(;start <= end; start++) {
read_Frame(dir + "scan" + to_string(start, 3) + ".frames", out_stream);
}
}
void usage(string progname) {
cout << "Usage: " << progname << " OPTIONS FILE" << endl <<
"\t -s \t\t first node in loop" << endl <<
"\t -e \t\t last node in loop" << endl <<
"\t -o \t\t output filename (must be given)" << endl;
}
int main(int argc, char *argv[])
{
if(argc < 2) {
usage(argv[0]);
exit(1);
}
int start = 0, end = 0;
string out_file;
/* options descriptor */
// 0: no arguments, 1: required argument, 2: optional argument
static struct option longopts[] = {
{ "end", required_argument, 0, 'e' },
{ "out", required_argument, 0, 'o' },
{ "start", required_argument, 0, 's' },
{ 0, 0, 0, 0} // needed, cf. getopt.h
};
int c;
while((c = getopt_long(argc, argv, "e:o:s:", longopts, NULL)) != -1) {
switch (c) {
case 'e':
end = atoi(optarg);
if(end < 0) {
cerr << "Error: Cannot end at a negative number." << endl;
exit(1);
}
break;
case 'o':
out_file = optarg;
break;
case 's':
start = atoi(optarg);
if(start < 0) {
cerr << "Error: Cannot start at a negative number." << endl;
exit(1);
}
break;
case '?':
default:
exit(1);
}
}
if(out_file.empty() || end == 0) {
usage(argv[0]);
exit(1);
}
string dir = argv[optind];
#ifndef _MSC_VER
if (dir[dir.length()-1] != '/') dir = dir + "/";
#else
if (dir[dir.length()-1] != '\\') dir = dir + "\\";
#endif
ofstream out_stream(out_file.c_str());
readFrames(dir, start, end, out_stream);
out_stream.close();
exit(0);
}