X-lanni
67 lines
2.6 KiB
C++
67 lines
2.6 KiB
C++
#include <iostream>
|
|
#include <fstream>
|
|
#include <math.h>
|
|
#include "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;
|
|
|
|
|
|
|
|
}
|
|
|