3dpcp/.svn/pristine/70/70fc0090876d8f47232de9bc22051c477cc7e569.svn-base

211 lines
5 KiB
Text
Raw Normal View History

2012-09-16 12:33:11 +00:00
/*
* match_with_ground_truth implementation
*
* Copyright (C) Andreas Nuechter, Kai Lingemann, Jochen Sprickerhof
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief Main programm for matching registered 3D scans against a
* ground_truth_path
*
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#include <string>
using std::string;
#include <iostream>
using std::cout;
using std::cerr;
using std::endl;
#include <fstream>
using std::ifstream;
using std::ofstream;
#include <vector>
using std::vector;
#include <stdexcept>
using std::exception;
#include "globals.icc"
#include "ptpair.h"
#include "icp6Dapx.h"
#include "icp6Dsvd.h"
#include "icp6Dquat.h"
#ifndef _MSC_VER
#include <getopt.h>
#else
#include "..\Visual_Studio_Projects\6D_SLAM\XGetopt.h"
#endif
/**
* Explains the usage of this program's command line parameters
*/
void usage(char* prog)
{
const string bold("\033[1m");
const string normal("\033[m");
cout << "to do" << endl;
exit(1);
}
int parseArgs(int argc, char **argv, string &dir, int &mni,
int &start, int &end, int &algo, bool &quiet, bool &veryQuiet)
{
int c;
// from unistd.h:
extern char *optarg;
extern int optind;
/* options descriptor */
// 0: no arguments, 1: required argument, 2: optional argument
static struct option longopts[] = {
{ "algo", required_argument, 0, 'a' },
{ "iter", required_argument, 0, 'i' },
{ "start", required_argument, 0, 's' },
{ "end", required_argument, 0, 'e' },
{ 0, 0, 0, 0} // needed, cf. getopt.h
};
cout << endl;
while ((c = getopt_long(argc, argv, "a:i:s:e:qQ", longopts, NULL)) != -1) {
switch (c)
{
case 'a':
algo = atoi(optarg);
if ((algo < 0) || (algo > 2)) { cerr << "Error: Algorithm not available.\n"; exit(1); }
break;
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;
case 'i':
mni = atoi(optarg);
break;
case 'q':
quiet = true;
break;
case 'Q':
quiet = veryQuiet = true;
break;
default:
abort();
}
}
if (optind != argc-1) {
cerr << "\n*** Directory missing ***" << endl;
usage(argv[0]);
}
dir = argv[optind];
}
int main(int argc, char **argv)
{
if (argc <= 1) {
usage(argv[0]);
}
string dir;
int mni = 50;
int start = 0, end = -1;
bool quiet = false;
bool veryQuiet = false;
int algo = 0;
parseArgs(argc, argv, dir, mni, start, end, algo, quiet, veryQuiet);
string groundTruthFileName = dir + "position_sync_interpol.dat";
ifstream gtruth_in(groundTruthFileName.c_str());
if (!gtruth_in.good()) {
cerr << "Cannot open " << groundTruthFileName << endl;
exit(-1);
}
vector<PtPair> pairs;
for (int i = 0; i < end; i++) {
// read position from ground truth file
double gtruthPos[3];
double dummy;
gtruth_in >> dummy >> gtruthPos[2] >> gtruthPos[0] >> gtruthPos[1]
>> dummy >> dummy >> dummy;
if (i < start) continue;
for (unsigned int j = 0; j < 3; j++) gtruthPos[j] *= 0.1;
string frameFileName = dir + "scan" + to_string(i,3) + ".frames";
cout << "Processing File " << frameFileName << endl;
ifstream frame_in(frameFileName.c_str());
if (!frame_in.good()) {
cerr << "Cannot open " << frameFileName << endl;
exit(-1);
}
double transMat[16];
int type;
while (frame_in.good()) {
try {
frame_in >> transMat >> type;
} catch (const exception &e) {
break;
}
}
// now transMat contains the last matrix
double rPos[3], rPosTheta[3];
Matrix4ToEuler(transMat, rPosTheta, rPos);
PtPair myPair(gtruthPos, rPos);
pairs.push_back(myPair);
}
cout << "Nr poses used for registration: " << pairs.size() << endl
<< algo << endl;
icp6Dminimizer *my_icp6Dminimizer = 0;
switch (algo) {
case 0 :
my_icp6Dminimizer = new icp6D_APX(quiet);
break;
case 1 :
my_icp6Dminimizer = new icp6D_QUAT(quiet);
break;
case 2 :
my_icp6Dminimizer = new icp6D_SVD(quiet);
break;
}
double totalalignxf[16];
M4identity(totalalignxf);
for (int iteration = 0; iteration < mni; iteration++) {
double alignxf[16];
my_icp6Dminimizer->Point_Point_Align(pairs, alignxf);
for (int i = 0; i < pairs.size(); i++) {
pairs[i].p2.transform(alignxf);
}
double tempxf[16];
MMult(alignxf, totalalignxf, tempxf);
memcpy(totalalignxf, tempxf, sizeof(totalalignxf));
}
string initialTransformFileName = dir + "initital.frame";
ofstream initial_out(initialTransformFileName.c_str());
initial_out << totalalignxf << endl;
initial_out.close();
}