Kindr
Kinematics and Dynamics for Robotics
gtest_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 #pragma once
29 
30 #include <type_traits>
31 #include <gtest/gtest.h>
32 #include "source_file_pos.hpp"
33 #include "assert_macros_eigen.hpp"
34 #include <Eigen/Core>
35 
36 namespace kindr {
37 
38 //TODO we should have a better place for test tools - especially libeigen independent ones
39 template <typename T>
41  // We need the shortest angle to 1 or -1, as the both represent rotation identity.
42  // If the real part is nonnegative we can measure to 1 otherwise we should measure to -1.
43  // The second case will have the same distance to -1 as the the negated quaternion to 1.
44  // This means we can just look at the absolute value of the real part (the value we calculate the distance based on).
45  const double wAbs = fabs(quat.w());
46  // by a simple geometric intuition we get the distance to -1 in the unit quaternions (along a great circle) as acos(wAbs).
47  // As the distance in rotations is always twice as big we finally get:
48  return acos(wAbs)*2;
49 }
50 
51 template <typename T>
52 double calcRotationQuatDisparityAngle(const T & q1, const T & q2){
53  return calcRotationQuatDisparityAngleToIdentity(q1.inverted() * q2);
54 }
55 
56 
57 
58 template<typename M1, typename M2>
59 void assertEqual(const M1 & A, const M2 & B, kindr::internal::source_file_pos const & sfp, std::string const & message = "")
60 {
61  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" << sfp.toString();
62  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" << sfp.toString();
63 
64  for(int r = 0; r < A.rows(); r++)
65  {
66  for(int c = 0; c < A.cols(); c++)
67  {
68  ASSERT_EQ(A(r,c),B(r,c)) << message << "\nEquality comparison failed at (" << r << "," << c << ")\n" << sfp.toString()
69  << "\nMatrix A:\n" << A << "\nand matrix B\n" << B;
70  }
71  }
72 }
73 
74 
75 template<typename M1, typename M2, typename T>
76 void assertNear(const M1 & A, const M2 & B, T tolerance, kindr::internal::source_file_pos const & sfp, std::string const & message = "")
77 {
78  // Note: If these assertions fail, they only abort this subroutine.
79  // see: http://code.google.com/p/googletest/wiki/AdvancedGuide#Using_Assertions_in_Sub-routines
80  // \todo better handling of this
81  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" << sfp.toString();
82  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" << sfp.toString();
83 
84  for(int r = 0; r < A.rows(); r++)
85  {
86  for(int c = 0; c < A.cols(); c++)
87  {
88  ASSERT_NEAR(A(r,c),B(r,c),tolerance) << message << "\nTolerance comparison failed at (" << r << "," << c << ")\n" << sfp.toString()
89  << "\nMatrix A:\n" << A << "\nand matrix B\n" << B;
90  }
91  }
92 }
93 
94 template<typename M1, typename M2, typename T>
95 void expectNear(const M1 & A, const M2 & B, T tolerance, kindr::internal::source_file_pos const & sfp, std::string const & message = "")
96 {
97  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" << sfp.toString();
98  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" << sfp.toString();
99 
100  for(int r = 0; r < A.rows(); r++)
101  {
102  for(int c = 0; c < A.cols(); c++)
103  {
104  EXPECT_NEAR(A(r,c),B(r,c),tolerance) << message << "\nTolerance comparison failed at (" << r << "," << c << ")\n" << sfp.toString()
105  << "\nMatrix A:\n" << A << "\nand matrix B\n" << B;
106  }
107  }
108 }
109 
110 
111 template<typename M1>
112 void assertFinite(const M1 & A, kindr::internal::source_file_pos const & sfp, std::string const & message = "")
113 {
114  for(int r = 0; r < A.rows(); r++)
115  {
116  for(int c = 0; c < A.cols(); c++)
117  {
118  ASSERT_TRUE(std::isfinite(A(r,c))) << sfp.toString() << std::endl << "Check for finite values failed at A(" << r << "," << c << "). Matrix A:" << std::endl << A << std::endl;
119  }
120  }
121 }
122 
123 #define KINDR_ASSERT_DOUBLE_MX_EQ_ZT(A, B, PERCENT_TOLERANCE, MSG, ZERO_THRESHOLD) \
124  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"; \
125  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"; \
126  for(int r = 0; r < (A).rows(); r++) \
127  { \
128  for(int c = 0; c < (A).cols(); c++) \
129  { \
130  typedef typename std::remove_reference<decltype(A)>::type::Scalar Scalar; \
131  Scalar percentError = static_cast<Scalar>(0.0); \
132  ASSERT_TRUE(kindr::compareRelative( (A)(r,c), (B)(r,c), PERCENT_TOLERANCE, &percentError, ZERO_THRESHOLD)) \
133  << MSG << "\nComparing:\n" \
134  << #A << "(" << r << "," << c << ") = " << (A)(r,c) << std::endl \
135  << #B << "(" << r << "," << c << ") = " << (B)(r,c) << std::endl \
136  << "Error was " << percentError << "% > " << PERCENT_TOLERANCE << "%\n" \
137  << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B; \
138  } \
139  }
140 
141 #define KINDR_ASSERT_DOUBLE_MX_EQ(A, B, PERCENT_TOLERANCE, MSG) \
142  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"; \
143  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"; \
144  for(int r = 0; r < (A).rows(); r++) \
145  { \
146  for(int c = 0; c < (A).cols(); c++) \
147  { \
148  typedef typename std::remove_reference<decltype(A)>::type::Scalar Scalar; \
149  Scalar percentError = static_cast<Scalar>(0.0); \
150  ASSERT_TRUE(kindr::compareRelative( (A)(r,c), (B)(r,c), PERCENT_TOLERANCE, &percentError)) \
151  << MSG << "\nComparing:\n" \
152  << #A << "(" << r << "," << c << ") = " << (A)(r,c) << std::endl \
153  << #B << "(" << r << "," << c << ") = " << (B)(r,c) << std::endl \
154  << "Error was " << percentError << "% > " << PERCENT_TOLERANCE << "%\n" \
155  << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B; \
156  } \
157  }
158 
160 #define KINDR_ASSERT_DOUBLE_MX_EQ_ABS_REL(M_TRUE, M_MEAS, ATOL, RTOL, MSG) \
161  ASSERT_EQ((size_t)(M_TRUE).rows(), (size_t)(M_MEAS).rows()) << MSG << "\nMatrix " << #M_TRUE << ":\n" << M_TRUE << "\nand matrix " << #M_MEAS << "\n" << M_MEAS << "\nare not the same size"; \
162  ASSERT_EQ((size_t)(M_TRUE).cols(), (size_t)(M_MEAS).cols()) << MSG << "\nMatrix " << #M_TRUE << ":\n" << M_TRUE << "\nand matrix " << #M_MEAS << "\n" << M_MEAS << "\nare not the same size"; \
163  for(int r = 0; r < (M_TRUE).rows(); r++) \
164  { \
165  for(int c = 0; c < (M_TRUE).cols(); c++) \
166  { \
167  typedef typename std::remove_reference<decltype(M_TRUE)>::type::Scalar Scalar; \
168  ASSERT_TRUE(fabs((M_MEAS)(r,c) - (M_TRUE)(r,c)) <= (fabs((M_TRUE)(r,c))*RTOL + ATOL)) \
169  << MSG << "\nComparing:\n" \
170  << #M_TRUE << "(" << r << "," << c << ") = " << (M_TRUE)(r,c) << std::endl \
171  << #M_MEAS << "(" << r << "," << c << ") = " << (M_MEAS)(r,c) << std::endl \
172  << "Absolute error was " << fabs((M_MEAS)(r,c) - (M_TRUE)(r,c)) << " > " << ATOL << "\n" \
173  << "Relative error was " << fabs((M_MEAS)(r,c) - (M_TRUE)(r,c))/fabs((M_TRUE)(r,c)) << " > " << RTOL << "\n" \
174  << "\nMatrix " << #M_TRUE << ":\n" << M_TRUE << "\nand matrix " << #M_MEAS << "\n" << M_MEAS; \
175  } \
176  }
177 
178 #define KINDR_ASSERT_DOUBLE_MX_EQ_PERIODIC(A, B, PERIODLENGTH, PERCENT_TOLERANCE, MSG) \
179  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"; \
180  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"; \
181  for(int r = 0; r < (A).rows(); r++) \
182  { \
183  for(int c = 0; c < (A).cols(); c++) \
184  { \
185  typedef typename std::remove_reference<decltype(A)>::type::Scalar Scalar; \
186  Scalar percentError = static_cast<Scalar>(0.0); \
187  ASSERT_TRUE(kindr::compareRelativePeriodic( (A)(r,c), (B)(r,c), PERIODLENGTH, PERCENT_TOLERANCE, &percentError)) \
188  << MSG << "\nComparing:\n" \
189  << #A << "(" << r << "," << c << ") = " << (A)(r,c) << std::endl \
190  << #B << "(" << r << "," << c << ") = " << (B)(r,c) << std::endl \
191  << "Error was " << percentError << "% > " << PERCENT_TOLERANCE << "%\n" \
192  << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B; \
193  } \
194  }
195 
196 
197 
198 
199 } // namespace kindr
200 
201 
double calcRotationQuatDisparityAngle(const T &q1, const T &q2)
Definition: gtest_eigen.hpp:52
void assertFinite(const M1 &A, kindr::internal::source_file_pos const &sfp, std::string const &message="")
void expectNear(const M1 &A, const M2 &B, T tolerance, kindr::internal::source_file_pos const &sfp, std::string const &message="")
Definition: gtest_eigen.hpp:95
void assertEqual(const M1 &A, const M2 &B, kindr::internal::source_file_pos const &sfp, std::string const &message="")
Definition: gtest_eigen.hpp:59
void assertNear(const M1 &A, const M2 &B, T tolerance, kindr::internal::source_file_pos const &sfp, std::string const &message="")
Definition: gtest_eigen.hpp:76
double calcRotationQuatDisparityAngleToIdentity(const T &quat)
Definition: gtest_eigen.hpp:40