gfs2stat.cpp

Go to the documentation of this file.

#include <gmapping/utils/stat.h>
#include <gmapping/particlefilter/particlefilter.h>
#include <iostream>
#include <fstream>
#include "gmapping/gridfastslam/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();
}