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 
10 #include "drake/common/text_logging.h"
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  const auto both_positive_infinity =
46  m1(ii, jj) == std::numeric_limits<double>::infinity() &&
47  m2(ii, jj) == std::numeric_limits<double>::infinity();
48 
49  const auto both_negative_infinity =
50  m1(ii, jj) == -std::numeric_limits<double>::infinity() &&
51  m2(ii, jj) == -std::numeric_limits<double>::infinity();
52 
53  using std::isnan;
54  const auto both_nan = isnan(m1(ii, jj)) && isnan(m2(ii, jj));
55 
56  if (both_positive_infinity || both_negative_infinity || both_nan)
57  continue;
58 
59  // Check for case where one value is NaN and the other is not
60  if ((isnan(m1(ii, jj)) && !isnan(m2(ii, jj))) ||
61  (!isnan(m1(ii, jj)) && isnan(m2(ii, jj)))) {
62  return ::testing::AssertionFailure() << "NaN missmatch at (" << ii
63  << ", " << jj << "):\nm1 =\n"
64  << m1 << "\nm2 =\n"
65  << m2;
66  }
67 
68  // Determine whether the difference between the two matrices is less than
69  // the tolerance.
70  using std::abs;
71  const auto delta = abs(m1(ii, jj) - m2(ii, jj));
72 
73  if (compare_type == MatrixCompareType::absolute) {
74  // Perform comparison using absolute tolerance.
75 
76  if (delta > tolerance) {
77  return ::testing::AssertionFailure()
78  << "Values at (" << ii << ", " << jj
79  << ") exceed tolerance: " << m1(ii, jj) << " vs. "
80  << m2(ii, jj) << ", diff = " << delta
81  << ", tolerance = " << tolerance << "\nm1 =\n"
82  << m1 << "\nm2 =\n"
83  << m2 << "\ndelta=\n"
84  << (m1 - m2);
85  }
86  } else {
87  // Perform comparison using relative tolerance, see:
88  // http://realtimecollisiondetection.net/blog/?p=89
89  using std::max;
90  const auto max_value = max(abs(m1(ii, jj)), abs(m2(ii, jj)));
91  const auto relative_tolerance =
92  tolerance * max(decltype(max_value){1}, max_value);
93 
94  if (delta > relative_tolerance) {
95  return ::testing::AssertionFailure()
96  << "Values at (" << ii << ", " << jj
97  << ") exceed tolerance: " << m1(ii, jj) << " vs. "
98  << m2(ii, jj) << ", diff = " << delta
99  << ", tolerance = " << tolerance
100  << ", relative tolerance = " << relative_tolerance
101  << "\nm1 =\n"
102  << m1 << "\nm2 =\n"
103  << m2 << "\ndelta=\n"
104  << (m1 - m2);
105  }
106  }
107  }
108  }
109 
110  return ::testing::AssertionSuccess() << "m1 =\n"
111  << m1
112  << "\nis approximately equal to m2 =\n"
113  << m2;
114 }
115 
116 } // 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:89
Expression abs(const Expression &e)
Definition: symbolic_expression.cc:536
MatrixCompareType
Definition: eigen_matrix_compare.h:14
Expression max(const Expression &e1, const Expression &e2)
Definition: symbolic_expression.cc:696
::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