gtest_eigen.hpp
Go to the documentation of this file.
00001 
00008 #pragma once
00009 
00010 #include <gtest/gtest.h>
00011 #include <Eigen/Core>
00012 #include <iostream>
00013 #include <cmath>
00014 
00015 namespace grid_map {
00016 
00017     template<int N>
00018     Eigen::Matrix<double,N,N> randomCovariance()
00019     {
00020       Eigen::Matrix<double,N,N> U;
00021       U.setRandom();
00022       return U.transpose() * U + 5.0 * Eigen::Matrix<double,N,N>::Identity();
00023     }
00024 
00025     inline Eigen::MatrixXd randomCovarianceXd(int N)
00026     {
00027         Eigen::MatrixXd U(N,N);
00028         U.setRandom();
00029         return U.transpose() * U + 5.0 * Eigen::MatrixXd::Identity(N,N);
00030     }
00031 
00032     template<typename M1, typename M2>
00033     void assertEqual(const M1 & A, const M2 & B, std::string const & message = "")
00034     {
00035         ASSERT_EQ((size_t)A.rows(),(size_t)B.rows()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n";
00036       ASSERT_EQ((size_t)A.cols(),(size_t)B.cols()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n";
00037 
00038       for(int r = 0; r < A.rows(); r++)
00039                 {
00040                   for(int c = 0; c < A.cols(); c++)
00041                         {
00042                     if (std::isnan(A(r,c))) {
00043                       ASSERT_TRUE(std::isnan(B(r,c)));
00044                     } else {
00045                             ASSERT_EQ(A(r,c),B(r,c)) << message << "\nEquality comparison failed at (" << r << "," << c << ")\n"
00046                                        << "\nMatrix A:\n" << A << "\nand matrix B\n" << B;
00047                     }
00048                         }
00049                 }
00050     }
00051 
00052     template<typename M1, typename M2, typename T>
00053     void assertNear(const M1 & A, const M2 & B, T tolerance, std::string const & message = "")
00054     {
00055       // Note: If these assertions fail, they only abort this subroutine.
00056       // see: http://code.google.com/p/googletest/wiki/AdvancedGuide#Using_Assertions_in_Sub-routines
00057       // \todo better handling of this
00058         ASSERT_EQ((size_t)A.rows(),(size_t)B.rows()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n";
00059       ASSERT_EQ((size_t)A.cols(),(size_t)B.cols()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n";
00060 
00061       for(int r = 0; r < A.rows(); r++)
00062                 {
00063                   for(int c = 0; c < A.cols(); c++)
00064                         {
00065                     if (std::isnan(A(r,c))) {
00066                       ASSERT_TRUE(std::isnan(B(r,c)));
00067                     } else {
00068                             ASSERT_NEAR(A(r,c),B(r,c),tolerance) << message << "\nTolerance comparison failed at (" << r << "," << c << ")\n"
00069                                                                                                    << "\nMatrix A:\n" << A << "\nand matrix B\n" << B;
00070                     }
00071                         }
00072                 }
00073     }
00074 
00075     template<typename M1, typename M2, typename T>
00076     void expectNear(const M1 & A, const M2 & B, T tolerance, std::string const & message = "")
00077     {
00078       EXPECT_EQ((size_t)A.rows(),(size_t)B.rows()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n";
00079       EXPECT_EQ((size_t)A.cols(),(size_t)B.cols()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n";
00080 
00081       for(int r = 0; r < A.rows(); r++)
00082         {
00083           for(int c = 0; c < A.cols(); c++)
00084             {
00085               if (std::isnan(A(r,c))) {
00086                 EXPECT_TRUE(std::isnan(B(r,c)));
00087               } else {
00088                 EXPECT_NEAR(A(r,c),B(r,c),tolerance) << message << "\nTolerance comparison failed at (" << r << "," << c << ")\n"
00089                              << "\nMatrix A:\n" << A << "\nand matrix B\n" << B;
00090               }
00091             }
00092         }
00093     }
00094 
00095     template<typename M1>
00096     void assertFinite(const M1 & A, std::string const & message = "")
00097     {
00098       for(int r = 0; r < A.rows(); r++)
00099                 {
00100                   for(int c = 0; c < A.cols(); c++)
00101                         {
00102                           ASSERT_TRUE(std::isfinite(A(r,c))) << std::endl << "Check for finite values failed at A(" << r << "," << c << "). Matrix A:" << std::endl << A << std::endl;
00103                         }
00104                 }
00105     }
00106 
00107     inline bool compareRelative(double a, double b, double percentTolerance, double * percentError = NULL)
00108     {
00109           // \todo: does anyone have a better idea?
00110       double fa = fabs(a);
00111       double fb = fabs(b);
00112       if( (fa < 1e-15 && fb < 1e-15) ||  // Both zero.
00113                   (fa == 0.0  && fb < 1e-6)  ||  // One exactly zero and the other small
00114                   (fb == 0.0  && fa < 1e-6) )    // ditto
00115                 return true;
00116 
00117       double diff = fabs(a - b)/std::max(fa,fb);
00118       if(diff > percentTolerance * 1e-2)
00119                 {
00120                   if(percentError)
00121                         *percentError = diff * 100.0;
00122                   return false;
00123                 }
00124       return true;
00125     }
00126 
00127 #define ASSERT_DOUBLE_MX_EQ(A, B, PERCENT_TOLERANCE, MSG)                               \
00128     ASSERT_EQ((size_t)(A).rows(), (size_t)(B).rows())  << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B << "\nare not the same size"; \
00129     ASSERT_EQ((size_t)(A).cols(), (size_t)(B).cols())  << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B << "\nare not the same size"; \
00130     for(int r = 0; r < (A).rows(); r++)                                                                 \
00131       {                                                                                                                                 \
00132                 for(int c = 0; c < (A).cols(); c++)                                                             \
00133                   {                                                                                                                             \
00134                         double percentError = 0.0;                                                                      \
00135                         ASSERT_TRUE(grid_map::compareRelative( (A)(r,c), (B)(r,c), PERCENT_TOLERANCE, &percentError)) \
00136                           << MSG << "\nComparing:\n"                                                            \
00137                           << #A << "(" << r << "," << c << ") = " << (A)(r,c) << std::endl \
00138                           << #B << "(" << r << "," << c << ") = " << (B)(r,c) << std::endl \
00139                           << "Error was " << percentError << "% > " << PERCENT_TOLERANCE << "%\n" \
00140                           << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B; \
00141                   }                                                                                                                             \
00142       }
00143 
00144 #define ASSERT_DOUBLE_SPARSE_MX_EQ(A, B, PERCENT_TOLERANCE, MSG)       \
00145     ASSERT_EQ((size_t)(A).rows(), (size_t)(B).rows())  << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B << "\nare not the same size"; \
00146     ASSERT_EQ((size_t)(A).cols(), (size_t)(B).cols())  << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B << "\nare not the same size"; \
00147     for(int r = 0; r < (A).rows(); r++)                 \
00148       {                                 \
00149     for(int c = 0; c < (A).cols(); c++)               \
00150       {                               \
00151       double percentError = 0.0;                  \
00152       ASSERT_TRUE(grid_map::compareRelative( (A).coeff(r,c), (B).coeff(r,c), PERCENT_TOLERANCE, &percentError)) \
00153         << MSG << "\nComparing:\n"                \
00154         << #A << "(" << r << "," << c << ") = " << (A).coeff(r,c) << std::endl \
00155         << #B << "(" << r << "," << c << ") = " << (B).coeff(r,c) << std::endl \
00156         << "Error was " << percentError << "% > " << PERCENT_TOLERANCE << "%\n" \
00157         << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B; \
00158       }                               \
00159       }
00160 
00161 } // namespace


grid_map_core
Author(s): P├ęter Fankhauser
autogenerated on Tue Jul 9 2019 05:06:13