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  for(int r = 0; r < A.rows(); r++)
102  {
103  for(int c = 0; c < A.cols(); c++)
104  {
105  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;
106  }
107  }
108  }
109 
110  inline bool compareRelative(double a, double b, double percentTolerance, double * percentError = NULL)
111  {
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  double diff = fabs(a - b)/std::max(fa,fb);
121  if(diff > percentTolerance * 1e-2)
122  {
123  if(percentError)
124  *percentError = diff * 100.0;
125  return false;
126  }
127  return true;
128  }
129 
130 #define ASSERT_DOUBLE_MX_EQ(A, B, PERCENT_TOLERANCE, MSG) \
131  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"; \
132  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"; \
133  for(int r = 0; r < (A).rows(); r++) \
134  { \
135  for(int c = 0; c < (A).cols(); c++) \
136  { \
137  double percentError = 0.0; \
138  ASSERT_TRUE(grid_map::compareRelative( (A)(r,c), (B)(r,c), PERCENT_TOLERANCE, &percentError)) \
139  << MSG << "\nComparing:\n" \
140  << #A << "(" << r << "," << c << ") = " << (A)(r,c) << std::endl \
141  << #B << "(" << r << "," << c << ") = " << (B)(r,c) << std::endl \
142  << "Error was " << percentError << "% > " << PERCENT_TOLERANCE << "%\n" \
143  << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B; \
144  } \
145  }
146 
147 #define ASSERT_DOUBLE_SPARSE_MX_EQ(A, B, PERCENT_TOLERANCE, MSG) \
148  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"; \
149  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"; \
150  for(int r = 0; r < (A).rows(); r++) \
151  { \
152  for(int c = 0; c < (A).cols(); c++) \
153  { \
154  double percentError = 0.0; \
155  ASSERT_TRUE(grid_map::compareRelative( (A).coeff(r,c), (B).coeff(r,c), PERCENT_TOLERANCE, &percentError)) \
156  << MSG << "\nComparing:\n" \
157  << #A << "(" << r << "," << c << ") = " << (A).coeff(r,c) << std::endl \
158  << #B << "(" << r << "," << c << ") = " << (B).coeff(r,c) << std::endl \
159  << "Error was " << percentError << "% > " << PERCENT_TOLERANCE << "%\n" \
160  << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B; \
161  } \
162  }
163 
164 } // namespace
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::assertFinite
void assertFinite(const M1 &A, std::string const &message="")
Definition: gtest_eigen.hpp:99
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::expectNear
void expectNear(const M1 &A, const M2 &B, T tolerance, std::string const &message="")
Definition: gtest_eigen.hpp:78
grid_map::randomCovariance
Eigen::Matrix< double, N, N > randomCovariance()
Definition: gtest_eigen.hpp:18
grid_map::compareRelative
bool compareRelative(double a, double b, double percentTolerance, double *percentError=NULL)
Definition: gtest_eigen.hpp:110

grid_map_core
Author(s): Péter Fankhauser
autogenerated on Tue Jun 1 2021 02:13:27