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 {
99  template <typename _Scalar, class _System>
100  class EulerAngles : public RotationBase<EulerAngles<_Scalar, _System>, 3>
101  {
102  public:
104 
106  typedef _Scalar Scalar;
108 
110  typedef _System System;
111 
119  const Vector3& u = Vector3::Unit(System::AlphaAxisAbs - 1);
120  return System::IsAlphaOpposite ? -u : u;
121  }
122 
125  const Vector3& u = Vector3::Unit(System::BetaAxisAbs - 1);
126  return System::IsBetaOpposite ? -u : u;
127  }
128 
131  const Vector3& u = Vector3::Unit(System::GammaAxisAbs - 1);
132  return System::IsGammaOpposite ? -u : u;
133  }
134 
135  private:
137 
138  public:
142  EulerAngles(const Scalar& alpha, const Scalar& beta, const Scalar& gamma) :
143  m_angles(alpha, beta, gamma) {}
144 
145  // TODO: Test this constructor
147  explicit EulerAngles(const Scalar* data) : m_angles(data) {}
148 
161  template<typename Derived>
162  explicit EulerAngles(const MatrixBase<Derived>& other) { *this = other; }
163 
175  template<typename Derived>
176  EulerAngles(const RotationBase<Derived, 3>& rot) { System::CalcEulerAngles(*this, rot.toRotationMatrix()); }
177 
178  /*EulerAngles(const QuaternionType& q)
179  {
180  // TODO: Implement it in a faster way for quaternions
181  // According to http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
182  // we can compute only the needed matrix cells and then convert to euler angles. (see ZYX example below)
183  // Currently we compute all matrix cells from quaternion.
184 
185  // Special case only for ZYX
186  //Scalar y2 = q.y() * q.y();
187  //m_angles[0] = std::atan2(2*(q.w()*q.z() + q.x()*q.y()), (1 - 2*(y2 + q.z()*q.z())));
188  //m_angles[1] = std::asin( 2*(q.w()*q.y() - q.z()*q.x()));
189  //m_angles[2] = std::atan2(2*(q.w()*q.x() + q.y()*q.z()), (1 - 2*(q.x()*q.x() + y2)));
190  }*/
191 
193  const Vector3& angles() const { return m_angles; }
195  Vector3& angles() { return m_angles; }
196 
198  Scalar alpha() const { return m_angles[0]; }
200  Scalar& alpha() { return m_angles[0]; }
201 
203  Scalar beta() const { return m_angles[1]; }
205  Scalar& beta() { return m_angles[1]; }
206 
208  Scalar gamma() const { return m_angles[2]; }
210  Scalar& gamma() { return m_angles[2]; }
211 
216  {
218  res.m_angles = -m_angles;
219  return res;
220  }
221 
226  {
227  return inverse();
228  }
229 
237  template<class Derived>
239  {
241  YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
242 
244  return *this;
245  }
246 
247  // TODO: Assign and construct from another EulerAngles (with different system)
248 
254  template<typename Derived>
256  System::CalcEulerAngles(*this, rot.toRotationMatrix());
257  return *this;
258  }
259 
265  const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const
266  { return angles().isApprox(other.angles(), prec); }
267 
270  {
271  // TODO: Calc it faster
272  return static_cast<QuaternionType>(*this).toRotationMatrix();
273  }
274 
276  operator QuaternionType() const
277  {
278  return
282  }
283 
284  friend std::ostream& operator<<(std::ostream& s, const EulerAngles<Scalar, System>& eulerAngles)
285  {
286  s << eulerAngles.angles().transpose();
287  return s;
288  }
289 
291  template <typename NewScalarType>
293  {
295  e.angles() = angles().template cast<NewScalarType>();
296  return e;
297  }
298  };
299 
300 #define EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(AXES, SCALAR_TYPE, SCALAR_POSTFIX) \
301  \
302  typedef EulerAngles<SCALAR_TYPE, EulerSystem##AXES> EulerAngles##AXES##SCALAR_POSTFIX;
303 
304 #define EIGEN_EULER_ANGLES_TYPEDEFS(SCALAR_TYPE, SCALAR_POSTFIX) \
305  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XYZ, SCALAR_TYPE, SCALAR_POSTFIX) \
306  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XYX, SCALAR_TYPE, SCALAR_POSTFIX) \
307  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XZY, SCALAR_TYPE, SCALAR_POSTFIX) \
308  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XZX, SCALAR_TYPE, SCALAR_POSTFIX) \
309  \
310  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YZX, SCALAR_TYPE, SCALAR_POSTFIX) \
311  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YZY, SCALAR_TYPE, SCALAR_POSTFIX) \
312  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YXZ, SCALAR_TYPE, SCALAR_POSTFIX) \
313  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YXY, SCALAR_TYPE, SCALAR_POSTFIX) \
314  \
315  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZXY, SCALAR_TYPE, SCALAR_POSTFIX) \
316  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZXZ, SCALAR_TYPE, SCALAR_POSTFIX) \
317  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZYX, SCALAR_TYPE, SCALAR_POSTFIX) \
318  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZYZ, SCALAR_TYPE, SCALAR_POSTFIX)
319 
322 
323  namespace internal
324  {
325  template<typename _Scalar, class _System>
326  struct traits<EulerAngles<_Scalar, _System> >
327  {
328  typedef _Scalar Scalar;
329  };
330 
331  // set from a rotation matrix
332  template<class System, class Other>
333  struct eulerangles_assign_impl<System,Other,3,3>
334  {
335  typedef typename Other::Scalar Scalar;
336  static void run(EulerAngles<Scalar, System>& e, const Other& m)
337  {
338  System::CalcEulerAngles(e, m);
339  }
340  };
341 
342  // set from a vector of Euler angles
343  template<class System, class Other>
344  struct eulerangles_assign_impl<System,Other,3,1>
345  {
346  typedef typename Other::Scalar Scalar;
347  static void run(EulerAngles<Scalar, System>& e, const Other& vec)
348  {
349  e.angles() = vec;
350  }
351  };
352  }
353 }
354 
355 #endif // EIGEN_EULERANGLESCLASS_H
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::EulerAngles::Matrix3
Matrix< Scalar, 3, 3 > Matrix3
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:112
Eigen
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
Eigen::EulerAngles::toRotationMatrix
Matrix3 toRotationMatrix() const
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:269
Eigen::EulerAngles::inverse
EulerAngles inverse() const
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:215
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
d
static const double d[K][N]
Definition: igam.h:11
Eigen::EulerAngles::angles
Vector3 & angles()
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:195
Eigen::EulerAngles::alpha
Scalar & alpha()
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:200
Eigen::EulerAngles::EulerAngles
EulerAngles()
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:140
Eigen::EulerAngles::RealScalar
NumTraits< Scalar >::Real RealScalar
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:107
Eigen::AngleAxis
Represents a 3D rotation as a rotation angle around an arbitrary 3D axis.
Definition: ForwardDeclarations.h:290
Eigen::EulerAngles::gamma
Scalar & gamma()
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:210
Eigen::internal::eulerangles_assign_impl< System, Other, 3, 3 >::Scalar
Other::Scalar Scalar
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:335
Eigen::EulerAngles::QuaternionType
Quaternion< Scalar > QuaternionType
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:114
Eigen::EulerAngles::AlphaAxisVector
static Vector3 AlphaAxisVector()
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:118
res
cout<< "Here is the matrix m:"<< endl<< m<< endl;Matrix< ptrdiff_t, 3, 1 > res
Definition: PartialRedux_count.cpp:3
Eigen::EulerAngles::cast
EulerAngles< NewScalarType, System > cast() const
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:292
Eigen::EulerAngles::EulerAngles
EulerAngles(const MatrixBase< Derived > &other)
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:162
Eigen::EulerAngles::operator=
EulerAngles & operator=(const RotationBase< Derived, 3 > &rot)
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:255
rot
int EIGEN_BLAS_FUNC() rot(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy, RealScalar *pc, RealScalar *ps)
Definition: level1_real_impl.h:79
Eigen::EulerAngles::EulerAngles
EulerAngles(const Scalar &alpha, const Scalar &beta, const Scalar &gamma)
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:142
Eigen::EulerAngles::Base
RotationBase< EulerAngles< _Scalar, _System >, 3 > Base
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:103
Eigen::internal::traits< EulerAngles< _Scalar, _System > >::Scalar
_Scalar Scalar
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:328
Eigen::EulerAngles::GammaAxisVector
static Vector3 GammaAxisVector()
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:130
data
int data[]
Definition: Map_placement_new.cpp:1
Eigen::EulerAngles::EulerAngles
EulerAngles(const Scalar *data)
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:147
Eigen::EulerAngles::Vector3
Matrix< Scalar, 3, 1 > Vector3
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:113
Eigen::EulerAngles::operator<<
friend std::ostream & operator<<(std::ostream &s, const EulerAngles< Scalar, System > &eulerAngles)
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:284
Eigen::RotationBase
Common base class for compact rotation representations.
Definition: ForwardDeclarations.h:286
Eigen::EulerAngles::alpha
Scalar alpha() const
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:198
Eigen::EulerAngles::angles
const Vector3 & angles() const
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:193
gtsam.examples.DogLegOptimizerExample.run
def run(args)
Definition: DogLegOptimizerExample.py:21
Eigen::EulerAngles::BetaAxisVector
static Vector3 BetaAxisVector()
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:124
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
Eigen::EulerAngles::operator-
EulerAngles operator-() const
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:225
Eigen::EulerAngles::gamma
Scalar gamma() const
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:208
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
Eigen::EulerAngles::isApprox
bool isApprox(const EulerAngles &other, const RealScalar &prec=NumTraits< Scalar >::dummy_precision()) const
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:264
Eigen::Quaternion
The quaternion class used to represent 3D orientations and rotations.
Definition: ForwardDeclarations.h:293
Eigen::EulerAngles::m_angles
Vector3 m_angles
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:136
EIGEN_EULER_ANGLES_TYPEDEFS
#define EIGEN_EULER_ANGLES_TYPEDEFS(SCALAR_TYPE, SCALAR_POSTFIX)
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:304
Eigen::internal::traits
Definition: ForwardDeclarations.h:17
EIGEN_STATIC_ASSERT
#define EIGEN_STATIC_ASSERT(CONDITION, MSG)
Definition: StaticAssert.h:127
Eigen::EulerAngles::beta
Scalar beta() const
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:203
Eigen::EulerAngles::operator=
EulerAngles & operator=(const MatrixBase< Derived > &other)
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:238
Eigen::internal::eulerangles_assign_impl< System, Other, 3, 1 >::Scalar
Other::Scalar Scalar
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:346
Eigen::EulerAngles::AngleAxisType
AngleAxis< Scalar > AngleAxisType
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:115
Eigen::internal::eulerangles_assign_impl< System, Other, 3, 3 >::run
static void run(EulerAngles< Scalar, System > &e, const Other &m)
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:336
Eigen::EulerAngles::EulerAngles
EulerAngles(const RotationBase< Derived, 3 > &rot)
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:176
Eigen::internal::is_same
Definition: Meta.h:148
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
Eigen::EulerAngles::Scalar
_Scalar Scalar
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:106
Eigen::internal::eulerangles_assign_impl
Definition: EulerSystem.h:44
Eigen::EulerAngles::beta
Scalar & beta()
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:205
internal
Definition: BandTriangularSolver.h:13
Eigen::MatrixBase
Base class for all dense matrices, vectors, and expressions.
Definition: MatrixBase.h:48
Eigen::internal::eulerangles_assign_impl< System, Other, 3, 1 >::run
static void run(EulerAngles< Scalar, System > &e, const Other &vec)
Definition: unsupported/Eigen/src/EulerAngles/EulerAngles.h:347
Eigen::NumTraits
Holds information about the various numeric (i.e. scalar) types allowed by Eigen.
Definition: NumTraits.h:232
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
Scalar
SCALAR Scalar
Definition: bench_gemm.cpp:46


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