gfs2stat.cpp
Go to the documentation of this file.
00001 #include <gmapping/utils/stat.h>
00002 #include <gmapping/particlefilter/particlefilter.h>
00003 #include <iostream>
00004 #include <fstream>
00005 #include "gfsreader.h"
00006 
00007 using namespace std;
00008 using namespace GMapping;
00009 using namespace GMapping::GFSReader;
00010 
00011 
00012 int main(int argc, char ** argv){
00013         if (argc<2){
00014                 cout << "usage gfs2stat <infilename> <outfilename>" << endl;
00015                 return 0;
00016         }
00017         ifstream is(argv[1]);
00018         if (!is){
00019                 cout << "no file found: " << argv[1] << endl;
00020                 return 0;
00021         }
00022         ofstream os(argv[2]);
00023         if (!os){
00024                 cout << "cannot open file: " << argv[1] << endl;
00025                 return 0;
00026         }
00027         cout << "loading... "<< flush;
00028         RecordList rl;
00029         rl.read(is);
00030         cout << " done" << endl;
00031         int count=-1;
00032         for (RecordList::const_iterator it=rl.begin(); it!=rl.end(); it++){
00033                 
00034                 count++;
00035                 const ScanMatchRecord* rec=dynamic_cast<const ScanMatchRecord*>(*it);
00036                 if (!rec)
00037                         continue;
00038                 Gaussian3 gaussian;
00039                 /*      
00040                 vector<double> nweights;
00041                 cout << "N"<< flush;
00042                 back_insert_iterator< vector<double> > out(nweights);
00043                 toNormalForm(out,rec->weights.begin(), rec->weights.end());
00044                 cout << "G"<< flush;
00045                 gaussian.computeFromSamples(rec->poses, nweights);
00046                 */
00047                 gaussian.computeFromSamples(rec->poses);
00048                 cout << "E"<< flush;
00049                 os << count <<" ";
00050                 os << gaussian.mean.x <<" ";
00051                 os << gaussian.mean.y <<" ";
00052                 os << gaussian.mean.theta <<" ";
00053                 os << gaussian.covariance.eval[0] <<" ";
00054                 os << gaussian.covariance.eval[1] <<" ";
00055                 os << gaussian.covariance.eval[2] <<endl;
00056         }
00057         os.close();
00058 }


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Fri Aug 28 2015 11:56:21