stat_test.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 #include <fstream>
00003 #include <math.h>
00004 #include "stat.h"
00005 
00006 using namespace std;
00007 using namespace GMapping;
00008 
00009 // struct Covariance3{
00010 //      double xx, yy, tt, xy, xt, yt;
00011 // };
00012 
00013 #define SAMPLES_NUMBER 10000
00014 
00015 int main(int argc, char**  argv){
00016         Covariance3 cov={1.,0.01,0.01,0,0,0};
00017         EigenCovariance3 ecov(cov);
00018         cout << "EigenValues: " << ecov.eval[0] << " "<< ecov.eval[1] << " "  << ecov.eval[2] << endl;
00019         
00020         cout << "EigenVectors:" <<endl;
00021         cout<< ecov.evec[0][0] << " "<< ecov.evec[0][1] << " "  << ecov.evec[0][2] << endl;
00022         cout<< ecov.evec[1][0] << " "<< ecov.evec[1][1] << " "  << ecov.evec[1][2] << endl;
00023         cout<< ecov.evec[2][0] << " "<< ecov.evec[2][1] << " "  << ecov.evec[2][2] << endl;
00024         
00025         EigenCovariance3 rcov(ecov.rotate(M_PI/4));
00026         cout << "*************** Rotated ***************" << endl;
00027         cout << "EigenValues: " << rcov.eval[0] << " "<< rcov.eval[1] << " "  << rcov.eval[2] << endl;
00028         
00029         cout << "EigenVectors:" <<endl;
00030         cout<< rcov.evec[0][0] << " "<< rcov.evec[0][1] << " "  << rcov.evec[0][2] << endl;
00031         cout<< rcov.evec[1][0] << " "<< rcov.evec[1][1] << " "  << rcov.evec[1][2] << endl;
00032         cout<< rcov.evec[2][0] << " "<< rcov.evec[2][1] << " "  << rcov.evec[2][2] << endl;
00033         
00034         cout << "sampling:" << endl;
00035         ofstream fs("stat_test.dat");
00036         std::vector<OrientedPoint> points;
00037         for (unsigned int i=0; i<SAMPLES_NUMBER; i++){
00038                 OrientedPoint op=rcov.sample();
00039                 points.push_back(op);
00040                 fs << op.x << " " <<  op.y << " " << op.theta << endl;
00041         }
00042         fs.close();
00043         std::vector<OrientedPoint>::iterator b = points.begin();
00044         std::vector<OrientedPoint>::iterator e = points.end();
00045         Gaussian3 gaussian=computeGaussianFromSamples(b, e);
00046         cov=gaussian.cov;
00047         ecov=gaussian.covariance;
00048         cout << "*************** Estimated with Templates ***************" << endl;
00049         cout << "EigenValues: " << ecov.eval[0] << " "<< ecov.eval[1] << " "  << ecov.eval[2] << endl;
00050         cout << "EigenVectors:" <<endl;
00051         cout<< ecov.evec[0][0] << " "<< ecov.evec[0][1] << " "  << ecov.evec[0][2] << endl;
00052         cout<< ecov.evec[1][0] << " "<< ecov.evec[1][1] << " "  << ecov.evec[1][2] << endl;
00053         cout<< ecov.evec[2][0] << " "<< ecov.evec[2][1] << " "  << ecov.evec[2][2] << endl;
00054         gaussian.computeFromSamples(points);
00055         ecov=gaussian.covariance;
00056         cout << "*************** Estimated without Templates ***************" << endl;
00057         cout << "EigenValues: " << ecov.eval[0] << " "<< ecov.eval[1] << " "  << ecov.eval[2] << endl;
00058         cout << "EigenVectors:" <<endl;
00059         cout<< ecov.evec[0][0] << " "<< ecov.evec[0][1] << " "  << ecov.evec[0][2] << endl;
00060         cout<< ecov.evec[1][0] << " "<< ecov.evec[1][1] << " "  << ecov.evec[1][2] << endl;
00061         cout<< ecov.evec[2][0] << " "<< ecov.evec[2][1] << " "  << ecov.evec[2][2] << endl;
00062         
00063         
00064         
00065 }
00066 


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Thu Jun 6 2019 19:25:13