gtest_eigen.hpp
Go to the documentation of this file.
1 
8 #pragma once
9 
10 #include <gtest/gtest.h>
11 #include <Eigen/Core>
12 #include <iostream>
13 #include <cmath>
14 
15 namespace grid_map {
16 
17  template<int N>
18  Eigen::Matrix<double,N,N> randomCovariance()
19  {
20  Eigen::Matrix<double,N,N> U;
21  U.setRandom();
22  return U.transpose() * U + 5.0 * Eigen::Matrix<double,N,N>::Identity();
23  }
24 
25  inline Eigen::MatrixXd randomCovarianceXd(int N)
26  {
27  Eigen::MatrixXd U(N,N);
28  U.setRandom();
29  return U.transpose() * U + 5.0 * Eigen::MatrixXd::Identity(N,N);
30  }
31 
32  template<typename M1, typename M2>
33  void assertEqual(const M1 & A, const M2 & B, std::string const & message = "")
34  {
35  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";
36  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";
37 
38  for(int r = 0; r < A.rows(); r++)
39  {
40  for(int c = 0; c < A.cols(); c++)
41  {
42  if (std::isnan(A(r,c))) {
43  ASSERT_TRUE(std::isnan(B(r,c)));
44  } else {
45  ASSERT_EQ(A(r,c),B(r,c)) << message << "\nEquality comparison failed at (" << r << "," << c << ")\n"
46  << "\nMatrix A:\n" << A << "\nand matrix B\n" << B;
47  }
48  }
49  }
50  }
51 
52  template<typename M1, typename M2, typename T>
53  void assertNear(const M1 & A, const M2 & B, T tolerance, std::string const & message = "")
54  {
55  // Note: If these assertions fail, they only abort this subroutine.
56  // see: http://code.google.com/p/googletest/wiki/AdvancedGuide#Using_Assertions_in_Sub-routines
57  // \todo better handling of this
58  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";
59  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";
60 
61  for(int r = 0; r < A.rows(); r++)
62  {
63  for(int c = 0; c < A.cols(); c++)
64  {
65  if (std::isnan(A(r,c))) {
66  ASSERT_TRUE(std::isnan(B(r,c)));
67  } else {
68  ASSERT_NEAR(A(r,c),B(r,c),tolerance) << message << "\nTolerance comparison failed at (" << r << "," << c << ")\n"
69  << "\nMatrix A:\n" << A << "\nand matrix B\n" << B;
70  }
71  }
72  }
73  }
74 
75  template<typename M1, typename M2, typename T>
76  void expectNear(const M1 & A, const M2 & B, T tolerance, std::string const & message = "")
77  {
78  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";
79  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";
80 
81  for(int r = 0; r < A.rows(); r++)
82  {
83  for(int c = 0; c < A.cols(); c++)
84  {
85  if (std::isnan(A(r,c))) {
86  EXPECT_TRUE(std::isnan(B(r,c)));
87  } else {
88  EXPECT_NEAR(A(r,c),B(r,c),tolerance) << message << "\nTolerance comparison failed at (" << r << "," << c << ")\n"
89  << "\nMatrix A:\n" << A << "\nand matrix B\n" << B;
90  }
91  }
92  }
93  }
94 
95  template<typename M1>
96  void assertFinite(const M1 & A, std::string const & message = "")
97  {
98  for(int r = 0; r < A.rows(); r++)
99  {
100  for(int c = 0; c < A.cols(); c++)
101  {
102  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;
103  }
104  }
105  }
106 
107  inline bool compareRelative(double a, double b, double percentTolerance, double * percentError = NULL)
108  {
109  // \todo: does anyone have a better idea?
110  double fa = fabs(a);
111  double fb = fabs(b);
112  if( (fa < 1e-15 && fb < 1e-15) || // Both zero.
113  (fa == 0.0 && fb < 1e-6) || // One exactly zero and the other small
114  (fb == 0.0 && fa < 1e-6) ) // ditto
115  return true;
116 
117  double diff = fabs(a - b)/std::max(fa,fb);
118  if(diff > percentTolerance * 1e-2)
119  {
120  if(percentError)
121  *percentError = diff * 100.0;
122  return false;
123  }
124  return true;
125  }
126 
127 #define ASSERT_DOUBLE_MX_EQ(A, B, PERCENT_TOLERANCE, MSG) \
128  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"; \
129  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"; \
130  for(int r = 0; r < (A).rows(); r++) \
131  { \
132  for(int c = 0; c < (A).cols(); c++) \
133  { \
134  double percentError = 0.0; \
135  ASSERT_TRUE(grid_map::compareRelative( (A)(r,c), (B)(r,c), PERCENT_TOLERANCE, &percentError)) \
136  << MSG << "\nComparing:\n" \
137  << #A << "(" << r << "," << c << ") = " << (A)(r,c) << std::endl \
138  << #B << "(" << r << "," << c << ") = " << (B)(r,c) << std::endl \
139  << "Error was " << percentError << "% > " << PERCENT_TOLERANCE << "%\n" \
140  << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B; \
141  } \
142  }
143 
144 #define ASSERT_DOUBLE_SPARSE_MX_EQ(A, B, PERCENT_TOLERANCE, MSG) \
145  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"; \
146  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"; \
147  for(int r = 0; r < (A).rows(); r++) \
148  { \
149  for(int c = 0; c < (A).cols(); c++) \
150  { \
151  double percentError = 0.0; \
152  ASSERT_TRUE(grid_map::compareRelative( (A).coeff(r,c), (B).coeff(r,c), PERCENT_TOLERANCE, &percentError)) \
153  << MSG << "\nComparing:\n" \
154  << #A << "(" << r << "," << c << ") = " << (A).coeff(r,c) << std::endl \
155  << #B << "(" << r << "," << c << ") = " << (B).coeff(r,c) << std::endl \
156  << "Error was " << percentError << "% > " << PERCENT_TOLERANCE << "%\n" \
157  << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B; \
158  } \
159  }
160 
161 } // namespace
Eigen::MatrixXd randomCovarianceXd(int N)
Definition: gtest_eigen.hpp:25
void assertNear(const M1 &A, const M2 &B, T tolerance, std::string const &message="")
Definition: gtest_eigen.hpp:53
void assertFinite(const M1 &A, std::string const &message="")
Definition: gtest_eigen.hpp:96
void assertEqual(const M1 &A, const M2 &B, std::string const &message="")
Definition: gtest_eigen.hpp:33
void expectNear(const M1 &A, const M2 &B, T tolerance, std::string const &message="")
Definition: gtest_eigen.hpp:76
Eigen::Matrix< double, N, N > randomCovariance()
Definition: gtest_eigen.hpp:18
bool compareRelative(double a, double b, double percentTolerance, double *percentError=NULL)


grid_map_core
Author(s): P├ęter Fankhauser
autogenerated on Tue Jun 25 2019 20:02:08