.. _program_listing_file__tmp_ws_src_grid_map_grid_map_core_include_grid_map_core_gtest_eigen.hpp:

Program Listing for File gtest_eigen.hpp
========================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_grid_map_grid_map_core_include_grid_map_core_gtest_eigen.hpp>` (``/tmp/ws/src/grid_map/grid_map_core/include/grid_map_core/gtest_eigen.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   
   #ifndef GRID_MAP_CORE__GTEST_EIGEN_HPP_
   #define GRID_MAP_CORE__GTEST_EIGEN_HPP_
   
   #include <gtest/gtest.h>
   #include <Eigen/Core>
   #include <iostream>
   #include <cmath>
   #include <string>
   #include <algorithm>
   
   namespace grid_map
   {
   
   template<int N>
   Eigen::Matrix<double, N, N> randomCovariance()
   {
     Eigen::Matrix<double, N, N> U;
     U.setRandom();
     return U.transpose() * U + 5.0 * Eigen::Matrix<double, N, N>::Identity();
   }
   
   inline Eigen::MatrixXd randomCovarianceXd(int N)
   {
     Eigen::MatrixXd U(N, N);
     U.setRandom();
     return U.transpose() * U + 5.0 * Eigen::MatrixXd::Identity(N, N);
   }
   
   template<typename M1, typename M2>
   void assertEqual(const M1 & A, const M2 & B, std::string const & message = "")
   {
     ASSERT_EQ(
       (size_t)A.rows(),
       (size_t)B.rows()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B <<
       "\nare not the same\n";
     ASSERT_EQ(
       (size_t)A.cols(),
       (size_t)B.cols()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B <<
       "\nare not the same\n";
   
     for (int r = 0; r < A.rows(); r++) {
       for (int c = 0; c < A.cols(); c++) {
         if (std::isnan(A(r, c))) {
           ASSERT_TRUE(std::isnan(B(r, c)));
         } else {
           ASSERT_EQ(
             A(r, c),
             B(r, c)) << message << "\nEquality comparison failed at (" << r << "," << c << ")\n" <<
             "\nMatrix A:\n" << A << "\nand matrix B\n" << B;
         }
       }
     }
   }
   
   template<typename M1, typename M2, typename T>
   void assertNear(const M1 & A, const M2 & B, T tolerance, std::string const & message = "")
   {
     // Note: If these assertions fail, they only abort this subroutine.
     // see: http://code.google.com/p/googletest/wiki/AdvancedGuide#Using_Assertions_in_Sub-routines
     // \todo better handling of this
     ASSERT_EQ(
       (size_t)A.rows(),
       (size_t)B.rows()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B <<
       "\nare not the same\n";
     ASSERT_EQ(
       (size_t)A.cols(),
       (size_t)B.cols()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B <<
       "\nare not the same\n";
   
     for (int r = 0; r < A.rows(); r++) {
       for (int c = 0; c < A.cols(); c++) {
         if (std::isnan(A(r, c))) {
           ASSERT_TRUE(std::isnan(B(r, c)));
         } else {
           ASSERT_NEAR(
             A(r, c), B(r, c),
             tolerance) << message << "\nTolerance comparison failed at (" << r << "," << c <<
             ")\n" <<
             "\nMatrix A:\n" << A << "\nand matrix B\n" << B;
         }
       }
     }
   }
   
   template<typename M1, typename M2, typename T>
   void expectNear(const M1 & A, const M2 & B, T tolerance, std::string const & message = "")
   {
     EXPECT_EQ(
       (size_t)A.rows(),
       (size_t)B.rows()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B <<
       "\nare not the same\n";
     EXPECT_EQ(
       (size_t)A.cols(),
       (size_t)B.cols()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B <<
       "\nare not the same\n";
   
     for (int r = 0; r < A.rows(); r++) {
       for (int c = 0; c < A.cols(); c++) {
         if (std::isnan(A(r, c))) {
           EXPECT_TRUE(std::isnan(B(r, c)));
         } else {
           EXPECT_NEAR(
             A(r, c), B(r, c),
             tolerance) << message << "\nTolerance comparison failed at (" << r << "," << c <<
             ")\n" <<
             "\nMatrix A:\n" << A << "\nand matrix B\n" << B;
         }
       }
     }
   }
   
   template<typename M1>
   void assertFinite(const M1 & A, std::string const & /* message */)
   {
     for (int r = 0; r < A.rows(); r++) {
       for (int c = 0; c < A.cols(); c++) {
         ASSERT_TRUE(
           std::isfinite(
             A(
               r,
               c))) << std::endl << "Check for finite values failed at A(" << r << "," << c <<
           "). Matrix A:" << std::endl << A << std::endl;
       }
     }
   }
   
   inline bool compareRelative(
     double a, double b, double percentTolerance,
     double * percentError = NULL)
   {
     // \todo: does anyone have a better idea?
     double fa = fabs(a);
     double fb = fabs(b);
     if ( (fa < 1e-15 && fb < 1e-15) ||     // Both zero.
       (fa == 0.0 && fb < 1e-6) ||                  // One exactly zero and the other small
       (fb == 0.0 && fa < 1e-6) )                   // ditto
     {
       return true;
     }
   
     double diff = fabs(a - b) / std::max(fa, fb);
     if (diff > percentTolerance * 1e-2) {
       if (percentError) {
         *percentError = diff * 100.0;
       }
       return false;
     }
     return true;
   }
   
   #define ASSERT_DOUBLE_MX_EQ(A, B, PERCENT_TOLERANCE, MSG) \
     ASSERT_EQ( \
       (size_t)(A).rows(), \
       (size_t)(B).rows()) << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << \
       "\n" << B << "\nare not the same size"; \
     ASSERT_EQ( \
       (size_t)(A).cols(), \
       (size_t)(B).cols()) << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << \
       "\n" << B << "\nare not the same size"; \
     for (int r = 0; r < (A).rows(); r++) \
     { \
       for (int c = 0; c < (A).cols(); c++) \
       { \
         double percentError = 0.0; \
         ASSERT_TRUE( \
           grid_map::compareRelative( \
             (A)(r, c), (B)(r, c), PERCENT_TOLERANCE, \
             &percentError)) \
           << MSG << "\nComparing:\n" \
           << #A << "(" << r << "," << c << ") = " << (A)(r, c) << std::endl \
           << #B << "(" << r << "," << c << ") = " << (B)(r, c) << std::endl \
           << "Error was " << percentError << "% > " << PERCENT_TOLERANCE << "%\n" \
           << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B; \
       } \
     }
   
   #define ASSERT_DOUBLE_SPARSE_MX_EQ(A, B, PERCENT_TOLERANCE, MSG) \
     ASSERT_EQ( \
       (size_t)(A).rows(), \
       (size_t)(B).rows()) << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << \
       "\n" << B << "\nare not the same size"; \
     ASSERT_EQ( \
       (size_t)(A).cols(), \
       (size_t)(B).cols()) << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << \
       "\n" << B << "\nare not the same size"; \
     for (int r = 0; r < (A).rows(); r++) \
     { \
       for (int c = 0; c < (A).cols(); c++) \
       { \
         double percentError = 0.0; \
         ASSERT_TRUE( \
           grid_map::compareRelative( \
             (A).coeff(r, c), (B).coeff(r, c), PERCENT_TOLERANCE, \
             &percentError)) \
           << MSG << "\nComparing:\n" \
           << #A << "(" << r << "," << c << ") = " << (A).coeff(r, c) << std::endl \
           << #B << "(" << r << "," << c << ") = " << (B).coeff(r, c) << std::endl \
           << "Error was " << percentError << "% > " << PERCENT_TOLERANCE << "%\n" \
           << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B; \
       } \
     }
   
   }  // namespace grid_map
   #endif  // GRID_MAP_CORE__GTEST_EIGEN_HPP_