stat_test.cpp
Go to the documentation of this file.
1 #include <iostream>
2 #include <fstream>
3 #include <math.h>
4 #include "stat.h"
5 
6 using namespace std;
7 using namespace GMapping;
8 
9 // struct Covariance3{
10 // double xx, yy, tt, xy, xt, yt;
11 // };
12 
13 #define SAMPLES_NUMBER 10000
14 
15 int main(int argc, char** argv){
16  Covariance3 cov={1.,0.01,0.01,0,0,0};
17  EigenCovariance3 ecov(cov);
18  cout << "EigenValues: " << ecov.eval[0] << " "<< ecov.eval[1] << " " << ecov.eval[2] << endl;
19 
20  cout << "EigenVectors:" <<endl;
21  cout<< ecov.evec[0][0] << " "<< ecov.evec[0][1] << " " << ecov.evec[0][2] << endl;
22  cout<< ecov.evec[1][0] << " "<< ecov.evec[1][1] << " " << ecov.evec[1][2] << endl;
23  cout<< ecov.evec[2][0] << " "<< ecov.evec[2][1] << " " << ecov.evec[2][2] << endl;
24 
25  EigenCovariance3 rcov(ecov.rotate(M_PI/4));
26  cout << "*************** Rotated ***************" << endl;
27  cout << "EigenValues: " << rcov.eval[0] << " "<< rcov.eval[1] << " " << rcov.eval[2] << endl;
28 
29  cout << "EigenVectors:" <<endl;
30  cout<< rcov.evec[0][0] << " "<< rcov.evec[0][1] << " " << rcov.evec[0][2] << endl;
31  cout<< rcov.evec[1][0] << " "<< rcov.evec[1][1] << " " << rcov.evec[1][2] << endl;
32  cout<< rcov.evec[2][0] << " "<< rcov.evec[2][1] << " " << rcov.evec[2][2] << endl;
33 
34  cout << "sampling:" << endl;
35  ofstream fs("stat_test.dat");
36  std::vector<OrientedPoint> points;
37  for (unsigned int i=0; i<SAMPLES_NUMBER; i++){
38  OrientedPoint op=rcov.sample();
39  points.push_back(op);
40  fs << op.x << " " << op.y << " " << op.theta << endl;
41  }
42  fs.close();
43  std::vector<OrientedPoint>::iterator b = points.begin();
44  std::vector<OrientedPoint>::iterator e = points.end();
46  cov=gaussian.cov;
47  ecov=gaussian.covariance;
48  cout << "*************** Estimated with Templates ***************" << endl;
49  cout << "EigenValues: " << ecov.eval[0] << " "<< ecov.eval[1] << " " << ecov.eval[2] << endl;
50  cout << "EigenVectors:" <<endl;
51  cout<< ecov.evec[0][0] << " "<< ecov.evec[0][1] << " " << ecov.evec[0][2] << endl;
52  cout<< ecov.evec[1][0] << " "<< ecov.evec[1][1] << " " << ecov.evec[1][2] << endl;
53  cout<< ecov.evec[2][0] << " "<< ecov.evec[2][1] << " " << ecov.evec[2][2] << endl;
54  gaussian.computeFromSamples(points);
55  ecov=gaussian.covariance;
56  cout << "*************** Estimated without Templates ***************" << endl;
57  cout << "EigenValues: " << ecov.eval[0] << " "<< ecov.eval[1] << " " << ecov.eval[2] << endl;
58  cout << "EigenVectors:" <<endl;
59  cout<< ecov.evec[0][0] << " "<< ecov.evec[0][1] << " " << ecov.evec[0][2] << endl;
60  cout<< ecov.evec[1][0] << " "<< ecov.evec[1][1] << " " << ecov.evec[1][2] << endl;
61  cout<< ecov.evec[2][0] << " "<< ecov.evec[2][1] << " " << ecov.evec[2][2] << endl;
62 
63 
64 
65 }
66 
EigenCovariance3 rotate(double angle) const
Gaussian3 computeGaussianFromSamples(PointIterator &pointBegin, PointIterator &pointEnd, WeightIterator &weightBegin, WeightIterator &weightEnd)
Definition: stat.h:43
Covariance3 cov
Definition: stat.h:36
#define SAMPLES_NUMBER
Definition: stat_test.cpp:13
void computeFromSamples(const std::vector< OrientedPoint > &poses)
double evec[3][3]
Definition: stat.h:30
EigenCovariance3 covariance
Definition: stat.h:35
int main(int argc, char **argv)
Definition: stat_test.cpp:15


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Mon Jun 10 2019 14:04:22