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 template<typename EulerSystem, typename Scalar>
18  bool positiveRangeAlpha, bool positiveRangeBeta, bool positiveRangeGamma)
19 {
20  typedef EulerAngles<Scalar, EulerSystem> EulerAnglesType;
21  typedef Matrix<Scalar,3,3> Matrix3;
23  typedef Quaternion<Scalar> QuaternionType;
24  typedef AngleAxis<Scalar> AngleAxisType;
25  using std::abs;
26 
27  Scalar alphaRangeStart, alphaRangeEnd;
28  Scalar betaRangeStart, betaRangeEnd;
29  Scalar gammaRangeStart, gammaRangeEnd;
30 
31  if (positiveRangeAlpha)
32  {
33  alphaRangeStart = Scalar(0);
34  alphaRangeEnd = Scalar(2 * EIGEN_PI);
35  }
36  else
37  {
38  alphaRangeStart = -Scalar(EIGEN_PI);
39  alphaRangeEnd = Scalar(EIGEN_PI);
40  }
41 
42  if (positiveRangeBeta)
43  {
44  betaRangeStart = Scalar(0);
45  betaRangeEnd = Scalar(2 * EIGEN_PI);
46  }
47  else
48  {
49  betaRangeStart = -Scalar(EIGEN_PI);
50  betaRangeEnd = Scalar(EIGEN_PI);
51  }
52 
53  if (positiveRangeGamma)
54  {
55  gammaRangeStart = Scalar(0);
56  gammaRangeEnd = Scalar(2 * EIGEN_PI);
57  }
58  else
59  {
60  gammaRangeStart = -Scalar(EIGEN_PI);
61  gammaRangeEnd = Scalar(EIGEN_PI);
62  }
63 
64  const int i = EulerSystem::AlphaAxisAbs - 1;
65  const int j = EulerSystem::BetaAxisAbs - 1;
66  const int k = EulerSystem::GammaAxisAbs - 1;
67 
68  const int iFactor = EulerSystem::IsAlphaOpposite ? -1 : 1;
69  const int jFactor = EulerSystem::IsBetaOpposite ? -1 : 1;
70  const int kFactor = EulerSystem::IsGammaOpposite ? -1 : 1;
71 
72  const Vector3 I = EulerAnglesType::AlphaAxisVector();
73  const Vector3 J = EulerAnglesType::BetaAxisVector();
74  const Vector3 K = EulerAnglesType::GammaAxisVector();
75 
76  EulerAnglesType e(ea[0], ea[1], ea[2]);
77 
78  Matrix3 m(e);
79  Vector3 eabis = EulerAnglesType(m, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma).angles();
80 
81  // Check that eabis in range
82  VERIFY(alphaRangeStart <= eabis[0] && eabis[0] <= alphaRangeEnd);
83  VERIFY(betaRangeStart <= eabis[1] && eabis[1] <= betaRangeEnd);
84  VERIFY(gammaRangeStart <= eabis[2] && eabis[2] <= gammaRangeEnd);
85 
86  Vector3 eabis2 = m.eulerAngles(i, j, k);
87 
88  // Invert the relevant axes
89  eabis2[0] *= iFactor;
90  eabis2[1] *= jFactor;
91  eabis2[2] *= kFactor;
92 
93  // Saturate the angles to the correct range
94  if (positiveRangeAlpha && (eabis2[0] < 0))
95  eabis2[0] += Scalar(2 * EIGEN_PI);
96  if (positiveRangeBeta && (eabis2[1] < 0))
97  eabis2[1] += Scalar(2 * EIGEN_PI);
98  if (positiveRangeGamma && (eabis2[2] < 0))
99  eabis2[2] += Scalar(2 * EIGEN_PI);
100 
101  VERIFY_IS_APPROX(eabis, eabis2);// Verify that our estimation is the same as m.eulerAngles() is
102 
103  Matrix3 mbis(AngleAxisType(eabis[0], I) * AngleAxisType(eabis[1], J) * AngleAxisType(eabis[2], K));
104  VERIFY_IS_APPROX(m, mbis);
105 
106  // Tests that are only relevant for no possitive range
107  if (!(positiveRangeAlpha || positiveRangeBeta || positiveRangeGamma))
108  {
109  /* If I==K, and ea[1]==0, then there no unique solution. */
110  /* The remark apply in the case where I!=K, and |ea[1]| is close to pi/2. */
111  if( (i!=k || ea[1]!=0) && (i==k || !internal::isApprox(abs(ea[1]),Scalar(EIGEN_PI/2),test_precision<Scalar>())) )
112  VERIFY((ea-eabis).norm() <= test_precision<Scalar>());
113 
114  // approx_or_less_than does not work for 0
115  VERIFY(0 < eabis[0] || test_isMuchSmallerThan(eabis[0], Scalar(1)));
116  }
117 
118  // Quaternions
119  QuaternionType q(e);
120  eabis = EulerAnglesType(q, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma).angles();
121  VERIFY_IS_APPROX(eabis, eabis2);// Verify that the euler angles are still the same
122 }
123 
124 template<typename EulerSystem, typename Scalar>
126 {
127  verify_euler_ranged<EulerSystem>(ea, false, false, false);
128  verify_euler_ranged<EulerSystem>(ea, false, false, true);
129  verify_euler_ranged<EulerSystem>(ea, false, true, false);
130  verify_euler_ranged<EulerSystem>(ea, false, true, true);
131  verify_euler_ranged<EulerSystem>(ea, true, false, false);
132  verify_euler_ranged<EulerSystem>(ea, true, false, true);
133  verify_euler_ranged<EulerSystem>(ea, true, true, false);
134  verify_euler_ranged<EulerSystem>(ea, true, true, true);
135 }
136 
137 template<typename Scalar> void check_all_var(const Matrix<Scalar,3,1>& ea)
138 {
139  verify_euler<EulerSystemXYZ>(ea);
140  verify_euler<EulerSystemXYX>(ea);
141  verify_euler<EulerSystemXZY>(ea);
142  verify_euler<EulerSystemXZX>(ea);
143 
144  verify_euler<EulerSystemYZX>(ea);
145  verify_euler<EulerSystemYZY>(ea);
146  verify_euler<EulerSystemYXZ>(ea);
147  verify_euler<EulerSystemYXY>(ea);
148 
149  verify_euler<EulerSystemZXY>(ea);
150  verify_euler<EulerSystemZXZ>(ea);
151  verify_euler<EulerSystemZYX>(ea);
152  verify_euler<EulerSystemZYZ>(ea);
153 }
154 
155 template<typename Scalar> void eulerangles()
156 {
157  typedef Matrix<Scalar,3,3> Matrix3;
158  typedef Matrix<Scalar,3,1> Vector3;
159  typedef Array<Scalar,3,1> Array3;
160  typedef Quaternion<Scalar> Quaternionx;
161  typedef AngleAxis<Scalar> AngleAxisType;
162 
163  Scalar a = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI));
164  Quaternionx q1;
165  q1 = AngleAxisType(a, Vector3::Random().normalized());
166  Matrix3 m;
167  m = q1;
168 
169  Vector3 ea = m.eulerAngles(0,1,2);
170  check_all_var(ea);
171  ea = m.eulerAngles(0,1,0);
172  check_all_var(ea);
173 
174  // Check with purely random Quaternion:
175  q1.coeffs() = Quaternionx::Coefficients::Random().normalized();
176  m = q1;
177  ea = m.eulerAngles(0,1,2);
178  check_all_var(ea);
179  ea = m.eulerAngles(0,1,0);
180  check_all_var(ea);
181 
182  // Check with random angles in range [0:pi]x[-pi:pi]x[-pi:pi].
183  ea = (Array3::Random() + Array3(1,0,0))*Scalar(EIGEN_PI)*Array3(0.5,1,1);
184  check_all_var(ea);
185 
186  ea[2] = ea[0] = internal::random<Scalar>(0,Scalar(EIGEN_PI));
187  check_all_var(ea);
188 
189  ea[0] = ea[1] = internal::random<Scalar>(0,Scalar(EIGEN_PI));
190  check_all_var(ea);
191 
192  ea[1] = 0;
193  check_all_var(ea);
194 
195  ea.head(2).setZero();
196  check_all_var(ea);
197 
198  ea.setZero();
199  check_all_var(ea);
200 }
201 
203 {
204  for(int i = 0; i < g_repeat; i++) {
205  CALL_SUBTEST_1( eulerangles<float>() );
206  CALL_SUBTEST_2( eulerangles<double>() );
207  }
208 }
Matrix3f m
SCALAR Scalar
Definition: bench_gemm.cpp:33
void eulerangles()
Point2 q1
Definition: testPose2.cpp:753
void verify_euler_ranged(const Matrix< Scalar, 3, 1 > &ea, bool positiveRangeAlpha, bool positiveRangeBeta, bool positiveRangeGamma)
Eigen::Vector3d Vector3
Definition: Vector.h:43
#define EIGEN_PI
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
bool test_isMuchSmallerThan(const int &a, const int &b)
Definition: main.h:372
Array33i a
void verify_euler(const Matrix< Scalar, 3, 1 > &ea)
#define VERIFY_IS_APPROX(a, b)
static int g_repeat
Definition: main.h:144
void test_EulerAngles()
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
static const Matrix I
Definition: lago.cpp:35
JacobiRotation< float > J
#define VERIFY(a)
Definition: main.h:325
The quaternion class used to represent 3D orientations and rotations.
General-purpose arrays with easy API for coefficient-wise operations.
Definition: Array.h:45
void check_all_var(const Matrix< Scalar, 3, 1 > &ea)
EIGEN_DEVICE_FUNC bool isApprox(const Scalar &x, const Scalar &y, const typename NumTraits< Scalar >::Real &precision=NumTraits< Scalar >::dummy_precision())
#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 Sat May 8 2021 02:42:02