X-lanni
59 lines
1.4 KiB
C++
59 lines
1.4 KiB
C++
#include <gmapping/utils/stat.h>
|
|
#include <gmapping/particlefilter/particlefilter.h>
|
|
#include <iostream>
|
|
#include <fstream>
|
|
#include "gfsreader.h"
|
|
|
|
using namespace std;
|
|
using namespace GMapping;
|
|
using namespace GMapping::GFSReader;
|
|
|
|
|
|
int main(int argc, char ** argv){
|
|
if (argc<2){
|
|
cout << "usage gfs2stat <infilename> <outfilename>" << endl;
|
|
return 0;
|
|
}
|
|
ifstream is(argv[1]);
|
|
if (!is){
|
|
cout << "no file found: " << argv[1] << endl;
|
|
return 0;
|
|
}
|
|
ofstream os(argv[2]);
|
|
if (!os){
|
|
cout << "cannot open file: " << argv[1] << endl;
|
|
return 0;
|
|
}
|
|
cout << "loading... "<< flush;
|
|
RecordList rl;
|
|
rl.read(is);
|
|
cout << " done" << endl;
|
|
int count=-1;
|
|
for (RecordList::const_iterator it=rl.begin(); it!=rl.end(); it++){
|
|
|
|
count++;
|
|
const ScanMatchRecord* rec=dynamic_cast<const ScanMatchRecord*>(*it);
|
|
if (!rec)
|
|
continue;
|
|
Gaussian3 gaussian;
|
|
/*
|
|
vector<double> nweights;
|
|
cout << "N"<< flush;
|
|
back_insert_iterator< vector<double> > out(nweights);
|
|
toNormalForm(out,rec->weights.begin(), rec->weights.end());
|
|
cout << "G"<< flush;
|
|
gaussian.computeFromSamples(rec->poses, nweights);
|
|
*/
|
|
gaussian.computeFromSamples(rec->poses);
|
|
cout << "E"<< flush;
|
|
os << count <<" ";
|
|
os << gaussian.mean.x <<" ";
|
|
os << gaussian.mean.y <<" ";
|
|
os << gaussian.mean.theta <<" ";
|
|
os << gaussian.covariance.eval[0] <<" ";
|
|
os << gaussian.covariance.eval[1] <<" ";
|
|
os << gaussian.covariance.eval[2] <<endl;
|
|
}
|
|
os.close();
|
|
}
|