gtest_utilities.h

Go to the documentation of this file.

#ifndef OPEN_WALKER_CORE_GTEST_UTILITIES_H
#define OPEN_WALKER_CORE_GTEST_UTILITIES_H

#include <gtest/gtest.h>
#include <Eigen/Dense>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Vector3.h>
#include <tf/LinearMath/Matrix3x3.h>
#include <tf/LinearMath/Quaternion.h>

namespace ow_test
{

// testing precision
const double PRECISION = 1e-7;

// test Eigen::Matrix m1 and m2 for acceptable error bounds
template <typename _DerivedA,typename _DerivedB>
::testing::AssertionResult eigenMatrixNear(const Eigen::MatrixBase<_DerivedA>& m1, const std::string m1_name, 
                                           const Eigen::MatrixBase<_DerivedB>& m2, const std::string m2_name,
                                           double prec = PRECISION)
{
  if(m1.cols() != m2.cols()) 
  {
    return ::testing::AssertionFailure() 
      << "Differnt number of cols for " << m1_name << " and " << m2_name << ":\n"
      << m1_name << ".cols()" << m1.cols() << ".\n"
      << m2_name << ".cols()" << m2.cols() << ".\n";
  }
  if(m2.rows()!= m2.rows()) 
  {
    return ::testing::AssertionFailure() 
      << "Differnt number of rows for " << m1_name << " and " << m2_name << ":\n"
      << m1_name << ".rows()" << m1.rows() << ".\n"
      << m2_name << ".rows()" << m2.rows() << ".\n";
  }
  if(m1.isApprox(m2,prec))
    return ::testing::AssertionSuccess();
  else
  {
    ::testing::AssertionResult res(false); 
    res << "Detla between Eigen::Matrix " << m1_name << " and Eigen::Matrix " << m2_name << " detected:\n";

    for (int j = 0; j < m1.cols(); ++j) 
    {
      for (int i = 0; i < m1.rows(); ++i) 
      {
        double delta = std::abs(m1(i,j) - m2(i,j));
        if(delta > prec) {
          res << "Detla at entry (" << i << "," << j << "):" << "\n";
          res << m1_name << "(" << i << "," << j << ")=" << m1(i,j) << ".\n";
          res << m2_name << "(" << i << "," << j << ")=" << m2(i,j) << ".\n";
          res << "delta=" << delta << ", prec=" << prec << ".\n";
        }
      }
    }
    return res; 
  }
  return ::testing::AssertionFailure();
}

// test Eigen::Quaternion Q_a and Q_b for acceptable error bounds
// note: compair RotationMatrix (unique)
template <typename _DerivedA, typename _DerivedB>
::testing::AssertionResult eigenQuaternionNear(const Eigen::QuaternionBase<_DerivedA>& Q_a, const std::string q_a_name,
                                               const Eigen::QuaternionBase<_DerivedB>& Q_b, const std::string q_b_name,
                                               double prec = PRECISION)
{
  typedef typename Eigen::QuaternionBase<_DerivedA>::Matrix3 Matrix3A;
  typedef typename Eigen::QuaternionBase<_DerivedB>::Matrix3 Matrix3B;

  Matrix3A R_a = Q_a.toRotationMatrix();
  Matrix3B R_b = Q_b.toRotationMatrix();

  ::testing::AssertionResult res = eigenMatrixNear(R_a, q_a_name, R_b, q_b_name);
  if(!res)
  {
    ::testing::AssertionResult res_wrapped(false);
    res_wrapped << "Detla between Eigen::Quaternion " << q_a_name << " and Eigen::Quaternion " << q_b_name << " detected:\n"; 
    res_wrapped << res.message();
    return res_wrapped;
  }
  return res;
}

}

#endif // OPEN_WALKER_CORE_GTEST_UTILITIES_H