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
00056
00057
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
00110 double fa = fabs(a);
00111 double fb = fabs(b);
00112 if( (fa < 1e-15 && fb < 1e-15) ||
00113 (fa == 0.0 && fb < 1e-6) ||
00114 (fb == 0.0 && fa < 1e-6) )
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 }