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

grid_map_core
Author(s): Péter Fankhauser
autogenerated on Wed Jul 5 2023 02:23:35