covariance_ellipsoids.cpp
Go to the documentation of this file.
1 
8 /*****************************************************************************
9 ** Includes
10 *****************************************************************************/
11 
12 #include <iostream>
13 #include <gtest/gtest.h>
14 #include <ecl/linear_algebra.hpp>
15 #include "../../include/ecl/statistics/covariance_ellipsoid.hpp"
16 
17 /*****************************************************************************
18 ** Using
19 *****************************************************************************/
20 
23 using ecl::linear_algebra::Matrix2d;
24 using ecl::linear_algebra::Vector2d;
25 using ecl::linear_algebra::Matrix3d;
26 using ecl::linear_algebra::Vector3d;
27 
28 /*****************************************************************************
29 ** Tests
30 *****************************************************************************/
31 
32 TEST(CovarianceTests,ellipsoid2D) {
33  Matrix2d M;
34  M << 3.0, 1.0, 1.0, 5.0; // must be a positive definite, symmetric matrix
35 // M << 0.001801, -0.000047, -0.000047, 0.000259;
36 // M << 0.0000009661, 0.0000065, 0.0000065, 0.0001261;
37 
38  CovarianceEllipsoid2d ellipse(M);
39  const Vector2d& lengths = ellipse.lengths();
40  double eigen_value_0 = lengths[0]*lengths[0];
41  double angle = ellipse.rotation();
42  const Vector2d& intercepts = ellipse.intercepts();
43  const Matrix2d& axes = ellipse.axes();
44 
45  EXPECT_GT(2.328,lengths[0]); EXPECT_LT(2.326,lengths[0]);
46  EXPECT_GT(5.415,eigen_value_0); EXPECT_LT(5.413,eigen_value_0);
47  EXPECT_GT(1.179,angle); EXPECT_LT(1.177,angle);
48  EXPECT_GT(1.674,intercepts[0]); EXPECT_LT(1.672,intercepts[0]);
49  EXPECT_GT(0.3828,axes(0,0)); EXPECT_LT(0.3826,axes(0,0));
50 
51 // std::cout << std::endl;
52 // std::cout << "***********************************************************" << std::endl;
53 // std::cout << " Covariance Ellipsoid2d" << std::endl;
54 // std::cout << "***********************************************************" << std::endl;
55 // std::cout << std::endl;
56 // std::cout << "Test Matrix: " << std::endl;
57 // std::cout << std::endl;
58 // std::cout << M << std::endl;
59 // std::cout << std::endl;
60 // std::cout << "Axis Lengths: " << ellipse.lengths().transpose() << std::endl;
61 // std::cout << "Eigen Values: " << ellipse.lengths()(0)*ellipse.lengths()(0) << " " << ellipse.lengths()(1)*ellipse.lengths()(1) << std::endl;
62 // std::cout << "Axis Vectors: " << std::endl;
63 // std::cout << "Rotation angle: " << ellipse.rotation() << std::endl;
64 // std::cout << "Intercepts: " << ellipse.intercepts().transpose() << std::endl;
65 // std::cout << "Eigenvector Dot Product: " << ellipse.axes().block(0,0,2,1).transpose()*ellipse.axes().block(0,1,2,1) << std::endl;
66 
67 }
68 
69 TEST(CovarianceTests,ellipsoid3D) {
70  Matrix3d P;
71  double sigmaX(0.1);
72  double sigmaY(0.5);
73  double sigmaT(0.3);
74  P << sigmaX*sigmaX, 0, 0, 0, sigmaY*sigmaY, 0, 0, 0, sigmaT*sigmaT;
75  CovarianceEllipsoid3d ellipse(P);
76 
77  const Vector3d& lengths = ellipse.lengths();
78  double eigen_value_0 = lengths[0]*lengths[0];
79  const Matrix3d& axes = ellipse.axes();
80  std::cout << axes << std::endl;
81  EXPECT_GT(0.101,lengths[0]); EXPECT_LT(0.099,lengths[0]);
82  EXPECT_GT(0.011,eigen_value_0); EXPECT_LT(0.009,eigen_value_0);
83  EXPECT_GT(1.001,axes(0,0)); EXPECT_LT(0.999,axes(0,0));
84 
85 // std::cout << std::endl;
86 // std::cout << "***********************************************************" << std::endl;
87 // std::cout << " Covariance Ellipsoid3d" << std::endl;
88 // std::cout << "***********************************************************" << std::endl;
89 // std::cout << std::endl;
90 // std::cout << "Suppose we handle robot pose variances on sigmaX, sigmaY," << std::endl;
91 // std::cout << "sigmaT(heta)" << std::endl;
92 // std::cout << std::endl;
93 // std::cout << "[ sigmaX, sigmaY, sigmaT ]: [" << sigmaX << ", " << sigmaY << ", " << sigmaT << "]" << std::endl;
94 // std::cout << P << std::endl;
95 // std::cout << std::endl;
96 // std::cout << "Now we build covariance matrix: " << std::endl;
97 // std::cout << std::endl;
98 // std::cout << "Axis Lengths: " << ellipse.lengths().transpose() << std::endl;
99 // std::cout << "Eigen Values: " << ellipse.lengths()(0)*ellipse.lengths()(0)
100 // << " " << ellipse.lengths()(1)*ellipse.lengths()(1)
101 // << " " << ellipse.lengths()(2)*ellipse.lengths()(2)
102 // << std::endl;
103 // std::cout << "Axis Vectors: " << std::endl;
104 // std::cout << ellipse.axes() << std::endl;
105 // std::cout << std::endl;
106 
107 }
108 
109 /*****************************************************************************
110 ** Main program
111 *****************************************************************************/
112 
113 int main(int argc, char **argv) {
114 
115  testing::InitGoogleTest(&argc,argv);
116  return RUN_ALL_TESTS();
117 }
118 
119 
ecl::CovarianceEllipsoid2d
CovarianceEllipsoid< double, 2 > CovarianceEllipsoid2d
Convenience typedef for 2d covariance ellipsoids.
Definition: covariance_ellipsoid.hpp:526
linear_algebra.hpp
Matrix2d
Matrix2< double > Matrix2d
Matrix3d
Matrix3< double > Matrix3d
Vector3d
Vector3< double > Vector3d
Vector2d
Vector2< double > Vector2d
main
int main(int argc, char **argv)
Definition: covariance_ellipsoids.cpp:113
ecl::CovarianceEllipsoid3d
CovarianceEllipsoid< double, 3 > CovarianceEllipsoid3d
Convenience typedef for 3d covariance ellipsoids.
Definition: covariance_ellipsoid.hpp:543
TEST
TEST(CovarianceTests, ellipsoid2D)
Definition: covariance_ellipsoids.cpp:32


ecl_statistics
Author(s): Daniel Stonier
autogenerated on Wed Mar 2 2022 00:16:31