Kindr
Kinematics and Dynamics for Robotics
RotationQuaternion.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  * * Redistributions of source code must retain the above copyright
8  * notice, this list of conditions and the following disclaimer.
9  * * Redistributions in binary form must reproduce the above copyright
10  * notice, this list of conditions and the following disclaimer in the
11  * documentation and/or other materials provided with the distribution.
12  * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
13  * names of its contributors may be used to endorse or promote products
14  * derived from this software without specific prior written permission.
15  *
16  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19  * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale,
20  * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
21  * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
22  * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
23  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
24  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26  *
27 */
28 
29 #pragma once
30 
31 #include <cmath>
32 
33 #include <Eigen/Geometry>
34 
35 #include "kindr/common/common.hpp"
39 
40 namespace kindr {
41 
42 
54 template<typename PrimType_>
55 class RotationQuaternion : public RotationBase<RotationQuaternion<PrimType_>> {
56  private:
60 
64  public:
69 
73  typedef PrimType_ Scalar;
74 
76  typedef Eigen::Matrix<PrimType_,3,1> Imaginary;
77 
79  typedef Eigen::Matrix<PrimType_,4,1> Vector4;
80 
84  : rotationQuaternion_(Implementation::Identity()) {
85  }
86 
94  RotationQuaternion(Scalar w, Scalar x, Scalar y, Scalar z)
95  : rotationQuaternion_(w,x,y,z) {
96  KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, rotationQuaternion_.norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-2), "Input quaternion has not unit length.");
97  }
98 
104  RotationQuaternion(Scalar real, const Imaginary& imag)
105  : rotationQuaternion_(real,imag(0),imag(1),imag(2)) {
106  KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, rotationQuaternion_.norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-2), "Input quaternion has not unit length.");
107  }
108 
113  RotationQuaternion(const Vector4 & vec)
114  : rotationQuaternion_(vec(0),vec(1),vec(2),vec(3)) {
115  KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, rotationQuaternion_.norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-2), "Input quaternion has not unit length.");
116  }
117 
122  explicit RotationQuaternion(const Implementation& other)
123  : rotationQuaternion_(other.w(), other.x(), other.y(), other.z()) {
124  KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, rotationQuaternion_.norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-2), "Input quaternion has not unit length.");
125  }
126 
131  explicit RotationQuaternion(const Base& other)
132  : rotationQuaternion_(other.w(), other.x(), other.y(), other.z()) {
133  KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, rotationQuaternion_.norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-2), "Input quaternion has not unit length.");
134  }
135 
139  template<typename OtherDerived_>
140  inline explicit RotationQuaternion(const RotationBase<OtherDerived_>& other)
141  : rotationQuaternion_(internal::ConversionTraits<RotationQuaternion, OtherDerived_>::convert(other.derived()).toImplementation()) {
142  }
143 
144  inline Scalar w() const {
145  return rotationQuaternion_.w();
146  }
147 
148  inline Scalar x() const {
149  return rotationQuaternion_.x();
150  }
151 
152  inline Scalar y() const {
153  return rotationQuaternion_.y();
154  }
155 
156  inline Scalar z() const {
157  return rotationQuaternion_.z();
158  }
159 
160  inline Scalar& w() {
161  return rotationQuaternion_.w();
162  }
163 
164  inline Scalar& x() {
165  return rotationQuaternion_.x();
166  }
167 
168  inline Scalar& y() {
169  return rotationQuaternion_.y();
170  }
171 
172  inline Scalar& z() {
173  return rotationQuaternion_.z();
174  }
175 
176  inline Scalar real() const {
177  return this->toUnitQuaternion().real();
178  }
179 
180  inline Imaginary imaginary() const {
181  return this->toUnitQuaternion().imaginary();
182  }
183 
184  inline Vector4 vector() const {
185  Vector4 vector4;
186  vector4 << w(), x(), y(), z();
187  return vector4;
188  }
189 
190  inline void setValues(Scalar w, Scalar x, Scalar y, Scalar z) {
191  rotationQuaternion_.w() = w;
192  rotationQuaternion_.x() = x;
193  rotationQuaternion_.y() = y;
194  rotationQuaternion_.z() = z;
195  }
196 
197  inline void setParts(Scalar real, const Imaginary& imag) {
198  rotationQuaternion_.w() = real;
199  rotationQuaternion_.x() = imag(0);
200  rotationQuaternion_.y() = imag(1);
201  rotationQuaternion_.z() = imag(2);
202  }
203 
207  inline Implementation& toImplementation() {
209  }
210 
214  inline const Implementation& toImplementation() const {
216  }
217 
219  return static_cast<Base&>(rotationQuaternion_);
220  }
221 
222  const Base& toUnitQuaternion() const {
223  return static_cast<const Base&>(rotationQuaternion_);
224  }
225 
230  template<typename PrimTypeIn_>
232  this->toImplementation() = Implementation(quat.w(),quat.x(),quat.y(),quat.z());
233  return *this;
234  }
235 
240  template<typename OtherDerived_>
242  this->toImplementation() = internal::ConversionTraits<RotationQuaternion, OtherDerived_>::convert(other.derived()).toImplementation();
243  return *this;
244  }
245 
250  template<typename PrimTypeIn_>
252  this->toImplementation() = other.toImplementation().template cast<PrimType_>();
253  return *this;
254  }
255 
260  template<typename PrimTypeIn_>
262  this->toImplementation() = Implementation(quat.w(),quat.x(),quat.y(),quat.z());
263  return *this;
264  }
265 
271  template<typename PrimTypeIn_>
273  this->toImplementation() = Implementation(quat.w(),quat.x(),quat.y(),quat.z());
274  KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input quaternion has not unit length.");
275  return *this;
276  }
277 
282  template<typename OtherDerived_>
284  this->toImplementation() = internal::ConversionTraits<RotationQuaternion, OtherDerived_>::convert(other.derived()).toImplementation();
285  return *this;
286  }
287 
292  return RotationQuaternion(this->toUnitQuaternion().inverted());
293  }
294 
299  *this = inverted();
300  return *this;
301  }
302 
308  }
309 
314  *this = conjugated();
315  return *this;
316  }
317 
322  rotationQuaternion_.w() = static_cast<Scalar>(1);
323  rotationQuaternion_.x() = static_cast<Scalar>(0);
324  rotationQuaternion_.y() = static_cast<Scalar>(0);
325  rotationQuaternion_.z() = static_cast<Scalar>(0);
326  return *this;
327  }
328 
334  if(this->w() > 0) {
335  return *this;
336  } else if (this->w() < 0){
337  return RotationQuaternion(-this->w(),-this->x(),-this->y(),-this->z());
338  } else { // w == 0
339 
340  if(this->x() > 0) {
341  return *this;
342  } else if (this->x() < 0){
343  return RotationQuaternion(-this->w(),-this->x(),-this->y(),-this->z());
344  } else { // x == 0
345 
346  if(this->y() > 0) {
347  return *this;
348  } else if (this->y() < 0){
349  return RotationQuaternion(-this->w(),-this->x(),-this->y(),-this->z());
350  } else { // y == 0
351 
352  if(this->z() > 0) { // z must be either -1 or 1 in this case
353  return *this;
354  } else {
355  return RotationQuaternion(-this->w(),-this->x(),-this->y(),-this->z());
356  }
357  }
358  }
359  }
360  }
361 
366  Eigen::Matrix<PrimType_,4,4> getQuaternionMatrix() const {
367  Eigen::Matrix<PrimType_,4,4> Qleft;
368  Qleft(0,0) = w(); Qleft(0,1) = -x(); Qleft(0,2) = -y(); Qleft(0,3) = -z();
369  Qleft(1,0) = x(); Qleft(1,1) = w(); Qleft(1,2) = -z(); Qleft(1,3) = y();
370  Qleft(2,0) = y(); Qleft(2,1) = z(); Qleft(2,2) = w(); Qleft(2,3) = -x();
371  Qleft(3,0) = z(); Qleft(3,1) = -y(); Qleft(3,2) = x(); Qleft(3,3) = w();
372 
373  return Qleft;
374  }
375 
380  Eigen::Matrix<PrimType_,4,4> getConjugateQuaternionMatrix() const {
381  Eigen::Matrix<PrimType_,4,4> Qright;
382  Qright(0,0) = w(); Qright(0,1) = -x(); Qright(0,2) = -y(); Qright(0,3) = -z();
383  Qright(1,0) = x(); Qright(1,1) = w(); Qright(1,2) = z(); Qright(1,3) = -y();
384  Qright(2,0) = y(); Qright(2,1) = -z(); Qright(2,2) = w(); Qright(2,3) = x();
385  Qright(3,0) = z(); Qright(3,1) = y(); Qright(3,2) = -x(); Qright(3,3) = w();
386 
387  return Qright;
388  }
389 
393  Eigen::Matrix<PrimType_,3,4> getGlobalQuaternionDiffMatrix() const {
394  Eigen::Matrix<PrimType_,3,4> H;
395  H(0,0) = -x(); H(0,1) = w(); H(0,2) = -z(); H(0,3) = y();
396  H(1,0) = -y(); H(1,1) = z(); H(1,2) = w(); H(1,3) = -x();
397  H(2,0) = -z(); H(2,1) = -y(); H(2,2) = x(); H(2,3) = w();
398  return H;
399  }
400 
404  Eigen::Matrix<PrimType_,3,4> getLocalQuaternionDiffMatrix() const {
405  Eigen::Matrix<PrimType_,3,4> HBar;
406  HBar(0,0) = -x(); HBar(0,1) = w(); HBar(0,2) = z(); HBar(0,3) = -y();
407  HBar(1,0) = -y(); HBar(1,1) = -z(); HBar(1,2) = w(); HBar(1,3) = x();
408  HBar(2,0) = -z(); HBar(2,1) = y(); HBar(2,2) = -x(); HBar(2,3) = w();
409  return HBar;
410  }
411 
416  *this = getUnique();
417  return *this;
418  }
419 
424  Scalar norm() {
425  return rotationQuaternion_.norm();
426  }
427 
433 
438  using RotationBase<RotationQuaternion<PrimType_>> ::operator==;
439 
440 
444  friend std::ostream& operator << (std::ostream& out, const RotationQuaternion& rquat) {
445  out << rquat.toUnitQuaternion();
446  return out;
447  }
448 };
449 
450 
455 
460 
461 
462 namespace internal {
463 
464 template<typename PrimType_>
465 class get_scalar<RotationQuaternion<PrimType_>> {
466  public:
467  typedef PrimType_ Scalar;
468 };
469 
470 template<typename PrimType_>
471 class get_matrix3X<RotationQuaternion<PrimType_>>{
472  public:
473  typedef int IndexType;
474 
475  template <IndexType Cols>
476  using Matrix3X = Eigen::Matrix<PrimType_, 3, Cols>;
477 };
478 
479 
480 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
481  * Conversion Traits
482  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
483 template<typename DestPrimType_, typename SourcePrimType_>
484 class ConversionTraits<RotationQuaternion<DestPrimType_>, AngleAxis<SourcePrimType_>> {
485  public:
486  inline static RotationQuaternion<DestPrimType_> convert(const AngleAxis<SourcePrimType_>& aa) {
487  return RotationQuaternion<DestPrimType_>(Eigen::Quaternion<DestPrimType_>(aa.toImplementation().template cast<DestPrimType_>()));
488  }
489 };
490 
491 
492 template <typename Scalar_ = double>
493 inline bool isLessThenEpsilons4thRoot(Scalar_ x){
494  static const Scalar_ epsilon4thRoot = pow(std::numeric_limits<Scalar_>::epsilon(), 1.0/4.0);
495  return x < epsilon4thRoot;
496 }
497 
498 template<typename DestPrimType_, typename SourcePrimType_>
499 class ConversionTraits<RotationQuaternion<DestPrimType_>, RotationVector<SourcePrimType_>> {
500  public:
501  inline static RotationQuaternion<DestPrimType_> convert(const RotationVector<SourcePrimType_>& rotationVector) {
504 // const Scalar v = rotationVector.toImplementation().norm();
505 // Scalar real;
506 // Imaginary imaginary;
507 // if (v < internal::NumTraits<Scalar>::dummy_precision()) {
508 // real = 1.0;
509 // imaginary= 0.5*rotationVector.toImplementation().template cast<DestPrimType_>();
510 // }
511 // else {
512 // real = cos(v/2);
513 // imaginary = sin(v/2)/v*rotationVector.toImplementation().template cast<DestPrimType_>();
514 // }
515 // return RotationQuaternion<DestPrimType_>(real, imaginary);
516 
517 
518  Scalar theta = (Scalar)rotationVector.toImplementation().norm();
519 
520  // na is 1/theta sin(theta/2)
521  Scalar na;
522  if(isLessThenEpsilons4thRoot(theta))
523  {
524  const Scalar one_over_48 = Scalar(1.0/48.0);
525  na = Scalar(0.5) - (theta * theta) * one_over_48;
526  }
527  else
528  {
529  na = sin(theta*0.5) / theta;
530  }
531  Imaginary axis = rotationVector.toImplementation().template cast<Scalar>()*na;
532  Scalar ct = cos(theta*0.5);
533  return RotationQuaternion<DestPrimType_>(ct, axis[0],axis[1],axis[2]);
534 // return Eigen::Vector4d(axis[0],axis[1],axis[2],ct);
535 
536  }
537 };
538 
539 template<typename DestPrimType_, typename SourcePrimType_>
540 class ConversionTraits<RotationQuaternion<DestPrimType_>, RotationQuaternion<SourcePrimType_>> {
541  public:
543  return RotationQuaternion<DestPrimType_>(q.toImplementation().template cast<DestPrimType_>());
544  }
545 };
546 
547 template<typename DestPrimType_, typename SourcePrimType_>
548 class ConversionTraits<RotationQuaternion<DestPrimType_>, RotationMatrix<SourcePrimType_>> {
549  public:
550  inline static RotationQuaternion<DestPrimType_> convert(const RotationMatrix<SourcePrimType_>& rotationMatrix) {
551  return RotationQuaternion<DestPrimType_>(Eigen::Quaternion<DestPrimType_>(rotationMatrix.toImplementation().template cast<DestPrimType_>()));
552  }
553 };
554 
555 template<typename DestPrimType_, typename SourcePrimType_>
556 class ConversionTraits<RotationQuaternion<DestPrimType_>, EulerAnglesXyz<SourcePrimType_>> {
557  public:
558  inline static RotationQuaternion<DestPrimType_> convert(const EulerAnglesXyz<SourcePrimType_>& xyz) {
559  return RotationQuaternion<DestPrimType_>(Eigen::Quaternion<DestPrimType_>(
560  Eigen::AngleAxis<DestPrimType_>((DestPrimType_)xyz.x(), Eigen::Matrix<DestPrimType_, 3, 1>::UnitX()) *
561  Eigen::AngleAxis<DestPrimType_>((DestPrimType_)xyz.y(), Eigen::Matrix<DestPrimType_, 3, 1>::UnitY()) *
562  Eigen::AngleAxis<DestPrimType_>((DestPrimType_)xyz.z(), Eigen::Matrix<DestPrimType_, 3, 1>::UnitZ())));
563  }
564 };
565 
566 template<typename DestPrimType_, typename SourcePrimType_>
567 class ConversionTraits<RotationQuaternion<DestPrimType_>, EulerAnglesZyx<SourcePrimType_>> {
568  public:
569  inline static RotationQuaternion<DestPrimType_> convert(const EulerAnglesZyx<SourcePrimType_>& zyx) {
570  return RotationQuaternion<DestPrimType_>(Eigen::Quaternion<DestPrimType_>(
571  Eigen::AngleAxis<DestPrimType_>((DestPrimType_)zyx.z(), Eigen::Matrix<DestPrimType_, 3, 1>::UnitZ()) *
572  Eigen::AngleAxis<DestPrimType_>((DestPrimType_)zyx.y(), Eigen::Matrix<DestPrimType_, 3, 1>::UnitY()) *
573  Eigen::AngleAxis<DestPrimType_>((DestPrimType_)zyx.x(), Eigen::Matrix<DestPrimType_, 3, 1>::UnitX())));
574  }
575 };
576 
577 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
578  * Multiplication Traits
579  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
580 
581 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
582  * Rotation Traits
583  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
584 
585 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
586  * Comparison Traits
587  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
588 template<typename PrimType_>
589 class ComparisonTraits<RotationBase<RotationQuaternion<PrimType_>>, RotationBase<RotationQuaternion<PrimType_>>> {
590  public:
591  inline static bool isEqual(const RotationBase<RotationQuaternion<PrimType_>>& a, const RotationBase<RotationQuaternion<PrimType_>>& b){
592  return a.derived().toImplementation().w() == b.derived().toImplementation().w() &&
593  a.derived().toImplementation().x() == b.derived().toImplementation().x() &&
594  a.derived().toImplementation().y() == b.derived().toImplementation().y() &&
595  a.derived().toImplementation().z() == b.derived().toImplementation().z();
596  }
597 };
598 
599 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
600  * Fixing Traits
601  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
602 template<typename PrimType_>
603 class FixingTraits<RotationQuaternion<PrimType_>> {
604  public:
605  inline static void fix(RotationQuaternion<PrimType_>& q) {
606  q.toImplementation().normalize();
607  }
608 };
609 
610 } // namespace internal
611 
612 } // namespace kindr
Eigen::Matrix< PrimType_, 4, 4 > getConjugateQuaternionMatrix() const
Returns the quaternion matrix Qright: q*p = Qright(p)*q This function can be used to get the derivati...
Implementation of a Quaternion based on Eigen::Quaternion.
Definition: Quaternion.hpp:55
RotationQuaternion(const RotationBase< OtherDerived_ > &other)
Constructor using another rotation.
PrimType_ Scalar
The primitive type. Float/Double.
RotationQuaternion< PrimType_ > & derived()
Gets the derived rotation. (only for advanced users)
Scalar y() const
Reading access to pitch (Y&#39;) angle.
Implementation of Euler angles (Z-Y&#39;-X&#39;&#39; / yaw-pitch-roll) rotation based on Eigen::Matrix<Scalar, 3, 1>
Scalar x() const
Definition: Quaternion.hpp:180
Representation of a generic rotationThis class defines the generic interface for a rotation...
Scalar z() const
Definition: Quaternion.hpp:413
RotationQuaternion conjugated() const
Returns the conjugated of the quaternion.
RotationQuaternion< float > RotationQuaternionF
Passive quaternion rotation with float primitive type.
const Implementation & toImplementation() const
Cast to the implementation type.
Eigen::Matrix< PrimType_, 3, 4 > getGlobalQuaternionDiffMatrix() const
Returns the global quaternion diff matrix H: GlobalAngularVelocity = 2*H*qdiff, qdiff = 0...
Scalar w() const
Definition: Quaternion.hpp:176
Scalar x() const
Reading access to roll (X&#39;&#39;) angle.
RotationQuaternion< float > RotationQuaternionPF
Passive quaternion rotation with float primitive type.
Scalar y() const
Definition: Quaternion.hpp:184
RotationQuaternion(Scalar real, const Imaginary &imag)
Constructor using real and imaginary part. In debug mode, an assertion is thrown if the quaternion ha...
RotationQuaternion & setIdentity()
Sets the rotation to identity.
RotationQuaternion(const Vector4 &vec)
Constructor using Eigen::Matrix<PrimType_,4,1>. In debug mode, an assertion is thrown if the quaterni...
void setParts(Scalar real, const Imaginary &imag)
Scalar w() const
Definition: Quaternion.hpp:401
RotationQuaternion & operator()(const UnitQuaternion< PrimTypeIn_ > &quat)
Bracket operator which assigns a UnitQuaternion to the RotationQuaternion.
RotationQuaternion(Scalar w, Scalar x, Scalar y, Scalar z)
Constructor using four scalars. In debug mode, an assertion is thrown if the quaternion has not unit ...
Scalar y() const
Gets pitch (Y&#39;) angle.
Scalar x() const
Definition: Quaternion.hpp:405
Quaternion< PrimType_ >::Implementation Implementation
the implementation type, i.e., Eigen::Quaternion<>
Definition: Quaternion.hpp:297
Implementation & toImplementation()
Cast to the implementation type.
Eigen::Matrix< PrimType_, 3, 1 > Imaginary
the imaginary type, i.e., Eigen::Quaternion<>
void fix()
Fixes the rotation to get rid of numerical errors (e.g. normalize quaternion).
const Base & toUnitQuaternion() const
Implementation of a rotation vector based on Eigen::Matrix<Scalar, 3, 1>
Definition: Rotation.hpp:43
void setValues(Scalar w, Scalar x, Scalar y, Scalar z)
RotationQuaternion & invert()
Inverts the rotation.
#define KINDR_ASSERT_SCALAR_NEAR_DBG(exceptionType, A, B, PERCENT_TOLERANCE, MESSAGE)
Eigen::Matrix< PrimType_, 3, 4 > getLocalQuaternionDiffMatrix() const
Returns the local quaternion diff matrix HBar: LocalAngularVelocity = 2*HBar*qdiff, qdiff = 0.5*HBar^T*LocalAngularVelocity.
RotationQuaternion & setUnique()
Modifies the quaternion rotation such that w >= 0.
Implementation of a unit quaternion based on Eigen::Quaternion.
Definition: Quaternion.hpp:40
Implementation of matrix rotation based on Eigen::Matrix<Scalar, 3, 3>
Definition: Rotation.hpp:49
Scalar z() const
Gets yaw (Z&#39;&#39;) angle.
Eigen::Matrix< PrimType_, 4, 4 > getQuaternionMatrix() const
Returns the quaternion matrix Qleft: q*p = Qleft(q)*p This function can be used to get the derivative...
Scalar real() const
Definition: Quaternion.hpp:433
Base::Implementation Implementation
The implementation type. The implementation type is always an Eigen object.
friend std::ostream & operator<<(std::ostream &out, const RotationQuaternion &rquat)
Used for printing the object with std::cout.
Scalar norm()
Returns the norm of the quaternion. The RotationQuaternion should always have unit length...
RotationQuaternion getUnique() const
Returns a unique quaternion rotation with w > 0. This function is used to compare different rotations...
Base rotationQuaternion_
The data container.
Implementation of Euler angles (X-Y&#39;-Z&#39;&#39; / roll-pitch-yaw) rotation based on Eigen::Matrix<Scalar,3,1>
Scalar norm() const
Definition: Quaternion.hpp:473
RotationQuaternion()
Default constructor using identity rotation.
Imaginary imaginary() const
Definition: Quaternion.hpp:437
RotationQuaternion< double > RotationQuaternionPD
Passive quaternion rotation with double primitive type.
RotationQuaternion inverted() const
Returns the inverse of the rotation.
RotationQuaternion & operator=(const UnitQuaternion< PrimTypeIn_ > &quat)
Assignment operator using a UnitQuaternion.
RotationQuaternion & conjugate()
Conjugates of the quaternion.
Scalar z() const
Definition: Quaternion.hpp:188
UnitQuaternion< PrimType_ > Base
The base type.
RotationQuaternion(const Base &other)
Constructor using UnitQuaternion. In debug mode, an assertion is thrown if the quaternion has not uni...
RotationQuaternion< double > RotationQuaternionD
Passive quaternion rotation with double primitive type.
RotationQuaternion(const Implementation &other)
Constructor using Eigen::Quaternion<PrimType_>. In debug mode, an assertion is thrown if the quaterni...
Implementation of an angle axis rotation based on Eigen::AngleAxis.
Definition: AngleAxis.hpp:54
Implementation & toImplementation()
Cast to the implementation type.
Scalar z() const
Reading access to yaw (Z) angle.
const Implementation & toImplementation() const
Definition: Quaternion.hpp:477
Implementation of quaternion rotation based on Eigen::Quaternion.
Definition: Rotation.hpp:46
Implementation & toImplementation()
Cast to the implementation type.
Eigen::Matrix< PrimType_, 4, 1 > Vector4
quaternion as 4x1 matrix: [w; x; y; z]
Implementation & toImplementation()
Cast to the implementation type.
Definition: AngleAxis.hpp:177
Scalar y() const
Definition: Quaternion.hpp:409
Scalar x() const
Gets roll (X) angle.