EulerSystem.h
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 #ifndef EIGEN_EULERSYSTEM_H
11 #define EIGEN_EULERSYSTEM_H
12 
13 namespace Eigen
14 {
15  // Forward declarations
16  template <typename _Scalar, class _System>
17  class EulerAngles;
18 
19  namespace internal
20  {
21  // TODO: Add this trait to the Eigen internal API?
22  template <int Num, bool IsPositive = (Num > 0)>
23  struct Abs
24  {
25  enum { value = Num };
26  };
27 
28  template <int Num>
29  struct Abs<Num, false>
30  {
31  enum { value = -Num };
32  };
33 
34  template <int Axis>
35  struct IsValidAxis
36  {
37  enum { value = Axis != 0 && Abs<Axis>::value <= 3 };
38  };
39 
40  template<typename System,
41  typename Other,
42  int OtherRows=Other::RowsAtCompileTime,
43  int OtherCols=Other::ColsAtCompileTime>
45  }
46 
47  #define EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(COND,MSG) typedef char static_assertion_##MSG[(COND)?1:-1]
48 
61  enum EulerAxis
62  {
63  EULER_X = 1,
64  EULER_Y = 2,
65  EULER_Z = 3
66  };
67 
125  template <int _AlphaAxis, int _BetaAxis, int _GammaAxis>
127  {
128  public:
129  // It's defined this way and not as enum, because I think
130  // that enum is not guerantee to support negative numbers
131 
133  static const int AlphaAxis = _AlphaAxis;
134 
136  static const int BetaAxis = _BetaAxis;
137 
139  static const int GammaAxis = _GammaAxis;
140 
141  enum
142  {
147  IsAlphaOpposite = (AlphaAxis < 0) ? 1 : 0,
148  IsBetaOpposite = (BetaAxis < 0) ? 1 : 0,
149  IsGammaOpposite = (GammaAxis < 0) ? 1 : 0,
151  // Parity is even if alpha axis X is followed by beta axis Y, or Y is followed
152  // by Z, or Z is followed by X; otherwise it is odd.
153  IsOdd = ((AlphaAxisAbs)%3 == (BetaAxisAbs - 1)%3) ? 0 : 1,
154  IsEven = IsOdd ? 0 : 1,
156  IsTaitBryan = ((unsigned)AlphaAxisAbs != (unsigned)GammaAxisAbs) ? 1 : 0
157  };
158 
159  private:
160 
162  ALPHA_AXIS_IS_INVALID);
163 
165  BETA_AXIS_IS_INVALID);
166 
168  GAMMA_AXIS_IS_INVALID);
169 
171  ALPHA_AXIS_CANT_BE_EQUAL_TO_BETA_AXIS);
172 
174  BETA_AXIS_CANT_BE_EQUAL_TO_GAMMA_AXIS);
175 
176  static const int
177  // I, J, K are the pivot indexes permutation for the rotation matrix, that match this Euler system.
178  // They are used in this class converters.
179  // They are always different from each other, and their possible values are: 0, 1, or 2.
181  J_ = (AlphaAxisAbs - 1 + 1 + IsOdd)%3,
182  K_ = (AlphaAxisAbs - 1 + 2 - IsOdd)%3
183  ;
184 
185  // TODO: Get @mat parameter in form that avoids double evaluation.
186  template <typename Derived>
188  {
189  using std::atan2;
190  using std::sqrt;
191 
192  typedef typename Derived::Scalar Scalar;
193 
194  const Scalar plusMinus = IsEven? 1 : -1;
195  const Scalar minusPlus = IsOdd? 1 : -1;
196 
197  const Scalar Rsum = sqrt((mat(I_,I_) * mat(I_,I_) + mat(I_,J_) * mat(I_,J_) + mat(J_,K_) * mat(J_,K_) + mat(K_,K_) * mat(K_,K_))/2);
198  res[1] = atan2(plusMinus * mat(I_,K_), Rsum);
199 
200  // There is a singularity when cos(beta) == 0
201  if(Rsum > 4 * NumTraits<Scalar>::epsilon()) {// cos(beta) != 0
202  res[0] = atan2(minusPlus * mat(J_, K_), mat(K_, K_));
203  res[2] = atan2(minusPlus * mat(I_, J_), mat(I_, I_));
204  }
205  else if(plusMinus * mat(I_, K_) > 0) {// cos(beta) == 0 and sin(beta) == 1
206  Scalar spos = mat(J_, I_) + plusMinus * mat(K_, J_); // 2*sin(alpha + plusMinus * gamma
207  Scalar cpos = mat(J_, J_) + minusPlus * mat(K_, I_); // 2*cos(alpha + plusMinus * gamma)
208  Scalar alphaPlusMinusGamma = atan2(spos, cpos);
209  res[0] = alphaPlusMinusGamma;
210  res[2] = 0;
211  }
212  else {// cos(beta) == 0 and sin(beta) == -1
213  Scalar sneg = plusMinus * (mat(K_, J_) + minusPlus * mat(J_, I_)); // 2*sin(alpha + minusPlus*gamma)
214  Scalar cneg = mat(J_, J_) + plusMinus * mat(K_, I_); // 2*cos(alpha + minusPlus*gamma)
215  Scalar alphaMinusPlusBeta = atan2(sneg, cneg);
216  res[0] = alphaMinusPlusBeta;
217  res[2] = 0;
218  }
219  }
220 
221  template <typename Derived>
223  const MatrixBase<Derived>& mat, internal::false_type /*isTaitBryan*/)
224  {
225  using std::atan2;
226  using std::sqrt;
227 
228  typedef typename Derived::Scalar Scalar;
229 
230  const Scalar plusMinus = IsEven? 1 : -1;
231  const Scalar minusPlus = IsOdd? 1 : -1;
232 
233  const Scalar Rsum = sqrt((mat(I_, J_) * mat(I_, J_) + mat(I_, K_) * mat(I_, K_) + mat(J_, I_) * mat(J_, I_) + mat(K_, I_) * mat(K_, I_)) / 2);
234 
235  res[1] = atan2(Rsum, mat(I_, I_));
236 
237  // There is a singularity when sin(beta) == 0
238  if(Rsum > 4 * NumTraits<Scalar>::epsilon()) {// sin(beta) != 0
239  res[0] = atan2(mat(J_, I_), minusPlus * mat(K_, I_));
240  res[2] = atan2(mat(I_, J_), plusMinus * mat(I_, K_));
241  }
242  else if(mat(I_, I_) > 0) {// sin(beta) == 0 and cos(beta) == 1
243  Scalar spos = plusMinus * mat(K_, J_) + minusPlus * mat(J_, K_); // 2*sin(alpha + gamma)
244  Scalar cpos = mat(J_, J_) + mat(K_, K_); // 2*cos(alpha + gamma)
245  res[0] = atan2(spos, cpos);
246  res[2] = 0;
247  }
248  else {// sin(beta) == 0 and cos(beta) == -1
249  Scalar sneg = plusMinus * mat(K_, J_) + plusMinus * mat(J_, K_); // 2*sin(alpha - gamma)
250  Scalar cneg = mat(J_, J_) - mat(K_, K_); // 2*cos(alpha - gamma)
251  res[0] = atan2(sneg, cneg);
252  res[2] = 0;
253  }
254  }
255 
256  template<typename Scalar>
257  static void CalcEulerAngles(
260  {
262  res.angles(), mat,
264 
265  if (IsAlphaOpposite)
266  res.alpha() = -res.alpha();
267 
268  if (IsBetaOpposite)
269  res.beta() = -res.beta();
270 
271  if (IsGammaOpposite)
272  res.gamma() = -res.gamma();
273  }
274 
275  template <typename _Scalar, class _System>
276  friend class Eigen::EulerAngles;
277 
278  template<typename System,
279  typename Other,
280  int OtherRows,
281  int OtherCols>
283  };
284 
285 #define EIGEN_EULER_SYSTEM_TYPEDEF(A, B, C) \
286  \
287  typedef EulerSystem<EULER_##A, EULER_##B, EULER_##C> EulerSystem##A##B##C;
288 
293 
298 
303 }
304 
305 #endif // EIGEN_EULERSYSTEM_H
Eigen::EULER_X
@ EULER_X
Definition: EulerSystem.h:63
Eigen::EulerAngles
Represents a rotation in a 3 dimensional space as three Euler angles.
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:100
Eigen::EulerAngles::System
_System System
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:110
Eigen
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
Eigen::EULER_Z
@ EULER_Z
Definition: EulerSystem.h:65
Eigen::atan2
const AutoDiffScalar< Matrix< typename internal::traits< typename internal::remove_all< DerTypeA >::type >::Scalar, Dynamic, 1 > > atan2(const AutoDiffScalar< DerTypeA > &a, const AutoDiffScalar< DerTypeB > &b)
Definition: AutoDiffScalar.h:654
Eigen::EulerSystem::IsAlphaOpposite
@ IsAlphaOpposite
Definition: EulerSystem.h:147
Eigen::EulerSystem::BetaAxis
static const int BetaAxis
Definition: EulerSystem.h:136
Eigen::EulerSystem::AlphaAxis
static const int AlphaAxis
Definition: EulerSystem.h:133
gtsam::Y
GaussianFactorGraphValuePair Y
Definition: HybridGaussianProductFactor.cpp:29
Eigen::internal::false_type
Definition: Meta.h:97
Eigen::EulerSystem::GammaAxis
static const int GammaAxis
Definition: EulerSystem.h:139
Eigen::EulerSystem::BetaAxisAbs
@ BetaAxisAbs
Definition: EulerSystem.h:144
X
#define X
Definition: icosphere.cpp:20
EIGEN_EULER_SYSTEM_TYPEDEF
#define EIGEN_EULER_SYSTEM_TYPEDEF(A, B, C)
Definition: EulerSystem.h:285
mat
MatrixXf mat
Definition: Tutorial_AdvancedInitialization_CommaTemporary.cpp:1
res
cout<< "Here is the matrix m:"<< endl<< m<< endl;Matrix< ptrdiff_t, 3, 1 > res
Definition: PartialRedux_count.cpp:3
Eigen::internal::Abs
Definition: EulerSystem.h:23
Eigen::EulerSystem::CalcEulerAngles_imp
static void CalcEulerAngles_imp(Matrix< typename MatrixBase< Derived >::Scalar, 3, 1 > &res, const MatrixBase< Derived > &mat, internal::true_type)
Definition: EulerSystem.h:187
Eigen::EulerSystem::IsGammaOpposite
@ IsGammaOpposite
Definition: EulerSystem.h:149
Eigen::EulerSystem::J_
static const int J_
Definition: EulerSystem.h:181
Eigen::EulerSystem::IsOdd
@ IsOdd
Definition: EulerSystem.h:153
Eigen::internal::true_type
Definition: Meta.h:96
Eigen::EULER_Y
@ EULER_Y
Definition: EulerSystem.h:64
Eigen::EulerSystem::I_
static const int I_
Definition: EulerSystem.h:180
Eigen::EulerSystem::AlphaAxisAbs
@ AlphaAxisAbs
Definition: EulerSystem.h:143
Eigen::internal::Abs::value
@ value
Definition: EulerSystem.h:25
Eigen::EulerSystem
Represents a fixed Euler rotation system.
Definition: EulerSystem.h:126
Eigen::EulerSystem::CalcEulerAngles
static void CalcEulerAngles(EulerAngles< Scalar, EulerSystem > &res, const typename EulerAngles< Scalar, EulerSystem >::Matrix3 &mat)
Definition: EulerSystem.h:257
atan2
AnnoyingScalar atan2(const AnnoyingScalar &y, const AnnoyingScalar &x)
Definition: AnnoyingScalar.h:110
Eigen::EulerSystem::IsTaitBryan
@ IsTaitBryan
Definition: EulerSystem.h:156
Eigen::internal::IsValidAxis
Definition: EulerSystem.h:35
Eigen::EulerSystem::CalcEulerAngles_imp
static void CalcEulerAngles_imp(Matrix< typename MatrixBase< Derived >::Scalar, 3, 1 > &res, const MatrixBase< Derived > &mat, internal::false_type)
Definition: EulerSystem.h:222
Eigen::MatrixBase::Scalar
internal::traits< Derived >::Scalar Scalar
Definition: MatrixBase.h:56
Eigen::EulerSystem::IsBetaOpposite
@ IsBetaOpposite
Definition: EulerSystem.h:148
Eigen::EulerSystem::K_
static const int K_
Definition: EulerSystem.h:182
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
Eigen::internal::eulerangles_assign_impl
Definition: EulerSystem.h:44
internal
Definition: BandTriangularSolver.h:13
Eigen::MatrixBase
Base class for all dense matrices, vectors, and expressions.
Definition: MatrixBase.h:48
EulerAngles
Definition: quaternion_demo.cpp:135
Eigen::EulerAxis
EulerAxis
Representation of a fixed signed rotation axis for EulerSystem.
Definition: EulerSystem.h:61
Eigen::EulerSystem::IsEven
@ IsEven
Definition: EulerSystem.h:154
Z
#define Z
Definition: icosphere.cpp:21
Eigen::EulerSystem::EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT
EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis< AlphaAxis >::value, ALPHA_AXIS_IS_INVALID)
Eigen::NumTraits
Holds information about the various numeric (i.e. scalar) types allowed by Eigen.
Definition: NumTraits.h:232
test_callbacks.value
value
Definition: test_callbacks.py:160
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
Scalar
SCALAR Scalar
Definition: bench_gemm.cpp:46
Eigen::EulerSystem::GammaAxisAbs
@ GammaAxisAbs
Definition: EulerSystem.h:145


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:02:16