covariance_ellipsoids.cpp
Go to the documentation of this file.
00001 
00008 /*****************************************************************************
00009 ** Includes
00010 *****************************************************************************/
00011 
00012 #include <iostream>
00013 #include <gtest/gtest.h>
00014 #include <ecl/linear_algebra.hpp>
00015 #include "../../include/ecl/statistics/covariance_ellipsoid.hpp"
00016 
00017 /*****************************************************************************
00018 ** Using
00019 *****************************************************************************/
00020 
00021 using ecl::CovarianceEllipsoid2d;
00022 using ecl::CovarianceEllipsoid3d;
00023 using ecl::linear_algebra::Matrix2d;
00024 using ecl::linear_algebra::Vector2d;
00025 using ecl::linear_algebra::Matrix3d;
00026 using ecl::linear_algebra::Vector3d;
00027 
00028 /*****************************************************************************
00029 ** Tests
00030 *****************************************************************************/
00031 
00032 TEST(CovarianceTests,ellipsoid2D) {
00033     Matrix2d M;
00034         M << 3.0, 1.0, 1.0, 5.0; // must be a positive definite, symmetric matrix
00035 //    M << 0.001801, -0.000047, -0.000047, 0.000259;
00036 //    M << 0.0000009661, 0.0000065, 0.0000065, 0.0001261;
00037 
00038     CovarianceEllipsoid2d ellipse(M);
00039     const Vector2d& lengths = ellipse.lengths();
00040         double eigen_value_0 = lengths[0]*lengths[0];
00041     double angle = ellipse.rotation();
00042     const Vector2d& intercepts = ellipse.intercepts();
00043     const Matrix2d& axes = ellipse.axes();
00044 
00045         EXPECT_GT(2.328,lengths[0]); EXPECT_LT(2.326,lengths[0]);
00046     EXPECT_GT(5.415,eigen_value_0); EXPECT_LT(5.413,eigen_value_0);
00047     EXPECT_GT(1.179,angle); EXPECT_LT(1.177,angle);
00048         EXPECT_GT(1.674,intercepts[0]); EXPECT_LT(1.672,intercepts[0]);
00049         EXPECT_GT(0.3828,axes(0,0)); EXPECT_LT(0.3826,axes(0,0));
00050 
00051 //    std::cout << std::endl;
00052 //    std::cout << "***********************************************************" << std::endl;
00053 //    std::cout << "                 Covariance Ellipsoid2d" << std::endl;
00054 //    std::cout << "***********************************************************" << std::endl;
00055 //    std::cout << std::endl;
00056 //    std::cout << "Test Matrix: " << std::endl;
00057 //    std::cout << std::endl;
00058 //    std::cout << M << std::endl;
00059 //    std::cout << std::endl;
00060 //    std::cout << "Axis Lengths: " <<  ellipse.lengths().transpose() << std::endl;
00061 //    std::cout << "Eigen Values: " << ellipse.lengths()(0)*ellipse.lengths()(0) << " " << ellipse.lengths()(1)*ellipse.lengths()(1) << std::endl;
00062 //    std::cout << "Axis Vectors: " << std::endl;
00063 //    std::cout << "Rotation angle: " << ellipse.rotation() << std::endl;
00064 //    std::cout << "Intercepts: " << ellipse.intercepts().transpose() << std::endl;
00065 //    std::cout << "Eigenvector Dot Product: " << ellipse.axes().block(0,0,2,1).transpose()*ellipse.axes().block(0,1,2,1) << std::endl;
00066 
00067 }
00068 
00069 TEST(CovarianceTests,ellipsoid3D) {
00070     Matrix3d P;
00071     double sigmaX(0.1);
00072     double sigmaY(0.5);
00073     double sigmaT(0.3);
00074     P << sigmaX*sigmaX, 0, 0, 0, sigmaY*sigmaY, 0, 0, 0, sigmaT*sigmaT;
00075         CovarianceEllipsoid3d ellipse(P);
00076 
00077     const Vector3d& lengths = ellipse.lengths();
00078         double eigen_value_0 = lengths[0]*lengths[0];
00079     const Matrix3d& axes = ellipse.axes();
00080     std::cout << axes << std::endl;
00081         EXPECT_GT(0.101,lengths[0]); EXPECT_LT(0.099,lengths[0]);
00082     EXPECT_GT(0.011,eigen_value_0); EXPECT_LT(0.009,eigen_value_0);
00083         EXPECT_GT(1.001,axes(0,0)); EXPECT_LT(0.999,axes(0,0));
00084 
00085 //  std::cout << std::endl;
00086 //      std::cout << "***********************************************************" << std::endl;
00087 //      std::cout << "                 Covariance Ellipsoid3d" << std::endl;
00088 //      std::cout << "***********************************************************" << std::endl;
00089 //      std::cout << std::endl;
00090 //      std::cout << "Suppose we handle robot pose variances on sigmaX, sigmaY," << std::endl;
00091 //      std::cout << "sigmaT(heta)" << std::endl;
00092 //      std::cout << std::endl;
00093 //      std::cout << "[ sigmaX, sigmaY, sigmaT ]: [" << sigmaX << ", " << sigmaY << ", " << sigmaT << "]" << std::endl;
00094 //      std::cout << P << std::endl;
00095 //      std::cout << std::endl;
00096 //      std::cout << "Now we build covariance matrix: " << std::endl;
00097 //      std::cout << std::endl;
00098 //      std::cout << "Axis Lengths: " <<  ellipse.lengths().transpose() << std::endl;
00099 //      std::cout << "Eigen Values: " << ellipse.lengths()(0)*ellipse.lengths()(0)
00100 //                                                                                                      << " " << ellipse.lengths()(1)*ellipse.lengths()(1)
00101 //                                                                                                      << " " <<  ellipse.lengths()(2)*ellipse.lengths()(2)
00102 //                                                                                                      << std::endl;
00103 //      std::cout << "Axis Vectors: " << std::endl;
00104 //      std::cout << ellipse.axes() << std::endl;
00105 //      std::cout << std::endl;
00106 
00107 }
00108 
00109 /*****************************************************************************
00110 ** Main program
00111 *****************************************************************************/
00112 
00113 int main(int argc, char **argv) {
00114 
00115     testing::InitGoogleTest(&argc,argv);
00116     return RUN_ALL_TESTS();
00117 }
00118 
00119 


ecl_statistics
Author(s): Daniel Stonier
autogenerated on Sun Oct 5 2014 23:35:31