38 #ifndef FCL_EIGEN_MATRIX_COMPARE_H
39 #define FCL_EIGEN_MATRIX_COMPARE_H
45 #include <Eigen/Dense>
46 #include <gtest/gtest.h>
62 template <
typename DerivedA,
typename DerivedB>
64 const Eigen::MatrixBase<DerivedA>& m1,
65 const Eigen::MatrixBase<DerivedB>& m2,
double tolerance = 0.0,
67 if (m1.rows() != m2.rows() || m1.cols() != m2.cols()) {
68 return ::testing::AssertionFailure()
69 <<
"Matrix size mismatch: (" << m1.rows() <<
" x " << m1.cols()
70 <<
" vs. " << m2.rows() <<
" x " << m2.cols() <<
")";
73 for (
int ii = 0; ii < m1.rows(); ii++) {
74 for (
int jj = 0; jj < m1.cols(); jj++) {
77 const auto both_positive_infinity =
78 m1(ii, jj) == std::numeric_limits<double>::infinity() &&
79 m2(ii, jj) == std::numeric_limits<double>::infinity();
81 const auto both_negative_infinity =
82 m1(ii, jj) == -std::numeric_limits<double>::infinity() &&
83 m2(ii, jj) == -std::numeric_limits<double>::infinity();
86 const auto both_nan = isnan(m1(ii, jj)) && isnan(m2(ii, jj));
88 if (both_positive_infinity || both_negative_infinity || both_nan)
92 if ((isnan(m1(ii, jj)) && !isnan(m2(ii, jj))) ||
93 (!isnan(m1(ii, jj)) && isnan(m2(ii, jj)))) {
94 return ::testing::AssertionFailure() <<
"NaN missmatch at (" << ii
95 <<
", " << jj <<
"):\nm1 =\n"
103 const auto delta = abs(m1(ii, jj) - m2(ii, jj));
109 return ::testing::AssertionFailure()
110 <<
"Values at (" << ii <<
", " << jj
111 <<
") exceed tolerance: " << m1(ii, jj) <<
" vs. "
112 << m2(ii, jj) <<
", diff = " << delta
113 <<
", tolerance = " <<
tolerance <<
"\nm1 =\n"
115 << m2 <<
"\ndelta=\n"
122 const auto max_value =
max(abs(m1(ii, jj)), abs(m2(ii, jj)));
123 const auto relative_tolerance =
126 if (delta > relative_tolerance) {
127 return ::testing::AssertionFailure()
128 <<
"Values at (" << ii <<
", " << jj
129 <<
") exceed tolerance: " << m1(ii, jj) <<
" vs. "
130 << m2(ii, jj) <<
", diff = " << delta
132 <<
", relative tolerance = " << relative_tolerance
135 << m2 <<
"\ndelta=\n"
142 return ::testing::AssertionSuccess() <<
"m1 =\n"
144 <<
"\nis approximately equal to m2 =\n"
150 #endif // FCL_EIGEN_MATRIX_COMPARE_H