unsupported/Eigen/src/EulerAngles/EulerAngles.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_EULERANGLESCLASS_H// TODO: Fix previous "EIGEN_EULERANGLES_H" definition?
11 #define EIGEN_EULERANGLESCLASS_H
12 
13 namespace Eigen
14 {
15  /*template<typename Other,
16  int OtherRows=Other::RowsAtCompileTime,
17  int OtherCols=Other::ColsAtCompileTime>
18  struct ei_eulerangles_assign_impl;*/
19 
110  template <typename _Scalar, class _System>
111  class EulerAngles : public RotationBase<EulerAngles<_Scalar, _System>, 3>
112  {
113  public:
115  typedef _Scalar Scalar;
116 
118  typedef _System System;
119 
126  static Vector3 AlphaAxisVector() {
127  const Vector3& u = Vector3::Unit(System::AlphaAxisAbs - 1);
128  return System::IsAlphaOpposite ? -u : u;
129  }
130 
132  static Vector3 BetaAxisVector() {
133  const Vector3& u = Vector3::Unit(System::BetaAxisAbs - 1);
134  return System::IsBetaOpposite ? -u : u;
135  }
136 
138  static Vector3 GammaAxisVector() {
139  const Vector3& u = Vector3::Unit(System::GammaAxisAbs - 1);
140  return System::IsGammaOpposite ? -u : u;
141  }
142 
143  private:
144  Vector3 m_angles;
145 
146  public:
150  EulerAngles(const Scalar& alpha, const Scalar& beta, const Scalar& gamma) :
151  m_angles(alpha, beta, gamma) {}
152 
157  template<typename Derived>
158  EulerAngles(const MatrixBase<Derived>& m) { *this = m; }
159 
171  template<typename Derived>
173  const MatrixBase<Derived>& m,
174  bool positiveRangeAlpha,
175  bool positiveRangeBeta,
176  bool positiveRangeGamma) {
177 
178  System::CalcEulerAngles(*this, m, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma);
179  }
180 
187  template<typename Derived>
189 
201  template<typename Derived>
204  bool positiveRangeAlpha,
205  bool positiveRangeBeta,
206  bool positiveRangeGamma) {
207 
208  System::CalcEulerAngles(*this, rot.toRotationMatrix(), positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma);
209  }
210 
212  const Vector3& angles() const { return m_angles; }
214  Vector3& angles() { return m_angles; }
215 
217  Scalar alpha() const { return m_angles[0]; }
219  Scalar& alpha() { return m_angles[0]; }
220 
222  Scalar beta() const { return m_angles[1]; }
224  Scalar& beta() { return m_angles[1]; }
225 
227  Scalar gamma() const { return m_angles[2]; }
229  Scalar& gamma() { return m_angles[2]; }
230 
235  {
237  res.m_angles = -m_angles;
238  return res;
239  }
240 
245  {
246  return inverse();
247  }
248 
260  template<
261  bool PositiveRangeAlpha,
262  bool PositiveRangeBeta,
263  bool PositiveRangeGamma,
264  typename Derived>
266  {
268 
269  EulerAngles e;
270  System::template CalcEulerAngles<
271  PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma, _Scalar>(e, m);
272  return e;
273  }
274 
286  template<
287  bool PositiveRangeAlpha,
288  bool PositiveRangeBeta,
289  bool PositiveRangeGamma,
290  typename Derived>
292  {
293  return FromRotation<PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma>(rot.toRotationMatrix());
294  }
295 
296  /*EulerAngles& fromQuaternion(const QuaternionType& q)
297  {
298  // TODO: Implement it in a faster way for quaternions
299  // According to http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
300  // we can compute only the needed matrix cells and then convert to euler angles. (see ZYX example below)
301  // Currently we compute all matrix cells from quaternion.
302 
303  // Special case only for ZYX
304  //Scalar y2 = q.y() * q.y();
305  //m_angles[0] = std::atan2(2*(q.w()*q.z() + q.x()*q.y()), (1 - 2*(y2 + q.z()*q.z())));
306  //m_angles[1] = std::asin( 2*(q.w()*q.y() - q.z()*q.x()));
307  //m_angles[2] = std::atan2(2*(q.w()*q.x() + q.y()*q.z()), (1 - 2*(q.x()*q.x() + y2)));
308  }*/
309 
311  template<typename Derived>
314 
315  System::CalcEulerAngles(*this, m);
316  return *this;
317  }
318 
319  // TODO: Assign and construct from another EulerAngles (with different system)
320 
322  template<typename Derived>
324  System::CalcEulerAngles(*this, rot.toRotationMatrix());
325  return *this;
326  }
327 
328  // TODO: Support isApprox function
329 
331  Matrix3 toRotationMatrix() const
332  {
333  return static_cast<QuaternionType>(*this).toRotationMatrix();
334  }
335 
337  operator QuaternionType() const
338  {
339  return
343  }
344 
345  friend std::ostream& operator<<(std::ostream& s, const EulerAngles<Scalar, System>& eulerAngles)
346  {
347  s << eulerAngles.angles().transpose();
348  return s;
349  }
350  };
351 
352 #define EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(AXES, SCALAR_TYPE, SCALAR_POSTFIX) \
353  \
354  typedef EulerAngles<SCALAR_TYPE, EulerSystem##AXES> EulerAngles##AXES##SCALAR_POSTFIX;
355 
356 #define EIGEN_EULER_ANGLES_TYPEDEFS(SCALAR_TYPE, SCALAR_POSTFIX) \
357  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XYZ, SCALAR_TYPE, SCALAR_POSTFIX) \
358  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XYX, SCALAR_TYPE, SCALAR_POSTFIX) \
359  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XZY, SCALAR_TYPE, SCALAR_POSTFIX) \
360  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XZX, SCALAR_TYPE, SCALAR_POSTFIX) \
361  \
362  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YZX, SCALAR_TYPE, SCALAR_POSTFIX) \
363  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YZY, SCALAR_TYPE, SCALAR_POSTFIX) \
364  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YXZ, SCALAR_TYPE, SCALAR_POSTFIX) \
365  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YXY, SCALAR_TYPE, SCALAR_POSTFIX) \
366  \
367  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZXY, SCALAR_TYPE, SCALAR_POSTFIX) \
368  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZXZ, SCALAR_TYPE, SCALAR_POSTFIX) \
369  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZYX, SCALAR_TYPE, SCALAR_POSTFIX) \
370  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZYZ, SCALAR_TYPE, SCALAR_POSTFIX)
371 
374 
375  namespace internal
376  {
377  template<typename _Scalar, class _System>
378  struct traits<EulerAngles<_Scalar, _System> >
379  {
380  typedef _Scalar Scalar;
381  };
382  }
383 
384 }
385 
386 #endif // EIGEN_EULERANGLESCLASS_H
EIGEN_DEVICE_FUNC RotationMatrixType toRotationMatrix() const
Definition: RotationBase.h:45
Matrix3f m
int EIGEN_BLAS_FUNC() rot(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy, RealScalar *pc, RealScalar *ps)
EulerAngles(const RotationBase< Derived, 3 > &rot)
EulerAngles & operator=(const RotationBase< Derived, 3 > &rot)
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
static EulerAngles FromRotation(const RotationBase< Derived, 3 > &rot)
EulerAngles & operator=(const MatrixBase< Derived > &m)
cout<< "Here is the matrix m:"<< endl<< m<< endl;Matrix< ptrdiff_t, 3, 1 > res
#define EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(TYPE, ROWS, COLS)
Definition: StaticAssert.h:159
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Common base class for compact rotation representations.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
Represents a rotation in a 3 dimensional space as three Euler angles.
#define EIGEN_EULER_ANGLES_TYPEDEFS(SCALAR_TYPE, SCALAR_POSTFIX)
EulerAngles(const Scalar &alpha, const Scalar &beta, const Scalar &gamma)
The quaternion class used to represent 3D orientations and rotations.
EulerAngles(const MatrixBase< Derived > &m, bool positiveRangeAlpha, bool positiveRangeBeta, bool positiveRangeGamma)
The matrix class, also used for vectors and row-vectors.
EulerAngles(const MatrixBase< Derived > &m)
Base class for all dense matrices, vectors, and expressions.
Definition: MatrixBase.h:48
Represents a 3D rotation as a rotation angle around an arbitrary 3D axis.
static EulerAngles FromRotation(const MatrixBase< Derived > &m)
EulerAngles(const RotationBase< Derived, 3 > &rot, bool positiveRangeAlpha, bool positiveRangeBeta, bool positiveRangeGamma)


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