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 declerations
16  template <typename _Scalar, class _System>
17  class EulerAngles;
18 
19  namespace internal
20  {
21  // TODO: Check if already exists on the rest 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 
41  #define EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(COND,MSG) typedef char static_assertion_##MSG[(COND)?1:-1]
42 
55  enum EulerAxis
56  {
57  EULER_X = 1,
58  EULER_Y = 2,
59  EULER_Z = 3
60  };
61 
119  template <int _AlphaAxis, int _BetaAxis, int _GammaAxis>
121  {
122  public:
123  // It's defined this way and not as enum, because I think
124  // that enum is not guerantee to support negative numbers
125 
127  static const int AlphaAxis = _AlphaAxis;
128 
130  static const int BetaAxis = _BetaAxis;
131 
133  static const int GammaAxis = _GammaAxis;
134 
135  enum
136  {
141  IsAlphaOpposite = (AlphaAxis < 0) ? 1 : 0,
142  IsBetaOpposite = (BetaAxis < 0) ? 1 : 0,
143  IsGammaOpposite = (GammaAxis < 0) ? 1 : 0,
145  IsOdd = ((AlphaAxisAbs)%3 == (BetaAxisAbs - 1)%3) ? 0 : 1,
146  IsEven = IsOdd ? 0 : 1,
148  IsTaitBryan = ((unsigned)AlphaAxisAbs != (unsigned)GammaAxisAbs) ? 1 : 0
149  };
150 
151  private:
152 
154  ALPHA_AXIS_IS_INVALID);
155 
157  BETA_AXIS_IS_INVALID);
158 
160  GAMMA_AXIS_IS_INVALID);
161 
162  EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT((unsigned)AlphaAxisAbs != (unsigned)BetaAxisAbs,
163  ALPHA_AXIS_CANT_BE_EQUAL_TO_BETA_AXIS);
164 
165  EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT((unsigned)BetaAxisAbs != (unsigned)GammaAxisAbs,
166  BETA_AXIS_CANT_BE_EQUAL_TO_GAMMA_AXIS);
167 
168  enum
169  {
170  // I, J, K are the pivot indexes permutation for the rotation matrix, that match this Euler system.
171  // They are used in this class converters.
172  // They are always different from each other, and their possible values are: 0, 1, or 2.
173  I = AlphaAxisAbs - 1,
174  J = (AlphaAxisAbs - 1 + 1 + IsOdd)%3,
175  K = (AlphaAxisAbs - 1 + 2 - IsOdd)%3
176  };
177 
178  // TODO: Get @mat parameter in form that avoids double evaluation.
179  template <typename Derived>
181  {
182  using std::atan2;
183  using std::sin;
184  using std::cos;
185 
186  typedef typename Derived::Scalar Scalar;
187  typedef Matrix<Scalar,2,1> Vector2;
188 
189  res[0] = atan2(mat(J,K), mat(K,K));
190  Scalar c2 = Vector2(mat(I,I), mat(I,J)).norm();
191  if((IsOdd && res[0]<Scalar(0)) || ((!IsOdd) && res[0]>Scalar(0))) {
192  if(res[0] > Scalar(0)) {
193  res[0] -= Scalar(EIGEN_PI);
194  }
195  else {
196  res[0] += Scalar(EIGEN_PI);
197  }
198  res[1] = atan2(-mat(I,K), -c2);
199  }
200  else
201  res[1] = atan2(-mat(I,K), c2);
202  Scalar s1 = sin(res[0]);
203  Scalar c1 = cos(res[0]);
204  res[2] = atan2(s1*mat(K,I)-c1*mat(J,I), c1*mat(J,J) - s1 * mat(K,J));
205  }
206 
207  template <typename Derived>
209  {
210  using std::atan2;
211  using std::sin;
212  using std::cos;
213 
214  typedef typename Derived::Scalar Scalar;
215  typedef Matrix<Scalar,2,1> Vector2;
216 
217  res[0] = atan2(mat(J,I), mat(K,I));
218  if((IsOdd && res[0]<Scalar(0)) || ((!IsOdd) && res[0]>Scalar(0)))
219  {
220  if(res[0] > Scalar(0)) {
221  res[0] -= Scalar(EIGEN_PI);
222  }
223  else {
224  res[0] += Scalar(EIGEN_PI);
225  }
226  Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm();
227  res[1] = -atan2(s2, mat(I,I));
228  }
229  else
230  {
231  Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm();
232  res[1] = atan2(s2, mat(I,I));
233  }
234 
235  // With a=(0,1,0), we have i=0; j=1; k=2, and after computing the first two angles,
236  // we can compute their respective rotation, and apply its inverse to M. Since the result must
237  // be a rotation around x, we have:
238  //
239  // c2 s1.s2 c1.s2 1 0 0
240  // 0 c1 -s1 * M = 0 c3 s3
241  // -s2 s1.c2 c1.c2 0 -s3 c3
242  //
243  // Thus: m11.c1 - m21.s1 = c3 & m12.c1 - m22.s1 = s3
244 
245  Scalar s1 = sin(res[0]);
246  Scalar c1 = cos(res[0]);
247  res[2] = atan2(c1*mat(J,K)-s1*mat(K,K), c1*mat(J,J) - s1 * mat(K,J));
248  }
249 
250  template<typename Scalar>
251  static void CalcEulerAngles(
254  {
255  CalcEulerAngles(res, mat, false, false, false);
256  }
257 
258  template<
259  bool PositiveRangeAlpha,
260  bool PositiveRangeBeta,
261  bool PositiveRangeGamma,
262  typename Scalar>
263  static void CalcEulerAngles(
266  {
267  CalcEulerAngles(res, mat, PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma);
268  }
269 
270  template<typename Scalar>
271  static void CalcEulerAngles(
274  bool PositiveRangeAlpha,
275  bool PositiveRangeBeta,
276  bool PositiveRangeGamma)
277  {
278  CalcEulerAngles_imp(
279  res.angles(), mat,
281 
282  if (IsAlphaOpposite == IsOdd)
283  res.alpha() = -res.alpha();
284 
285  if (IsBetaOpposite == IsOdd)
286  res.beta() = -res.beta();
287 
288  if (IsGammaOpposite == IsOdd)
289  res.gamma() = -res.gamma();
290 
291  // Saturate results to the requested range
292  if (PositiveRangeAlpha && (res.alpha() < 0))
293  res.alpha() += Scalar(2 * EIGEN_PI);
294 
295  if (PositiveRangeBeta && (res.beta() < 0))
296  res.beta() += Scalar(2 * EIGEN_PI);
297 
298  if (PositiveRangeGamma && (res.gamma() < 0))
299  res.gamma() += Scalar(2 * EIGEN_PI);
300  }
301 
302  template <typename _Scalar, class _System>
303  friend class Eigen::EulerAngles;
304  };
305 
306 #define EIGEN_EULER_SYSTEM_TYPEDEF(A, B, C) \
307  \
308  typedef EulerSystem<EULER_##A, EULER_##B, EULER_##C> EulerSystem##A##B##C;
309 
314 
319 
324 }
325 
326 #endif // EIGEN_EULERSYSTEM_H
SCALAR Scalar
Definition: bench_gemm.cpp:33
static const Key c2
Represents a fixed Euler rotation system.
Definition: EulerSystem.h:120
static void CalcEulerAngles(EulerAngles< Scalar, EulerSystem > &res, const typename EulerAngles< Scalar, EulerSystem >::Matrix3 &mat)
Definition: EulerSystem.h:263
#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)
internal::traits< Derived >::Scalar Scalar
Definition: MatrixBase.h:56
static void CalcEulerAngles(EulerAngles< Scalar, EulerSystem > &res, const typename EulerAngles< Scalar, EulerSystem >::Matrix3 &mat)
Definition: EulerSystem.h:251
static void CalcEulerAngles_imp(Matrix< typename MatrixBase< Derived >::Scalar, 3, 1 > &res, const MatrixBase< Derived > &mat, internal::true_type)
Definition: EulerSystem.h:180
cout<< "Here is the matrix m:"<< endl<< m<< endl;Matrix< ptrdiff_t, 3, 1 > res
EIGEN_DEVICE_FUNC const CosReturnType cos() const
#define Z
Definition: icosphere.cpp:21
#define EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(COND, MSG)
Definition: EulerSystem.h:41
Represents a rotation in a 3 dimensional space as three Euler angles.
static const Matrix I
Definition: lago.cpp:35
JacobiRotation< float > J
#define EIGEN_EULER_SYSTEM_TYPEDEF(A, B, C)
Definition: EulerSystem.h:306
Jet< T, N > atan2(const Jet< T, N > &g, const Jet< T, N > &f)
Definition: jet.h:556
static void CalcEulerAngles(EulerAngles< Scalar, EulerSystem > &res, const typename EulerAngles< Scalar, EulerSystem >::Matrix3 &mat, bool PositiveRangeAlpha, bool PositiveRangeBeta, bool PositiveRangeGamma)
Definition: EulerSystem.h:271
Eigen::Vector2d Vector2
Definition: Vector.h:42
static void CalcEulerAngles_imp(Matrix< typename MatrixBase< Derived >::Scalar, 3, 1 > &res, const MatrixBase< Derived > &mat, internal::false_type)
Definition: EulerSystem.h:208
const AutoDiffScalar< Matrix< typename internal::traits< typename internal::remove_all< DerTypeA >::type >::Scalar, Dynamic, 1 > > atan2(const AutoDiffScalar< DerTypeA > &a, const AutoDiffScalar< DerTypeB > &b)
EIGEN_DEVICE_FUNC const SinReturnType sin() const
The matrix class, also used for vectors and row-vectors.
#define X
Definition: icosphere.cpp:20
Base class for all dense matrices, vectors, and expressions.
Definition: MatrixBase.h:48
EulerAxis
Representation of a fixed signed rotation axis for EulerSystem.
Definition: EulerSystem.h:55
static const Key c1


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:42:02