test/EulerAngles.cpp
Go to the documentation of this file.
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2015 Tal Hadad <tal_hd@hotmail.com>
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>
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>
36 {
37  typedef EulerAngles<Scalar, EulerSystem> EulerAnglesType;
38  typedef Matrix<Scalar,3,3> Matrix3;
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(
50  std::cos(e.beta()) :
51  std::sin(e.beta())
52  );
53  Scalar precision = test_precision<Scalar>() / longitudeRadius;
54 
55  Scalar betaRangeStart, betaRangeEnd;
57  {
58  betaRangeStart = -HALF_PI;
59  betaRangeEnd = HALF_PI;
60  }
61  else
62  {
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>
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>
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 
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 
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 
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 
197 template<typename Scalar> void eulerangles_manual()
198 {
199  typedef Matrix<Scalar,3,1> Vector3;
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 
212 
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 
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 
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 }
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
Definition: bench_gemm.cpp:49
Matrix3f m
SCALAR Scalar
Definition: bench_gemm.cpp:46
const char Y
#define CALL_SUBTEST_4(FUNC)
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
Represents a fixed Euler rotation system.
Definition: EulerSystem.h:126
Scalar * b
Definition: benchVecAdd.cpp:17
Eigen::Vector3d Vector3
Definition: Vector.h:43
bool isApprox(const EulerAngles &other, const RealScalar &prec=NumTraits< Scalar >::dummy_precision()) const
#define EIGEN_PI
void check_singular_cases(const Scalar &singularBeta)
#define CALL_SUBTEST_3(FUNC)
Matrix< Scalar, Dynamic, 1 > VectorX
Definition: sparse_lu.cpp:41
#define VERIFY_APPROXED_RANGE(a, x, b)
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
const char X
Holds information about the various numeric (i.e. scalar) types allowed by Eigen. ...
Definition: NumTraits.h:232
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
void verify_euler(const EulerAngles< Scalar, EulerSystem > &e)
void verify_euler_all_neg(const Matrix< Scalar, 3, 1 > &ea)
static double epsilon
Definition: testRot3.cpp:37
EIGEN_DECLARE_TEST(EulerAngles)
const char Z
#define VERIFY_IS_APPROX(a, b)
#define CALL_SUBTEST_1(FUNC)
static int g_repeat
Definition: main.h:169
RealScalar alpha
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Represents a rotation in a 3 dimensional space as three Euler angles.
EIGEN_DEVICE_FUNC const Scalar & q
void eulerangles_manual()
void verify_euler_vec(const Matrix< Scalar, 3, 1 > &ea)
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
DiscreteKey E(5, 2)
void eulerangles_rand()
#define VERIFY(a)
Definition: main.h:380
The quaternion class used to represent 3D orientations and rotations.
cout precision(2)
General-purpose arrays with easy API for coefficient-wise operations.
Definition: Array.h:45
bool verifyIsApprox(const Type1 &a, const Type2 &b)
Definition: main.h:585
#define VERIFY_IS_NOT_APPROX(a, b)
#define CALL_SUBTEST_2(FUNC)
void check_all_var(const Matrix< Scalar, 3, 1 > &ea)
The matrix class, also used for vectors and row-vectors.
#define abs(x)
Definition: datatypes.h:17
Represents a 3D rotation as a rotation angle around an arbitrary 3D axis.
std::ptrdiff_t j


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:12