1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2015 Tal Hadad <[email protected]>
5 //
6 // This Source Code Form is subject to the terms of the Mozilla
7 // Public License v. 2.0. If a copy of the MPL was not distributed
8 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9
10 #include "main.h"
11
12 #include <unsupported/Eigen/EulerAngles>
13
14 using namespace Eigen;
15
16 // Unfortunately, we need to specialize it in order to work. (We could add it in main.h test framework)
17 template <typename Scalar, class System>
verifyIsApprox(const Eigen::EulerAngles<Scalar,System> & a,const Eigen::EulerAngles<Scalar,System> & b)18 bool verifyIsApprox(const Eigen::EulerAngles<Scalar, System>& a, const Eigen::EulerAngles<Scalar, System>& b)
19 {
20 return verifyIsApprox(a.angles(), b.angles());
21 }
22
23 // Verify that x is in the approxed range [a, b]
24 #define VERIFY_APPROXED_RANGE(a, x, b) \
25 do { \
26 VERIFY_IS_APPROX_OR_LESS_THAN(a, x); \
27 VERIFY_IS_APPROX_OR_LESS_THAN(x, b); \
28 } while(0)
29
30 const char X = EULER_X;
31 const char Y = EULER_Y;
32 const char Z = EULER_Z;
33
34 template<typename Scalar, class EulerSystem>
verify_euler(const EulerAngles<Scalar,EulerSystem> & e)35 void verify_euler(const EulerAngles<Scalar, EulerSystem>& e)
36 {
37 typedef EulerAngles<Scalar, EulerSystem> EulerAnglesType;
38 typedef Matrix<Scalar,3,3> Matrix3;
39 typedef Matrix<Scalar,3,1> Vector3;
40 typedef Quaternion<Scalar> QuaternionType;
41 typedef AngleAxis<Scalar> AngleAxisType;
42
43 const Scalar ONE = Scalar(1);
44 const Scalar HALF_PI = Scalar(EIGEN_PI / 2);
45 const Scalar PI = Scalar(EIGEN_PI);
46
47 // It's very important calc the acceptable precision depending on the distance from the pole.
48 const Scalar longitudeRadius = std::abs(
49 EulerSystem::IsTaitBryan ?
50 std::cos(e.beta()) :
51 std::sin(e.beta())
52 );
53 Scalar precision = test_precision<Scalar>() / longitudeRadius;
54
55 Scalar betaRangeStart, betaRangeEnd;
56 if (EulerSystem::IsTaitBryan)
57 {
58 betaRangeStart = -HALF_PI;
59 betaRangeEnd = HALF_PI;
60 }
61 else
62 {
63 if (!EulerSystem::IsBetaOpposite)
64 {
65 betaRangeStart = 0;
66 betaRangeEnd = PI;
67 }
68 else
69 {
70 betaRangeStart = -PI;
71 betaRangeEnd = 0;
72 }
73 }
74
75 const Vector3 I_ = EulerAnglesType::AlphaAxisVector();
76 const Vector3 J_ = EulerAnglesType::BetaAxisVector();
77 const Vector3 K_ = EulerAnglesType::GammaAxisVector();
78
79 // Is approx checks
80 VERIFY(e.isApprox(e));
81 VERIFY_IS_APPROX(e, e);
82 VERIFY_IS_NOT_APPROX(e, EulerAnglesType(e.alpha() + ONE, e.beta() + ONE, e.gamma() + ONE));
83
84 const Matrix3 m(e);
85 VERIFY_IS_APPROX(Scalar(m.determinant()), ONE);
86
87 EulerAnglesType ebis(m);
88
89 // When no roll(acting like polar representation), we have the best precision.
90 // One of those cases is when the Euler angles are on the pole, and because it's singular case,
91 // the computation returns no roll.
92 if (ebis.beta() == 0)
93 precision = test_precision<Scalar>();
94
95 // Check that eabis in range
96 VERIFY_APPROXED_RANGE(-PI, ebis.alpha(), PI);
97 VERIFY_APPROXED_RANGE(betaRangeStart, ebis.beta(), betaRangeEnd);
98 VERIFY_APPROXED_RANGE(-PI, ebis.gamma(), PI);
99
100 const Matrix3 mbis(AngleAxisType(ebis.alpha(), I_) * AngleAxisType(ebis.beta(), J_) * AngleAxisType(ebis.gamma(), K_));
101 VERIFY_IS_APPROX(Scalar(mbis.determinant()), ONE);
102 VERIFY_IS_APPROX(mbis, ebis.toRotationMatrix());
103 /*std::cout << "===================\n" <<
104 "e: " << e << std::endl <<
105 "eabis: " << eabis.transpose() << std::endl <<
106 "m: " << m << std::endl <<
107 "mbis: " << mbis << std::endl <<
108 "X: " << (m * Vector3::UnitX()).transpose() << std::endl <<
109 "X: " << (mbis * Vector3::UnitX()).transpose() << std::endl;*/
110 VERIFY(m.isApprox(mbis, precision));
111
112 // Test if ea and eabis are the same
113 // Need to check both singular and non-singular cases
114 // There are two singular cases.
115 // 1. When I==K and sin(ea(1)) == 0
116 // 2. When I!=K and cos(ea(1)) == 0
117
118 // TODO: Make this test work well, and use range saturation function.
119 /*// If I==K, and ea[1]==0, then there no unique solution.
120 // The remark apply in the case where I!=K, and |ea[1]| is close to +-pi/2.
121 if( (i!=k || ea[1]!=0) && (i==k || !internal::isApprox(abs(ea[1]),Scalar(EIGEN_PI/2),test_precision<Scalar>())) )
122 VERIFY_IS_APPROX(ea, eabis);*/
123
124 // Quaternions
125 const QuaternionType q(e);
126 ebis = q;
127 const QuaternionType qbis(ebis);
128 VERIFY(internal::isApprox<Scalar>(std::abs(q.dot(qbis)), ONE, precision));
129 //VERIFY_IS_APPROX(eabis, eabis2);// Verify that the euler angles are still the same
130
131 // A suggestion for simple product test when will be supported.
132 /*EulerAnglesType e2(PI/2, PI/2, PI/2);
133 Matrix3 m2(e2);
134 VERIFY_IS_APPROX(e*e2, m*m2);*/
135 }
136
137 template<signed char A, signed char B, signed char C, typename Scalar>
verify_euler_vec(const Matrix<Scalar,3,1> & ea)138 void verify_euler_vec(const Matrix<Scalar,3,1>& ea)
139 {
140 verify_euler(EulerAngles<Scalar, EulerSystem<A, B, C> >(ea[0], ea[1], ea[2]));
141 }
142
143 template<signed char A, signed char B, signed char C, typename Scalar>
verify_euler_all_neg(const Matrix<Scalar,3,1> & ea)144 void verify_euler_all_neg(const Matrix<Scalar,3,1>& ea)
145 {
146 verify_euler_vec<+A,+B,+C>(ea);
147 verify_euler_vec<+A,+B,-C>(ea);
148 verify_euler_vec<+A,-B,+C>(ea);
149 verify_euler_vec<+A,-B,-C>(ea);
150
151 verify_euler_vec<-A,+B,+C>(ea);
152 verify_euler_vec<-A,+B,-C>(ea);
153 verify_euler_vec<-A,-B,+C>(ea);
154 verify_euler_vec<-A,-B,-C>(ea);
155 }
156
check_all_var(const Matrix<Scalar,3,1> & ea)157 template<typename Scalar> void check_all_var(const Matrix<Scalar,3,1>& ea)
158 {
159 verify_euler_all_neg<X,Y,Z>(ea);
160 verify_euler_all_neg<X,Y,X>(ea);
161 verify_euler_all_neg<X,Z,Y>(ea);
162 verify_euler_all_neg<X,Z,X>(ea);
163
164 verify_euler_all_neg<Y,Z,X>(ea);
165 verify_euler_all_neg<Y,Z,Y>(ea);
166 verify_euler_all_neg<Y,X,Z>(ea);
167 verify_euler_all_neg<Y,X,Y>(ea);
168
169 verify_euler_all_neg<Z,X,Y>(ea);
170 verify_euler_all_neg<Z,X,Z>(ea);
171 verify_euler_all_neg<Z,Y,X>(ea);
172 verify_euler_all_neg<Z,Y,Z>(ea);
173 }
174
check_singular_cases(const Scalar & singularBeta)175 template<typename Scalar> void check_singular_cases(const Scalar& singularBeta)
176 {
177 typedef Matrix<Scalar,3,1> Vector3;
178 const Scalar PI = Scalar(EIGEN_PI);
179
180 for (Scalar epsilon = NumTraits<Scalar>::epsilon(); epsilon < 1; epsilon *= Scalar(1.2))
181 {
182 check_all_var(Vector3(PI/4, singularBeta, PI/3));
183 check_all_var(Vector3(PI/4, singularBeta - epsilon, PI/3));
184 check_all_var(Vector3(PI/4, singularBeta - Scalar(1.5)*epsilon, PI/3));
185 check_all_var(Vector3(PI/4, singularBeta - 2*epsilon, PI/3));
186 check_all_var(Vector3(PI*Scalar(0.8), singularBeta - epsilon, Scalar(0.9)*PI));
187 check_all_var(Vector3(PI*Scalar(-0.9), singularBeta + epsilon, PI*Scalar(0.3)));
188 check_all_var(Vector3(PI*Scalar(-0.6), singularBeta + Scalar(1.5)*epsilon, PI*Scalar(0.3)));
189 check_all_var(Vector3(PI*Scalar(-0.5), singularBeta + 2*epsilon, PI*Scalar(0.4)));
190 check_all_var(Vector3(PI*Scalar(0.9), singularBeta + epsilon, Scalar(0.8)*PI));
191 }
192
193 // This one for sanity, it had a problem with near pole cases in float scalar.
194 check_all_var(Vector3(PI*Scalar(0.8), singularBeta - Scalar(1E-6), Scalar(0.9)*PI));
195 }
196
eulerangles_manual()197 template<typename Scalar> void eulerangles_manual()
198 {
199 typedef Matrix<Scalar,3,1> Vector3;
200 typedef Matrix<Scalar,Dynamic,1> VectorX;
201 const Vector3 Zero = Vector3::Zero();
202 const Scalar PI = Scalar(EIGEN_PI);
203
204 check_all_var(Zero);
205
206 // singular cases
207 check_singular_cases(PI/2);
208 check_singular_cases(-PI/2);
209
210 check_singular_cases(Scalar(0));
211 check_singular_cases(Scalar(-0));
212
213 check_singular_cases(PI);
214 check_singular_cases(-PI);
215
216 // non-singular cases
217 VectorX alpha = VectorX::LinSpaced(20, Scalar(-0.99) * PI, PI);
218 VectorX beta = VectorX::LinSpaced(20, Scalar(-0.49) * PI, Scalar(0.49) * PI);
219 VectorX gamma = VectorX::LinSpaced(20, Scalar(-0.99) * PI, PI);
220 for (int i = 0; i < alpha.size(); ++i) {
221 for (int j = 0; j < beta.size(); ++j) {
222 for (int k = 0; k < gamma.size(); ++k) {
223 check_all_var(Vector3(alpha(i), beta(j), gamma(k)));
224 }
225 }
226 }
227 }
228
eulerangles_rand()229 template<typename Scalar> void eulerangles_rand()
230 {
231 typedef Matrix<Scalar,3,3> Matrix3;
232 typedef Matrix<Scalar,3,1> Vector3;
233 typedef Array<Scalar,3,1> Array3;
234 typedef Quaternion<Scalar> Quaternionx;
235 typedef AngleAxis<Scalar> AngleAxisType;
236
237 Scalar a = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI));
238 Quaternionx q1;
239 q1 = AngleAxisType(a, Vector3::Random().normalized());
240 Matrix3 m;
241 m = q1;
242
243 Vector3 ea = m.eulerAngles(0,1,2);
244 check_all_var(ea);
245 ea = m.eulerAngles(0,1,0);
246 check_all_var(ea);
247
248 // Check with purely random Quaternion:
249 q1.coeffs() = Quaternionx::Coefficients::Random().normalized();
250 m = q1;
251 ea = m.eulerAngles(0,1,2);
252 check_all_var(ea);
253 ea = m.eulerAngles(0,1,0);
254 check_all_var(ea);
255
256 // Check with random angles in range [0:pi]x[-pi:pi]x[-pi:pi].
257 ea = (Array3::Random() + Array3(1,0,0))*Scalar(EIGEN_PI)*Array3(0.5,1,1);
258 check_all_var(ea);
259
260 ea[2] = ea[0] = internal::random<Scalar>(0,Scalar(EIGEN_PI));
261 check_all_var(ea);
262
263 ea[0] = ea[1] = internal::random<Scalar>(0,Scalar(EIGEN_PI));
264 check_all_var(ea);
265
266 ea[1] = 0;
267 check_all_var(ea);
268
269 ea.head(2).setZero();
270 check_all_var(ea);
271
272 ea.setZero();
273 check_all_var(ea);
274 }
275
EIGEN_DECLARE_TEST(EulerAngles)276 EIGEN_DECLARE_TEST(EulerAngles)
277 {
278 // Simple cast test
279 EulerAnglesXYZd onesEd(1, 1, 1);
280 EulerAnglesXYZf onesEf = onesEd.cast<float>();
281 VERIFY_IS_APPROX(onesEd, onesEf.cast<double>());
282
283 // Simple Construction from Vector3 test
284 VERIFY_IS_APPROX(onesEd, EulerAnglesXYZd(Vector3d::Ones()));
285
286 CALL_SUBTEST_1( eulerangles_manual<float>() );
287 CALL_SUBTEST_2( eulerangles_manual<double>() );
288
289 for(int i = 0; i < g_repeat; i++) {
290 CALL_SUBTEST_3( eulerangles_rand<float>() );
291 CALL_SUBTEST_4( eulerangles_rand<double>() );
292 }
293
294 // TODO: Add tests for auto diff
295 // TODO: Add tests for complex numbers
296 }
297