Kindr
Kinematics and Dynamics for Robotics
assert_macros_eigen.hpp File Reference
#include <cmath>
#include "assert_macros.hpp"
#include <Eigen/Core>
#include "common.hpp"
Include dependency graph for assert_macros_eigen.hpp:
This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Namespaces

 kindr
 

Macros

#define KINDR_ASSERT_MATRIX_NEAR_DBG(exceptionType, A, B, PERCENT_TOLERANCE, MSG)
 
#define KINDR_ASSERT_SCALAR_NEAR_DBG(exceptionType, A, B, PERCENT_TOLERANCE, MESSAGE)
 
#define PRINT(MESSAGE)   std::cout << MESSAGE << std::endl;
 
#define KINDR_ASSERT_MAT_IS_FINITE(exceptionType, matrix, message)
 
#define KINDR_ASSERT_MAT_IS_FINITE_DBG(exceptionType, matrix, message)
 

Functions

template<typename SCALAR , typename SCALAR2 , typename SCALAR3 >
bool kindr::compareRelative (SCALAR a, SCALAR2 b, SCALAR3 percentTolerance, SCALAR *percentError=NULL, SCALAR bothZeroThreshold=SCALAR(1.0e-15))
 
template<typename SCALAR >
bool kindr::compareRelativePeriodic (SCALAR a, SCALAR b, double periodlength, double percentTolerance, SCALAR *percentError=NULL)
 

Macro Definition Documentation

◆ KINDR_ASSERT_MAT_IS_FINITE

#define KINDR_ASSERT_MAT_IS_FINITE (   exceptionType,
  matrix,
  message 
)
Value:
{ \
for(int r = 0; r < matrix.rows(); ++r) \
{ \
for(int c = 0; c < matrix.cols(); ++c) \
{ \
if(!std::isfinite(matrix(r,c))) \
{ \
std::stringstream kindr_assert_stringstream; \
kindr_assert_stringstream << "debug assert( isfinite(" << #matrix << "(" << r << ", " << c << ") ) failed. [ isfinite(" << matrix(r,c) << " ) ]" << message << std::endl << matrix; \
kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,kindr_assert_stringstream.str()); \
} \
} \
} \
}

Definition at line 148 of file assert_macros_eigen.hpp.

◆ KINDR_ASSERT_MAT_IS_FINITE_DBG

#define KINDR_ASSERT_MAT_IS_FINITE_DBG (   exceptionType,
  matrix,
  message 
)
Value:
{ \
for(int r = 0; r < matrix.rows(); ++r) \
{ \
for(int c = 0; c < matrix.cols(); ++c) \
{ \
if(!std::isfinite(matrix(r,c))) \
{ \
std::stringstream kindr_assert_stringstream; \
kindr_assert_stringstream << "assert( isfinite(" << #matrix << "(" << r << ", " << c << ") ) failed. [ isfinite(" << matrix(r,c) << " ) ]" << message << std::endl << matrix; \
kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,kindr_assert_stringstream.str()); \
} \
} \
} \
}

Definition at line 167 of file assert_macros_eigen.hpp.

◆ KINDR_ASSERT_MATRIX_NEAR_DBG

#define KINDR_ASSERT_MATRIX_NEAR_DBG (   exceptionType,
  A,
  B,
  PERCENT_TOLERANCE,
  MSG 
)
Value:
if ((size_t)(A).rows() != (size_t)(B).rows()) { \
std::stringstream kindr_assert_stringstream; \
kindr_assert_stringstream << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B << "\nare not the same size"; \
kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,kindr_assert_stringstream.str()); \
} \
if ((size_t)(A).cols() != (size_t)(B).cols()) { \
std::stringstream kindr_assert_stringstream; \
kindr_assert_stringstream << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B << "\nare not the same size"; \
kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,kindr_assert_stringstream.str()); \
} \
for(int r = 0; r < (A).rows(); r++) \
{ \
for(int c = 0; c < (A).cols(); c++) \
{ \
typedef typename std::remove_reference<decltype(A)>::type::Scalar Scalar; \
Scalar percentError = static_cast<Scalar>(0.0); \
if(!kindr::compareRelative( (A)(r,c), (B)(r,c), PERCENT_TOLERANCE, &percentError)) { \
std::stringstream kindr_assert_stringstream; \
kindr_assert_stringstream << 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; \
kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,kindr_assert_stringstream.str()); \
} \
} \
}
bool compareRelative(SCALAR a, SCALAR2 b, SCALAR3 percentTolerance, SCALAR *percentError=NULL, SCALAR bothZeroThreshold=SCALAR(1.0e-15))

Definition at line 103 of file assert_macros_eigen.hpp.

◆ KINDR_ASSERT_SCALAR_NEAR_DBG

#define KINDR_ASSERT_SCALAR_NEAR_DBG (   exceptionType,
  A,
  B,
  PERCENT_TOLERANCE,
  MESSAGE 
)
Value:
decltype(A) percentError = static_cast<decltype(A)>(0.0); \
if(!kindr::compareRelative( (A), (B), PERCENT_TOLERANCE, &percentError)) \
{ \
std::stringstream kindr_assert_stringstream; \
kindr_assert_stringstream << MESSAGE << "\nComparing Scalars:\n" \
<< "Scalar 1: " << #A << " = " << (A) << std::endl \
<< "Scalar 2: " << #B << " = " << (B) << std::endl \
<< "Error was " << percentError << "% > " << PERCENT_TOLERANCE << "%\n"; \
kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,kindr_assert_stringstream.str()); \
}
bool compareRelative(SCALAR a, SCALAR2 b, SCALAR3 percentTolerance, SCALAR *percentError=NULL, SCALAR bothZeroThreshold=SCALAR(1.0e-15))

Definition at line 131 of file assert_macros_eigen.hpp.

◆ PRINT

#define PRINT (   MESSAGE)    std::cout << MESSAGE << std::endl;

Definition at line 142 of file assert_macros_eigen.hpp.