151 lines
5.9 KiB
C++
151 lines
5.9 KiB
C++
/*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* Copyright (c) 2018. Toyota Research Institute
|
|
* All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions
|
|
* are met:
|
|
*
|
|
* * Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* * Redistributions in binary form must reproduce the above
|
|
* copyright notice, this list of conditions and the following
|
|
* disclaimer in the documentation and/or other materials provided
|
|
* with the distribution.
|
|
* * Neither the name of Open Source Robotics Foundation nor the names of its
|
|
* contributors may be used to endorse or promote products derived
|
|
* from this software without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*/
|
|
|
|
// This code was taken from Drake.
|
|
// https://github.com/RobotLocomotion/drake/blob/master/common/test_utilities/eigen_matrix_compare.h
|
|
|
|
#ifndef FCL_EIGEN_MATRIX_COMPARE_H
|
|
#define FCL_EIGEN_MATRIX_COMPARE_H
|
|
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <limits>
|
|
|
|
#include <Eigen/Dense>
|
|
#include <gtest/gtest.h>
|
|
|
|
namespace fcl {
|
|
|
|
enum class MatrixCompareType { absolute, relative };
|
|
|
|
/**
|
|
* Compares two matrices to determine whether they are equal to within a certain
|
|
* threshold.
|
|
*
|
|
* @param m1 The first matrix to compare.
|
|
* @param m2 The second matrix to compare.
|
|
* @param tolerance The tolerance for determining equivalence.
|
|
* @param compare_type Whether the tolerance is absolute or relative.
|
|
* @return true if the two matrices are equal based on the specified tolerance.
|
|
*/
|
|
template <typename DerivedA, typename DerivedB>
|
|
::testing::AssertionResult CompareMatrices(
|
|
const Eigen::MatrixBase<DerivedA>& m1,
|
|
const Eigen::MatrixBase<DerivedB>& m2, double tolerance = 0.0,
|
|
MatrixCompareType compare_type = MatrixCompareType::absolute) {
|
|
if (m1.rows() != m2.rows() || m1.cols() != m2.cols()) {
|
|
return ::testing::AssertionFailure()
|
|
<< "Matrix size mismatch: (" << m1.rows() << " x " << m1.cols()
|
|
<< " vs. " << m2.rows() << " x " << m2.cols() << ")";
|
|
}
|
|
|
|
for (int ii = 0; ii < m1.rows(); ii++) {
|
|
for (int jj = 0; jj < m1.cols(); jj++) {
|
|
// First handle the corner cases of positive infinity, negative infinity,
|
|
// and NaN
|
|
const auto both_positive_infinity =
|
|
m1(ii, jj) == std::numeric_limits<double>::infinity() &&
|
|
m2(ii, jj) == std::numeric_limits<double>::infinity();
|
|
|
|
const auto both_negative_infinity =
|
|
m1(ii, jj) == -std::numeric_limits<double>::infinity() &&
|
|
m2(ii, jj) == -std::numeric_limits<double>::infinity();
|
|
|
|
using std::isnan;
|
|
const auto both_nan = isnan(m1(ii, jj)) && isnan(m2(ii, jj));
|
|
|
|
if (both_positive_infinity || both_negative_infinity || both_nan)
|
|
continue;
|
|
|
|
// Check for case where one value is NaN and the other is not
|
|
if ((isnan(m1(ii, jj)) && !isnan(m2(ii, jj))) ||
|
|
(!isnan(m1(ii, jj)) && isnan(m2(ii, jj)))) {
|
|
return ::testing::AssertionFailure() << "NaN missmatch at (" << ii
|
|
<< ", " << jj << "):\nm1 =\n"
|
|
<< m1 << "\nm2 =\n"
|
|
<< m2;
|
|
}
|
|
|
|
// Determine whether the difference between the two matrices is less than
|
|
// the tolerance.
|
|
using std::abs;
|
|
const auto delta = abs(m1(ii, jj) - m2(ii, jj));
|
|
|
|
if (compare_type == MatrixCompareType::absolute) {
|
|
// Perform comparison using absolute tolerance.
|
|
|
|
if (delta > tolerance) {
|
|
return ::testing::AssertionFailure()
|
|
<< "Values at (" << ii << ", " << jj
|
|
<< ") exceed tolerance: " << m1(ii, jj) << " vs. "
|
|
<< m2(ii, jj) << ", diff = " << delta
|
|
<< ", tolerance = " << tolerance << "\nm1 =\n"
|
|
<< m1 << "\nm2 =\n"
|
|
<< m2 << "\ndelta=\n"
|
|
<< (m1 - m2);
|
|
}
|
|
} else {
|
|
// Perform comparison using relative tolerance, see:
|
|
// http://realtimecollisiondetection.net/blog/?p=89
|
|
using std::max;
|
|
const auto max_value = max(abs(m1(ii, jj)), abs(m2(ii, jj)));
|
|
const auto relative_tolerance =
|
|
tolerance * max(decltype(max_value){1}, max_value);
|
|
|
|
if (delta > relative_tolerance) {
|
|
return ::testing::AssertionFailure()
|
|
<< "Values at (" << ii << ", " << jj
|
|
<< ") exceed tolerance: " << m1(ii, jj) << " vs. "
|
|
<< m2(ii, jj) << ", diff = " << delta
|
|
<< ", tolerance = " << tolerance
|
|
<< ", relative tolerance = " << relative_tolerance
|
|
<< "\nm1 =\n"
|
|
<< m1 << "\nm2 =\n"
|
|
<< m2 << "\ndelta=\n"
|
|
<< (m1 - m2);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return ::testing::AssertionSuccess() << "m1 =\n"
|
|
<< m1
|
|
<< "\nis approximately equal to m2 =\n"
|
|
<< m2;
|
|
}
|
|
|
|
} // namespace fcl
|
|
|
|
#endif // FCL_EIGEN_MATRIX_COMPARE_H
|