AngleAxis.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) 2008 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 #ifndef EIGEN_ANGLEAXIS_H
11 #define EIGEN_ANGLEAXIS_H
12 
13 namespace Eigen {
14 
41 namespace internal {
42 template<typename _Scalar> struct traits<AngleAxis<_Scalar> >
43 {
44  typedef _Scalar Scalar;
45 };
46 }
47 
48 template<typename _Scalar>
49 class AngleAxis : public RotationBase<AngleAxis<_Scalar>,3>
50 {
52 
53 public:
54 
55  using Base::operator*;
56 
57  enum { Dim = 3 };
59  typedef _Scalar Scalar;
63 
64 protected:
65 
66  Vector3 m_axis;
67  Scalar m_angle;
68 
69 public:
70 
72  EIGEN_DEVICE_FUNC AngleAxis() {}
78  template<typename Derived>
79  EIGEN_DEVICE_FUNC
80  inline AngleAxis(const Scalar& angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {}
84  template<typename QuatDerived>
85  EIGEN_DEVICE_FUNC inline explicit AngleAxis(const QuaternionBase<QuatDerived>& q) { *this = q; }
87  template<typename Derived>
88  EIGEN_DEVICE_FUNC inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; }
89 
91  EIGEN_DEVICE_FUNC Scalar angle() const { return m_angle; }
93  EIGEN_DEVICE_FUNC Scalar& angle() { return m_angle; }
94 
96  EIGEN_DEVICE_FUNC const Vector3& axis() const { return m_axis; }
101  EIGEN_DEVICE_FUNC Vector3& axis() { return m_axis; }
102 
104  EIGEN_DEVICE_FUNC inline QuaternionType operator* (const AngleAxis& other) const
105  { return QuaternionType(*this) * QuaternionType(other); }
106 
108  EIGEN_DEVICE_FUNC inline QuaternionType operator* (const QuaternionType& other) const
109  { return QuaternionType(*this) * other; }
110 
112  friend EIGEN_DEVICE_FUNC inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b)
113  { return a * QuaternionType(b); }
114 
116  EIGEN_DEVICE_FUNC AngleAxis inverse() const
117  { return AngleAxis(-m_angle, m_axis); }
118 
119  template<class QuatDerived>
120  EIGEN_DEVICE_FUNC AngleAxis& operator=(const QuaternionBase<QuatDerived>& q);
121  template<typename Derived>
122  EIGEN_DEVICE_FUNC AngleAxis& operator=(const MatrixBase<Derived>& m);
123 
124  template<typename Derived>
125  EIGEN_DEVICE_FUNC AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m);
126  EIGEN_DEVICE_FUNC Matrix3 toRotationMatrix(void) const;
127 
133  template<typename NewScalarType>
136 
138  template<typename OtherScalarType>
139  EIGEN_DEVICE_FUNC inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other)
140  {
141  m_axis = other.axis().template cast<Scalar>();
142  m_angle = Scalar(other.angle());
143  }
144 
145  EIGEN_DEVICE_FUNC static inline const AngleAxis Identity() { return AngleAxis(Scalar(0), Vector3::UnitX()); }
146 
151  EIGEN_DEVICE_FUNC bool isApprox(const AngleAxis& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const
152  { return m_axis.isApprox(other.m_axis, prec) && internal::isApprox(m_angle,other.m_angle, prec); }
153 };
154 
161 
168 template<typename Scalar>
169 template<typename QuatDerived>
171 {
172  EIGEN_USING_STD_MATH(atan2)
173  EIGEN_USING_STD_MATH(abs)
174  Scalar n = q.vec().norm();
176  n = q.vec().stableNorm();
177 
178  if (n != Scalar(0))
179  {
180  m_angle = Scalar(2)*atan2(n, abs(q.w()));
181  if(q.w() < Scalar(0))
182  n = -n;
183  m_axis = q.vec() / n;
184  }
185  else
186  {
187  m_angle = Scalar(0);
188  m_axis << Scalar(1), Scalar(0), Scalar(0);
189  }
190  return *this;
191 }
192 
195 template<typename Scalar>
196 template<typename Derived>
198 {
199  // Since a direct conversion would not be really faster,
200  // let's use the robust Quaternion implementation:
201  return *this = QuaternionType(mat);
202 }
203 
207 template<typename Scalar>
208 template<typename Derived>
210 {
211  return *this = QuaternionType(mat);
212 }
213 
216 template<typename Scalar>
218 EIGEN_DEVICE_FUNC AngleAxis<Scalar>::toRotationMatrix(void) const
219 {
220  EIGEN_USING_STD_MATH(sin)
221  EIGEN_USING_STD_MATH(cos)
222  Matrix3 res;
223  Vector3 sin_axis = sin(m_angle) * m_axis;
224  Scalar c = cos(m_angle);
225  Vector3 cos1_axis = (Scalar(1)-c) * m_axis;
226 
227  Scalar tmp;
228  tmp = cos1_axis.x() * m_axis.y();
229  res.coeffRef(0,1) = tmp - sin_axis.z();
230  res.coeffRef(1,0) = tmp + sin_axis.z();
231 
232  tmp = cos1_axis.x() * m_axis.z();
233  res.coeffRef(0,2) = tmp + sin_axis.y();
234  res.coeffRef(2,0) = tmp - sin_axis.y();
235 
236  tmp = cos1_axis.y() * m_axis.z();
237  res.coeffRef(1,2) = tmp - sin_axis.x();
238  res.coeffRef(2,1) = tmp + sin_axis.x();
239 
240  res.diagonal() = (cos1_axis.cwiseProduct(m_axis)).array() + c;
241 
242  return res;
243 }
244 
245 } // end namespace Eigen
246 
247 #endif // EIGEN_ANGLEAXIS_H
Matrix3f m
int array[24]
SCALAR Scalar
Definition: bench_gemm.cpp:33
EIGEN_DEVICE_FUNC AngleAxis(const MatrixBase< Derived > &m)
Definition: AngleAxis.h:88
Scalar * b
Definition: benchVecAdd.cpp:17
EIGEN_DEVICE_FUNC AngleAxis(const Scalar &angle, const MatrixBase< Derived > &axis)
Definition: AngleAxis.h:80
static EIGEN_DEVICE_FUNC const AngleAxis Identity()
Definition: AngleAxis.h:145
Scalar m_angle
Definition: AngleAxis.h:67
EIGEN_DEVICE_FUNC AngleAxis & operator=(const QuaternionBase< QuatDerived > &q)
Matrix< Scalar, 3, 3 > Matrix3
Definition: AngleAxis.h:60
EIGEN_DEVICE_FUNC AngleAxis inverse() const
Definition: AngleAxis.h:116
int n
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
AngleAxis< double > AngleAxisd
Definition: AngleAxis.h:160
Holds information about the various numeric (i.e. scalar) types allowed by Eigen. ...
Definition: NumTraits.h:150
EIGEN_DEVICE_FUNC AngleAxis()
Definition: AngleAxis.h:72
EIGEN_DEVICE_FUNC internal::cast_return_type< AngleAxis, AngleAxis< NewScalarType > >::type cast() const
Definition: AngleAxis.h:134
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar & coeffRef(Index rowId, Index colId)
Array33i a
EIGEN_DEVICE_FUNC const Vector3 & axis() const
Definition: AngleAxis.h:96
EIGEN_DEVICE_FUNC CoeffReturnType w() const
EIGEN_DEVICE_FUNC const VectorBlock< const Coefficients, 3 > vec() const
cout<< "Here is the matrix m:"<< endl<< m<< endl;Matrix< ptrdiff_t, 3, 1 > res
EIGEN_DEVICE_FUNC const CosReturnType cos() const
EIGEN_DEVICE_FUNC Scalar angle() const
Definition: AngleAxis.h:91
Vector3 m_axis
Definition: AngleAxis.h:66
static EIGEN_DEVICE_FUNC Matrix< Scalar, 2, 2 > toRotationMatrix(const Scalar &s)
Definition: RotationBase.h:182
EIGEN_DEVICE_FUNC Matrix3 toRotationMatrix(void) const
Definition: AngleAxis.h:218
AngleAxis< float > AngleAxisf
Definition: AngleAxis.h:157
Common base class for compact rotation representations.
EIGEN_DEVICE_FUNC const Scalar & q
_Scalar Scalar
Definition: AngleAxis.h:59
static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorUInt128< uint64_t, uint64_t > operator*(const TensorUInt128< HL, LL > &lhs, const TensorUInt128< HR, LR > &rhs)
Base class for quaternion expressions.
Matrix< Scalar, 3, 1 > Vector3
Definition: AngleAxis.h:61
EIGEN_DEVICE_FUNC AngleAxis & fromRotationMatrix(const MatrixBase< Derived > &m)
The quaternion class used to represent 3D orientations and rotations.
Quaternion< Scalar > QuaternionType
Definition: AngleAxis.h:62
EIGEN_DEVICE_FUNC Vector3 & axis()
Definition: AngleAxis.h:101
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 Scalar & angle()
Definition: AngleAxis.h:93
EIGEN_DEVICE_FUNC const SinReturnType sin() const
The matrix class, also used for vectors and row-vectors.
EIGEN_DEVICE_FUNC bool isApprox(const Scalar &x, const Scalar &y, const typename NumTraits< Scalar >::Real &precision=NumTraits< Scalar >::dummy_precision())
EIGEN_DEVICE_FUNC AngleAxis(const QuaternionBase< QuatDerived > &q)
Definition: AngleAxis.h:85
#define abs(x)
Definition: datatypes.h:17
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.
EIGEN_DEVICE_FUNC AngleAxis(const AngleAxis< OtherScalarType > &other)
Definition: AngleAxis.h:139
RotationBase< AngleAxis< _Scalar >, 3 > Base
Definition: AngleAxis.h:51
EIGEN_DEVICE_FUNC bool isApprox(const AngleAxis &other, const typename NumTraits< Scalar >::Real &prec=NumTraits< Scalar >::dummy_precision()) const
Definition: AngleAxis.h:151
Definition: pytypes.h:897


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