stat_test.cpp

Go to the documentation of this file.

#include <iostream>
#include <fstream>
#include <math.h>
#include "gmapping/utils/stat.h"
 
using namespace std;
using namespace GMapping;
 
// struct Covariance3{
//      double xx, yy, tt, xy, xt, yt;
// };
 
#define SAMPLES_NUMBER 10000
 
int main(int argc, char**  argv){
        Covariance3 cov={1.,0.01,0.01,0,0,0};
        EigenCovariance3 ecov(cov);
        cout << "EigenValues: " << ecov.eval[0] << " "<< ecov.eval[1] << " "  << ecov.eval[2] << endl;
        
        cout << "EigenVectors:" <<endl;
        cout<< ecov.evec[0][0] << " "<< ecov.evec[0][1] << " "  << ecov.evec[0][2] << endl;
        cout<< ecov.evec[1][0] << " "<< ecov.evec[1][1] << " "  << ecov.evec[1][2] << endl;
        cout<< ecov.evec[2][0] << " "<< ecov.evec[2][1] << " "  << ecov.evec[2][2] << endl;
        
        EigenCovariance3 rcov(ecov.rotate(M_PI/4));
        cout << "*************** Rotated ***************" << endl;
        cout << "EigenValues: " << rcov.eval[0] << " "<< rcov.eval[1] << " "  << rcov.eval[2] << endl;
        
        cout << "EigenVectors:" <<endl;
        cout<< rcov.evec[0][0] << " "<< rcov.evec[0][1] << " "  << rcov.evec[0][2] << endl;
        cout<< rcov.evec[1][0] << " "<< rcov.evec[1][1] << " "  << rcov.evec[1][2] << endl;
        cout<< rcov.evec[2][0] << " "<< rcov.evec[2][1] << " "  << rcov.evec[2][2] << endl;
        
        cout << "sampling:" << endl;
        ofstream fs("stat_test.dat");
        std::vector<OrientedPoint> points;
        for (unsigned int i=0; i<SAMPLES_NUMBER; i++){
                OrientedPoint op=rcov.sample();
                points.push_back(op);
                fs << op.x << " " <<  op.y << " " << op.theta << endl;
        }
        fs.close();
        std::vector<OrientedPoint>::iterator b = points.begin();
        std::vector<OrientedPoint>::iterator e = points.end();
        Gaussian3 gaussian=computeGaussianFromSamples(b, e);
        cov=gaussian.cov;
        ecov=gaussian.covariance;
        cout << "*************** Estimated with Templates ***************" << endl;
        cout << "EigenValues: " << ecov.eval[0] << " "<< ecov.eval[1] << " "  << ecov.eval[2] << endl;
        cout << "EigenVectors:" <<endl;
        cout<< ecov.evec[0][0] << " "<< ecov.evec[0][1] << " "  << ecov.evec[0][2] << endl;
        cout<< ecov.evec[1][0] << " "<< ecov.evec[1][1] << " "  << ecov.evec[1][2] << endl;
        cout<< ecov.evec[2][0] << " "<< ecov.evec[2][1] << " "  << ecov.evec[2][2] << endl;
        gaussian.computeFromSamples(points);
        ecov=gaussian.covariance;
        cout << "*************** Estimated without Templates ***************" << endl;
        cout << "EigenValues: " << ecov.eval[0] << " "<< ecov.eval[1] << " "  << ecov.eval[2] << endl;
        cout << "EigenVectors:" <<endl;
        cout<< ecov.evec[0][0] << " "<< ecov.evec[0][1] << " "  << ecov.evec[0][2] << endl;
        cout<< ecov.evec[1][0] << " "<< ecov.evec[1][1] << " "  << ecov.evec[1][2] << endl;
        cout<< ecov.evec[2][0] << " "<< ecov.evec[2][1] << " "  << ecov.evec[2][2] << endl;
        
        
        
}