stat_test.cpp
Go to the documentation of this file.
1 #include <iostream>
2 #include <fstream>
3 #include <math.h>
4 #include "gmapping/utils/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 
GMapping::Gaussian3::computeFromSamples
void computeFromSamples(const std::vector< OrientedPoint > &poses)
GMapping::EigenCovariance3::rotate
EigenCovariance3 rotate(double angle) const
GMapping
Definition: configfile.cpp:34
GMapping::Gaussian3
Definition: stat.h:34
GMapping::Gaussian3::covariance
EigenCovariance3 covariance
Definition: stat.h:36
GMapping::point::y
T y
Definition: point.h:16
GMapping::orientedpoint::theta
A theta
Definition: point.h:60
SAMPLES_NUMBER
#define SAMPLES_NUMBER
Definition: stat_test.cpp:13
GMapping::EigenCovariance3
Definition: stat.h:25
GMapping::computeGaussianFromSamples
Gaussian3 computeGaussianFromSamples(PointIterator &pointBegin, PointIterator &pointEnd, WeightIterator &weightBegin, WeightIterator &weightEnd)
Definition: stat.h:44
GMapping::Gaussian3::cov
Covariance3 cov
Definition: stat.h:37
GMapping::EigenCovariance3::evec
double evec[3][3]
Definition: stat.h:31
GMapping::Covariance3
Definition: graphmap.cpp:25
stat.h
main
int main(int argc, char **argv)
Definition: stat_test.cpp:15
GMapping::orientedpoint< double, double >
GMapping::EigenCovariance3::eval
double eval[3]
Definition: stat.h:30
GMapping::EigenCovariance3::sample
OrientedPoint sample() const
GMapping::point::x
T x
Definition: point.h:16


openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Thu Oct 19 2023 02:25:51