Drake
eigen_matrix_compare.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <algorithm>
4 #include <cmath>
5 #include <limits>
6 
7 #include <Eigen/Dense>
8 #include <gtest/gtest.h>
9 
11 
12 namespace drake {
13 
15 
16 /**
17  * Compares two matrices to determine whether they are equal to within a certain
18  * threshold.
19  *
20  * @param m1 The first matrix to compare.
21  * @param m2 The second matrix to compare.
22  * @param tolerance The tolerance for determining equivalence.
23  * @param compare_type Whether the tolereance is absolute or relative.
24  * @param explanation A pointer to a string variable for saving an explanation
25  * of why @p m1 and @p m2 are unequal. This parameter is optional and defaults
26  * to `nullptr`. If this is `nullptr` and @p m1 and @p m2 are not equal, an
27  * explanation is logged as an error message.
28  * @return true if the two matrices are equal based on the specified tolerance.
29  */
30 template <typename DerivedA, typename DerivedB>
31 ::testing::AssertionResult CompareMatrices(
32  const Eigen::MatrixBase<DerivedA>& m1,
33  const Eigen::MatrixBase<DerivedB>& m2, double tolerance = 0.0,
35  if (m1.rows() != m2.rows() || m1.cols() != m2.cols()) {
36  return ::testing::AssertionFailure()
37  << "Matrix size mismatch: (" << m1.rows() << " x " << m1.cols()
38  << " vs. " << m2.rows() << " x " << m2.cols() << ")";
39  }
40 
41  for (int ii = 0; ii < m1.rows(); ii++) {
42  for (int jj = 0; jj < m1.cols(); jj++) {
43  // First handle the corner cases of positive infinity, negative infinity,
44  // and NaN
45  bool both_positive_infinity =
46  m1(ii, jj) == std::numeric_limits<double>::infinity() &&
47  m2(ii, jj) == std::numeric_limits<double>::infinity();
48 
49  bool both_negative_infinity =
50  m1(ii, jj) == -std::numeric_limits<double>::infinity() &&
51  m2(ii, jj) == -std::numeric_limits<double>::infinity();
52 
53  bool both_nan = std::isnan(m1(ii, jj)) && std::isnan(m2(ii, jj));
54 
55  if (both_positive_infinity || both_negative_infinity || both_nan)
56  continue;
57 
58  // Check for case where one value is NaN and the other is not
59  if ((std::isnan(m1(ii, jj)) && !std::isnan(m2(ii, jj))) ||
60  (!std::isnan(m1(ii, jj)) && std::isnan(m2(ii, jj)))) {
61  return ::testing::AssertionFailure() << "NaN missmatch at (" << ii
62  << ", " << jj << "):\nm1 =\n"
63  << m1 << "\nm2 =\n"
64  << m2;
65  }
66 
67  // Determine whether the difference between the two matrices is less than
68  // the tolerance.
69  double delta = std::abs(m1(ii, jj) - m2(ii, jj));
70 
71  if (compare_type == MatrixCompareType::absolute) {
72  // Perform comparison using absolute tolerance.
73 
74  if (delta > tolerance) {
75  return ::testing::AssertionFailure()
76  << "Values at (" << ii << ", " << jj
77  << ") exceed tolerance: " << m1(ii, jj) << " vs. "
78  << m2(ii, jj) << ", diff = " << delta
79  << ", tolerance = " << tolerance << "\nm1 =\n"
80  << m1 << "\nm2 =\n"
81  << m2 << "\ndelta=\n"
82  << (m1 - m2);
83  }
84  } else {
85  // Perform comparison using relative tolerance, see:
86  // http://realtimecollisiondetection.net/blog/?p=89
87  double max_value = std::max(std::abs(m1(ii, jj)), std::abs(m2(ii, jj)));
88  double relative_tolerance = tolerance * std::max(1.0, max_value);
89 
90  if (delta > relative_tolerance) {
91  return ::testing::AssertionFailure()
92  << "Values at (" << ii << ", " << jj
93  << ") exceed tolerance: " << m1(ii, jj) << " vs. "
94  << m2(ii, jj) << ", diff = " << delta
95  << ", tolerance = " << tolerance
96  << ", relative tolerance = " << relative_tolerance
97  << "\nm1 =\n"
98  << m1 << "\nm2 =\n"
99  << m2 << "\ndelta=\n"
100  << (m1 - m2);
101  }
102  }
103  }
104  }
105 
106  return ::testing::AssertionSuccess() << "m1 =\n"
107  << m1
108  << "\nis approximately equal to m2 =\n"
109  << m2;
110 }
111 
112 } // namespace drake
bool isnan(const Eigen::AutoDiffScalar< DerType > &x)
Overloads isnan to mimic std::isnan from <cmath>.
Definition: autodiff_overloads.h:52
Definition: automotive_demo.cc:88
Expression abs(const Expression &e)
Definition: symbolic_expression.cc:537
MatrixCompareType
Definition: eigen_matrix_compare.h:14
This is the entry point for all text logging within Drake.
Expression max(const Expression &e1, const Expression &e2)
Definition: symbolic_expression.cc:697
::testing::AssertionResult CompareMatrices(const Eigen::MatrixBase< DerivedA > &m1, const Eigen::MatrixBase< DerivedB > &m2, double tolerance=0.0, MatrixCompareType compare_type=MatrixCompareType::absolute)
Compares two matrices to determine whether they are equal to within a certain threshold.
Definition: eigen_matrix_compare.h:31