Kindr
Kinematics and Dynamics for Robotics
assert_macros_eigen.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  * * Redistributions of source code must retain the above copyright
8  * notice, this list of conditions and the following disclaimer.
9  * * Redistributions in binary form must reproduce the above copyright
10  * notice, this list of conditions and the following disclaimer in the
11  * documentation and/or other materials provided with the distribution.
12  * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
13  * names of its contributors may be used to endorse or promote products
14  * derived from this software without specific prior written permission.
15  *
16  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19  * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale,
20  * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
21  * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
22  * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
23  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
24  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26  *
27  */
28 
29 #ifndef ASSERT_MACROS_EIGEN_HPP_
30 #define ASSERT_MACROS_EIGEN_HPP_
31 
32 #include <cmath>
33 #include "assert_macros.hpp"
34 #include <Eigen/Core>
35 
36 #include "common.hpp"
37 
38 
39 namespace kindr {
40 
41 // Importing these from namespace std allows us to inject other overloads
42 // from e.g. Ceres in the same namespace.
43 using std::abs;
44 using std::max;
45 using std::min;
46 
47 template<typename SCALAR, typename SCALAR2, typename SCALAR3>
48 inline bool compareRelative(SCALAR a, SCALAR2 b, SCALAR3 percentTolerance, SCALAR * percentError = NULL, SCALAR bothZeroThreshold = SCALAR(1.0e-15))
49 {
50  // \todo: does anyone have a better idea?
51  SCALAR fa = abs(a);
52  SCALAR fb = abs(b);
53  SCALAR tolerance_zero = static_cast<SCALAR>(1e-6);
54  if( (fa < bothZeroThreshold && fb < bothZeroThreshold) || // Both zero.
55  (fa == static_cast<SCALAR>(0.0) && fb < tolerance_zero) || // One exactly zero and the other small
56  (fb == static_cast<SCALAR>(0.0) && fa < tolerance_zero) ) // ditto
57  return true;
58 
59  SCALAR diff = abs(a - b) / max(fa,fb);
60  if(diff > percentTolerance * static_cast<SCALAR>(1e-2))
61  {
62  if(percentError)
63  *percentError = diff * static_cast<SCALAR>(100.0);
64  return false;
65  }
66  return true;
67 }
68 
69 template<typename SCALAR>
70 inline bool compareRelativePeriodic(SCALAR a, SCALAR b, double periodlength, double percentTolerance, SCALAR * percentError = NULL)
71 {
72  // \todo: does anyone have a better idea?
73  SCALAR fa = floatingPointModulo(a, static_cast<SCALAR>(periodlength)); // a now lies in [0,periodlength)
74  SCALAR fb = floatingPointModulo(b, static_cast<SCALAR>(periodlength)); // b now lies in [0,periodlength)
75  SCALAR tolerance = static_cast<SCALAR>(1e-15);
76  if( ((periodlength - tolerance < fa || fa < tolerance) && (periodlength - tolerance < fb || fb < tolerance)) || // Both zero or near periodlength
77  (fa == static_cast<SCALAR>(0.0) && fb < static_cast<SCALAR>(1e-6)) || // One exactly zero and the other small
78  (fb == static_cast<SCALAR>(0.0) && fa < static_cast<SCALAR>(1e-6)) ) // ditto
79  return true;
80 
81  SCALAR diff = min(floatingPointModulo(a - b, static_cast<SCALAR>(periodlength)), floatingPointModulo(b - a, static_cast<SCALAR>(periodlength)))/static_cast<SCALAR>(periodlength);
82  if(diff > percentTolerance * static_cast<SCALAR>(1e-2))
83  {
84  if(percentError)
85  *percentError = diff * static_cast<SCALAR>(100.0);
86  return false;
87  }
88  return true;
89 }
90 
91 
92 // kindr::assertNear((A),(B),1e-6,(MESSAGE))
93 /* std::cout << MESSAGE << std::endl << "Difference between " << #A << " and " << #B << " exceeds tolerance of " << TOL << "." << std::endl; \
94  std::cout << #A << "(" << i << "," << j << ") = " << (A)(i,j) << std::endl; \
95  std::cout << #B << "(" << i << "," << j << ") = " << (B)(i,j) << std::endl; \ */
96 /* std::cout << MESSAGE << std::endl << "Matrix " << #A << std::endl << A << std::endl << "and matrix " << #B << std::endl << B << std::endl << "have different sizes."; \ */
97 
98 #ifdef NDEBUG
99 #define KINDR_ASSERT_MATRIX_NEAR_DBG(exceptionType, A, B, PERCENT_TOLERANCE, MSG)
100 #define KINDR_ASSERT_SCALAR_NEAR_DBG(exceptionType, A, B, PERCENT_TOLERANCE, MESSAGE)
101 #define PRINT(MESSAGE)
102 #else
103 #define KINDR_ASSERT_MATRIX_NEAR_DBG(exceptionType, A, B, PERCENT_TOLERANCE, MSG) \
104  if ((size_t)(A).rows() != (size_t)(B).rows()) { \
105  std::stringstream kindr_assert_stringstream; \
106  kindr_assert_stringstream << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B << "\nare not the same size"; \
107  kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,kindr_assert_stringstream.str()); \
108  } \
109  if ((size_t)(A).cols() != (size_t)(B).cols()) { \
110  std::stringstream kindr_assert_stringstream; \
111  kindr_assert_stringstream << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B << "\nare not the same size"; \
112  kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,kindr_assert_stringstream.str()); \
113  } \
114  for(int r = 0; r < (A).rows(); r++) \
115  { \
116  for(int c = 0; c < (A).cols(); c++) \
117  { \
118  typedef typename std::remove_reference<decltype(A)>::type::Scalar Scalar; \
119  Scalar percentError = static_cast<Scalar>(0.0); \
120  if(!kindr::compareRelative( (A)(r,c), (B)(r,c), PERCENT_TOLERANCE, &percentError)) { \
121  std::stringstream kindr_assert_stringstream; \
122  kindr_assert_stringstream << MSG << "\nComparing:\n" \
123  << #A << "(" << r << "," << c << ") = " << (A)(r,c) << std::endl \
124  << #B << "(" << r << "," << c << ") = " << (B)(r,c) << std::endl \
125  << "Error was " << percentError << "% > " << PERCENT_TOLERANCE << "%\n" \
126  << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B; \
127  kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,kindr_assert_stringstream.str()); \
128  } \
129  } \
130  }
131 #define KINDR_ASSERT_SCALAR_NEAR_DBG(exceptionType, A, B, PERCENT_TOLERANCE, MESSAGE) \
132  decltype(A) percentError = static_cast<decltype(A)>(0.0); \
133  if(!kindr::compareRelative( (A), (B), PERCENT_TOLERANCE, &percentError)) \
134  { \
135  std::stringstream kindr_assert_stringstream; \
136  kindr_assert_stringstream << MESSAGE << "\nComparing Scalars:\n" \
137  << "Scalar 1: " << #A << " = " << (A) << std::endl \
138  << "Scalar 2: " << #B << " = " << (B) << std::endl \
139  << "Error was " << percentError << "% > " << PERCENT_TOLERANCE << "%\n"; \
140  kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,kindr_assert_stringstream.str()); \
141  }
142 #define PRINT(MESSAGE) std::cout << MESSAGE << std::endl;
143 #endif
144 
145 
146 
147 
148 #define KINDR_ASSERT_MAT_IS_FINITE(exceptionType, matrix, message) \
149  { \
150  for(int r = 0; r < matrix.rows(); ++r) \
151  { \
152  for(int c = 0; c < matrix.cols(); ++c) \
153  { \
154  if(!std::isfinite(matrix(r,c))) \
155  { \
156  std::stringstream kindr_assert_stringstream; \
157  kindr_assert_stringstream << "debug assert( isfinite(" << #matrix << "(" << r << ", " << c << ") ) failed. [ isfinite(" << matrix(r,c) << " ) ]" << message << std::endl << matrix; \
158  kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,kindr_assert_stringstream.str()); \
159  } \
160  } \
161  } \
162  }
163 
164 
165 #ifndef NDEBUG
166 
167 #define KINDR_ASSERT_MAT_IS_FINITE_DBG(exceptionType, matrix, message) \
168  { \
169  for(int r = 0; r < matrix.rows(); ++r) \
170  { \
171  for(int c = 0; c < matrix.cols(); ++c) \
172  { \
173  if(!std::isfinite(matrix(r,c))) \
174  { \
175  std::stringstream kindr_assert_stringstream; \
176  kindr_assert_stringstream << "assert( isfinite(" << #matrix << "(" << r << ", " << c << ") ) failed. [ isfinite(" << matrix(r,c) << " ) ]" << message << std::endl << matrix; \
177  kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,kindr_assert_stringstream.str()); \
178  } \
179  } \
180  } \
181  }
182 
183 
184 #else
185 
186 #define KINDR_ASSERT_MAT_IS_FINITE_DBG(exceptionType, matrix, message)
187 
188 #endif
189 
190 
191 
192 } // namespace kindr
193 
194 
195 
196 #endif /* ASSERT_MACROS_EIGEN_HPP_ */
T floatingPointModulo(T x, T y)
Floating-point modulo.
Definition: common.hpp:46
bool compareRelativePeriodic(SCALAR a, SCALAR b, double periodlength, double percentTolerance, SCALAR *percentError=NULL)
bool compareRelative(SCALAR a, SCALAR2 b, SCALAR3 percentTolerance, SCALAR *percentError=NULL, SCALAR bothZeroThreshold=SCALAR(1.0e-15))