geo_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) 2008-2012 Gael Guennebaud <gael.guennebaud@inria.fr>
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 #include <Eigen/Geometry>
12 #include <Eigen/LU>
13 #include <Eigen/SVD>
14 
15 
16 template<typename Scalar>
17 void verify_euler(const Matrix<Scalar,3,1>& ea, int i, int j, int k)
18 {
19  typedef Matrix<Scalar,3,3> Matrix3;
21  typedef AngleAxis<Scalar> AngleAxisx;
22  using std::abs;
23  Matrix3 m(AngleAxisx(ea[0], Vector3::Unit(i)) * AngleAxisx(ea[1], Vector3::Unit(j)) * AngleAxisx(ea[2], Vector3::Unit(k)));
24  Vector3 eabis = m.eulerAngles(i, j, k);
25  Matrix3 mbis(AngleAxisx(eabis[0], Vector3::Unit(i)) * AngleAxisx(eabis[1], Vector3::Unit(j)) * AngleAxisx(eabis[2], Vector3::Unit(k)));
26  VERIFY_IS_APPROX(m, mbis);
27  /* If I==K, and ea[1]==0, then there no unique solution. */
28  /* The remark apply in the case where I!=K, and |ea[1]| is close to pi/2. */
29  if( (i!=k || ea[1]!=0) && (i==k || !internal::isApprox(abs(ea[1]),Scalar(EIGEN_PI/2),test_precision<Scalar>())) )
30  VERIFY((ea-eabis).norm() <= test_precision<Scalar>());
31 
32  // approx_or_less_than does not work for 0
33  VERIFY(0 < eabis[0] || test_isMuchSmallerThan(eabis[0], Scalar(1)));
39 }
40 
41 template<typename Scalar> void check_all_var(const Matrix<Scalar,3,1>& ea)
42 {
43  verify_euler(ea, 0,1,2);
44  verify_euler(ea, 0,1,0);
45  verify_euler(ea, 0,2,1);
46  verify_euler(ea, 0,2,0);
47 
48  verify_euler(ea, 1,2,0);
49  verify_euler(ea, 1,2,1);
50  verify_euler(ea, 1,0,2);
51  verify_euler(ea, 1,0,1);
52 
53  verify_euler(ea, 2,0,1);
54  verify_euler(ea, 2,0,2);
55  verify_euler(ea, 2,1,0);
56  verify_euler(ea, 2,1,2);
57 }
58 
59 template<typename Scalar> void eulerangles()
60 {
61  typedef Matrix<Scalar,3,3> Matrix3;
63  typedef Array<Scalar,3,1> Array3;
64  typedef Quaternion<Scalar> Quaternionx;
65  typedef AngleAxis<Scalar> AngleAxisx;
66 
67  Scalar a = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI));
68  Quaternionx q1;
69  q1 = AngleAxisx(a, Vector3::Random().normalized());
70  Matrix3 m;
71  m = q1;
72 
73  Vector3 ea = m.eulerAngles(0,1,2);
74  check_all_var(ea);
75  ea = m.eulerAngles(0,1,0);
76  check_all_var(ea);
77 
78  // Check with purely random Quaternion:
79  q1.coeffs() = Quaternionx::Coefficients::Random().normalized();
80  m = q1;
81  ea = m.eulerAngles(0,1,2);
82  check_all_var(ea);
83  ea = m.eulerAngles(0,1,0);
84  check_all_var(ea);
85 
86  // Check with random angles in range [0:pi]x[-pi:pi]x[-pi:pi].
87  ea = (Array3::Random() + Array3(1,0,0))*Scalar(EIGEN_PI)*Array3(0.5,1,1);
88  check_all_var(ea);
89 
90  ea[2] = ea[0] = internal::random<Scalar>(0,Scalar(EIGEN_PI));
91  check_all_var(ea);
92 
93  ea[0] = ea[1] = internal::random<Scalar>(0,Scalar(EIGEN_PI));
94  check_all_var(ea);
95 
96  ea[1] = 0;
97  check_all_var(ea);
98 
99  ea.head(2).setZero();
100  check_all_var(ea);
101 
102  ea.setZero();
103  check_all_var(ea);
104 }
105 
106 EIGEN_DECLARE_TEST(geo_eulerangles)
107 {
108  for(int i = 0; i < g_repeat; i++) {
109  CALL_SUBTEST_1( eulerangles<float>() );
110  CALL_SUBTEST_2( eulerangles<double>() );
111  }
112 }
EIGEN_PI
#define EIGEN_PI
Definition: Eigen/src/Core/MathFunctions.h:16
eulerangles
void eulerangles()
Definition: geo_eulerangles.cpp:59
Eigen::AngleAxis
Represents a 3D rotation as a rotation angle around an arbitrary 3D axis.
Definition: ForwardDeclarations.h:290
Eigen::Array
General-purpose arrays with easy API for coefficient-wise operations.
Definition: Array.h:45
Eigen::internal::isApprox
EIGEN_DEVICE_FUNC bool isApprox(const Scalar &x, const Scalar &y, const typename NumTraits< Scalar >::Real &precision=NumTraits< Scalar >::dummy_precision())
Definition: Eigen/src/Core/MathFunctions.h:1947
EIGEN_DECLARE_TEST
EIGEN_DECLARE_TEST(geo_eulerangles)
Definition: geo_eulerangles.cpp:106
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
test_isMuchSmallerThan
bool test_isMuchSmallerThan(const AnnoyingScalar &a, const AnnoyingScalar &b)
Definition: AnnoyingScalar.h:162
CALL_SUBTEST_1
#define CALL_SUBTEST_1(FUNC)
Definition: split_test_helper.h:4
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
check_all_var
void check_all_var(const Matrix< Scalar, 3, 1 > &ea)
Definition: geo_eulerangles.cpp:41
Eigen::g_repeat
static int g_repeat
Definition: main.h:169
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
CALL_SUBTEST_2
#define CALL_SUBTEST_2(FUNC)
Definition: split_test_helper.h:10
VERIFY_IS_APPROX
#define VERIFY_IS_APPROX(a, b)
Definition: integer_types.cpp:15
verify_euler
void verify_euler(const Matrix< Scalar, 3, 1 > &ea, int i, int j, int k)
Definition: geo_eulerangles.cpp:17
Eigen::Quaternion
The quaternion class used to represent 3D orientations and rotations.
Definition: ForwardDeclarations.h:293
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
main.h
Eigen::Matrix< Scalar, 3, 1 >
abs
#define abs(x)
Definition: datatypes.h:17
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
VERIFY_IS_APPROX_OR_LESS_THAN
#define VERIFY_IS_APPROX_OR_LESS_THAN(a, b)
Definition: main.h:392
Scalar
SCALAR Scalar
Definition: bench_gemm.cpp:46
VERIFY
#define VERIFY(a)
Definition: main.h:380


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:37