eigen_matrix_compare.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018. Toyota Research Institute
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Open Source Robotics Foundation nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
35 // This code was taken from Drake.
36 // https://github.com/RobotLocomotion/drake/blob/master/common/test_utilities/eigen_matrix_compare.h
37 
38 #ifndef FCL_EIGEN_MATRIX_COMPARE_H
39 #define FCL_EIGEN_MATRIX_COMPARE_H
40 
41 #include <algorithm>
42 #include <cmath>
43 #include <limits>
44 
45 #include <Eigen/Dense>
46 #include <gtest/gtest.h>
47 
48 namespace fcl {
49 
51 
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()) {
69  << "Matrix size mismatch: (" << m1.rows() << " x " << m1.cols()
70  << " vs. " << m2.rows() << " x " << m2.cols() << ")";
71  }
72 
73  for (int ii = 0; ii < m1.rows(); ii++) {
74  for (int jj = 0; jj < m1.cols(); jj++) {
75  // First handle the corner cases of positive infinity, negative infinity,
76  // and NaN
77  const auto both_positive_infinity =
78  m1(ii, jj) == std::numeric_limits<double>::infinity() &&
79  m2(ii, jj) == std::numeric_limits<double>::infinity();
80 
81  const auto both_negative_infinity =
82  m1(ii, jj) == -std::numeric_limits<double>::infinity() &&
83  m2(ii, jj) == -std::numeric_limits<double>::infinity();
84 
85  using std::isnan;
86  const auto both_nan = isnan(m1(ii, jj)) && isnan(m2(ii, jj));
87 
88  if (both_positive_infinity || both_negative_infinity || both_nan)
89  continue;
90 
91  // Check for case where one value is NaN and the other is not
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"
96  << m1 << "\nm2 =\n"
97  << m2;
98  }
99 
100  // Determine whether the difference between the two matrices is less than
101  // the tolerance.
102  using std::abs;
103  const auto delta = abs(m1(ii, jj) - m2(ii, jj));
104 
105  if (compare_type == MatrixCompareType::absolute) {
106  // Perform comparison using absolute tolerance.
107 
108  if (delta > tolerance) {
110  << "Values at (" << ii << ", " << jj
111  << ") exceed tolerance: " << m1(ii, jj) << " vs. "
112  << m2(ii, jj) << ", diff = " << delta
113  << ", tolerance = " << tolerance << "\nm1 =\n"
114  << m1 << "\nm2 =\n"
115  << m2 << "\ndelta=\n"
116  << (m1 - m2);
117  }
118  } else {
119  // Perform comparison using relative tolerance, see:
120  // http://realtimecollisiondetection.net/blog/?p=89
121  using std::max;
122  const auto max_value = max(abs(m1(ii, jj)), abs(m2(ii, jj)));
123  const auto relative_tolerance =
124  tolerance * max(decltype(max_value){1}, max_value);
125 
126  if (delta > relative_tolerance) {
128  << "Values at (" << ii << ", " << jj
129  << ") exceed tolerance: " << m1(ii, jj) << " vs. "
130  << m2(ii, jj) << ", diff = " << delta
131  << ", tolerance = " << tolerance
132  << ", relative tolerance = " << relative_tolerance
133  << "\nm1 =\n"
134  << m1 << "\nm2 =\n"
135  << m2 << "\ndelta=\n"
136  << (m1 - m2);
137  }
138  }
139  }
140  }
141 
143  << m1
144  << "\nis approximately equal to m2 =\n"
145  << m2;
146 }
147 
148 } // namespace fcl
149 
150 #endif // FCL_EIGEN_MATRIX_COMPARE_H
testing::AssertionFailure
GTEST_API_ AssertionResult AssertionFailure()
Definition: gtest.cc:976
gtest.h
fcl::MatrixCompareType::relative
@ relative
fcl::CompareMatrices
::testing::AssertionResult CompareMatrices(const Eigen::MatrixBase< DerivedA > &m1, const Eigen::MatrixBase< DerivedB > &m2, double tolerance=0.0, MatrixCompareType compare_type=MatrixCompareType::absolute)
Definition: eigen_matrix_compare.h:63
max
static T max(T x, T y)
Definition: svm.cpp:52
testing::AssertionResult
Definition: gtest.h:256
testing::AssertionSuccess
GTEST_API_ AssertionResult AssertionSuccess()
Definition: gtest.cc:971
fcl::MatrixCompareType::absolute
@ absolute
fcl::MatrixCompareType
MatrixCompareType
Definition: eigen_matrix_compare.h:50
tolerance
S tolerance()
Definition: test_fcl_geometric_shapes.cpp:87
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45


fcl
Author(s):
autogenerated on Fri Apr 2 2021 02:37:59