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 
12 
13 namespace drake {
14 
16 
31 template <typename DerivedA, typename DerivedB>
32 ::testing::AssertionResult CompareMatrices(
33  const Eigen::MatrixBase<DerivedA>& m1,
34  const Eigen::MatrixBase<DerivedB>& m2, double tolerance = 0.0,
36  if (m1.rows() != m2.rows() || m1.cols() != m2.cols()) {
37  return ::testing::AssertionFailure()
38  << "Matrix size mismatch: (" << m1.rows() << " x " << m1.cols()
39  << " vs. " << m2.rows() << " x " << m2.cols() << ")";
40  }
41 
42  for (int ii = 0; ii < m1.rows(); ii++) {
43  for (int jj = 0; jj < m1.cols(); jj++) {
44  // First handle the corner cases of positive infinity, negative infinity,
45  // and NaN
46  bool both_positive_infinity =
47  m1(ii, jj) == std::numeric_limits<double>::infinity() &&
48  m2(ii, jj) == std::numeric_limits<double>::infinity();
49 
50  bool both_negative_infinity =
51  m1(ii, jj) == -std::numeric_limits<double>::infinity() &&
52  m2(ii, jj) == -std::numeric_limits<double>::infinity();
53 
54  bool both_nan = std::isnan(m1(ii, jj)) && std::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 ((std::isnan(m1(ii, jj)) && !std::isnan(m2(ii, jj))) ||
61  (!std::isnan(m1(ii, jj)) && std::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  double delta = std::abs(m1(ii, jj) - m2(ii, jj));
71 
72  if (compare_type == MatrixCompareType::absolute) {
73  // Perform comparison using absolute tolerance.
74 
75  if (delta > tolerance) {
76  return ::testing::AssertionFailure()
77  << "Values at (" << ii << ", " << jj
78  << ") exceed tolerance: " << m1(ii, jj) << " vs. "
79  << m2(ii, jj) << ", diff = " << delta
80  << ", tolerance = " << tolerance << "\nm1 =\n"
81  << m1 << "\nm2 =\n"
82  << m2 << "\ndelta=\n"
83  << (m1 - m2);
84  }
85  } else {
86  // Perform comparison using relative tolerance, see:
87  // http://realtimecollisiondetection.net/blog/?p=89
88  double max_value = std::max(std::abs(m1(ii, jj)), std::abs(m2(ii, jj)));
89  double relative_tolerance = tolerance * std::max(1.0, max_value);
90 
91  if (delta > relative_tolerance) {
92  return ::testing::AssertionFailure()
93  << "Values at (" << ii << ", " << jj
94  << ") exceed tolerance: " << m1(ii, jj) << " vs. "
95  << m2(ii, jj) << ", diff = " << delta
96  << ", tolerance = " << tolerance
97  << ", relative tolerance = " << relative_tolerance
98  << "\nm1 =\n"
99  << m1 << "\nm2 =\n"
100  << m2 << "\ndelta=\n"
101  << (m1 - m2);
102  }
103  }
104  }
105  }
106 
107  return ::testing::AssertionSuccess() << "m1 =\n"
108  << m1
109  << "\nis approximately equal to m2 =\n"
110  << m2;
111 }
112 
113 } // namespace drake
bool isnan(const Eigen::AutoDiffScalar< DerType > &x)
Overloads isnan to mimic std::isnan from <cmath>.
Definition: autodiff_overloads.h:48
Definition: automotive_demo.cc:88
This header provides a std::make_unique implementation to be used when the compiler does not support ...
Expression abs(const Expression &e)
Definition: symbolic_expression.cc:537
MatrixCompareType
Definition: eigen_matrix_compare.h:15
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:32