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