Kindr
Kinematics and Dynamics for Robotics
Quaternion.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 #pragma once
29 
30 #include <Eigen/Geometry>
31 
32 #include "kindr/common/common.hpp"
35 
36 namespace kindr {
37 
38 
39 template<typename PrimType_>
41 
43 
54 template<typename PrimType_>
55 class Quaternion : public QuaternionBase<Quaternion<PrimType_>>, private Eigen::Quaternion<PrimType_> {
56  private:
57  typedef Eigen::Quaternion<PrimType_> Base;
58  public:
60  typedef Base Implementation;
62  typedef PrimType_ Scalar;
64  typedef Eigen::Matrix<PrimType_,3,1> Imaginary;
66  typedef Eigen::Matrix<PrimType_,4,1> Vector4;
67 
70  : Base(Implementation(0,0,0,0)) {
71  }
72 
79  Quaternion(Scalar w, Scalar x, Scalar y, Scalar z)
80  : Base(w,x,y,z) {
81  }
82 
87  Quaternion(Scalar real, const Imaginary& imag)
88  : Base(real,imag(0),imag(1),imag(2)) {
89  }
90 
94  Quaternion(const Vector4& vector4)
95  : Base(vector4(0),vector4(1),vector4(2),vector4(3)) {
96  }
97 
98  // create from Eigen::Quaternion
99  explicit Quaternion(const Base& other)
100  : Base(other) {
101  }
102 
106  return Quaternion(Implementation::inverse());
107  }
108 
112  *this = Quaternion(Implementation::inverse());
113  return *this;
114  }
115 
119  return Quaternion(Implementation::conjugate());
120  }
121 
125  *this = Quaternion(Implementation::conjugate());
126  return *this;
127  }
128 
130  this->w() = other.w();
131  this->x() = other.x();
132  this->y() = other.y();
133  this->z() = other.z();
134  return *this;
135  }
136 
138  *this = Quaternion(other.toImplementation());
139  return *this;
140  }
141 
142 // bool operator ==(const Quaternion<PrimType_>& other) {
143 // return this->isEqual(other);
144 // }
145 
146  template<typename PrimTypeIn_>
148 // *this = other.template cast<PrimType_>();
149  this->w() = static_cast<PrimType_>(other.w());
150  this->x() = static_cast<PrimType_>(other.x());
151  this->y() = static_cast<PrimType_>(other.y());
152  this->z() = static_cast<PrimType_>(other.z());
153  return *this;
154  }
155 
156  template<typename PrimTypeIn_>
158 // *this = other.uq.template cast<PrimType_>(); // uq is private
159  this->w() = static_cast<PrimType_>(other.w());
160  this->x() = static_cast<PrimType_>(other.x());
161  this->y() = static_cast<PrimType_>(other.y());
162  this->z() = static_cast<PrimType_>(other.z());
163  return *this;
164  }
165 
166  inline Implementation& toImplementation() {
167  return static_cast<Implementation&>(*this);
168  }
169  inline const Implementation& toImplementation() const {
170  return static_cast<const Implementation&>(*this);
171  }
172 
173  using QuaternionBase<Quaternion<PrimType_>>::operator==;
174  using QuaternionBase<Quaternion<PrimType_>>::operator*;
175 
176  inline Scalar w() const {
177  return Base::w();
178  }
179 
180  inline Scalar x() const {
181  return Base::x();
182  }
183 
184  inline Scalar y() const {
185  return Base::y();
186  }
187 
188  inline Scalar z() const {
189  return Base::z();
190  }
191 
192  inline Scalar& w() { // todo: attention: no assertion for unitquaternions!
193  return Base::w();
194  }
195 
196  inline Scalar& x() {
197  return Base::x();
198  }
199 
200  inline Scalar& y() {
201  return Base::y();
202  }
203 
204  inline Scalar& z() {
205  return Base::z();
206  }
207 
208  inline Scalar real() const {
209  return Base::w();
210  }
211 
212  inline Imaginary imaginary() const {
213  return Imaginary(Base::x(),Base::y(),Base::z());
214  }
215 
216  inline Vector4 vector() const {
217  Vector4 vector4;
218  vector4 << w(), x(), y(), z();
219  return vector4;
220  }
221 
222  inline Scalar norm() const {
223  return Base::norm();
224  }
225 
227  return Quaternion(this->Base::normalized());
228  }
229 
231  this->Base::normalize();
232  return *this;
233  }
234 
236  this->w() = Scalar(0.0);
237  this->x() = Scalar(0.0);
238  this->y() = Scalar(0.0);
239  this->z() = Scalar(0.0);
240  return *this;
241  }
242 
244  return UnitQuaternion<PrimType_>(this->Base::normalized());
245  }
246 
251  Eigen::Matrix<PrimType_,4,4> getQuaternionMatrix() {
252  Eigen::Matrix<PrimType_,4,4> Qleft;
253  Qleft(0,0) = w(); Qleft(0,1) = -x(); Qleft(0,2) = -y(); Qleft(0,3) = -z();
254  Qleft(1,0) = x(); Qleft(1,1) = w(); Qleft(1,2) = -z(); Qleft(1,3) = y();
255  Qleft(2,0) = y(); Qleft(2,1) = z(); Qleft(2,2) = w(); Qleft(2,3) = -x();
256  Qleft(3,0) = z(); Qleft(3,1) = -y(); Qleft(3,2) = x(); Qleft(3,3) = w();
257  return Qleft;
258  }
259 
264  Eigen::Matrix<PrimType_,4,4> getConjugateQuaternionMatrix() {
265  Eigen::Matrix<PrimType_,4,4> Qright;
266  Qright(0,0) = w(); Qright(0,1) = -x(); Qright(0,2) = -y(); Qright(0,3) = -z();
267  Qright(1,0) = x(); Qright(1,1) = w(); Qright(1,2) = z(); Qright(1,3) = -y();
268  Qright(2,0) = y(); Qright(2,1) = -z(); Qright(2,2) = w(); Qright(2,3) = x();
269  Qright(3,0) = z(); Qright(3,1) = y(); Qright(3,2) = -x(); Qright(3,3) = w();
270  return Qright;
271  }
272 };
273 
278 
280 
290 template<typename PrimType_>
291 class UnitQuaternion : public UnitQuaternionBase<UnitQuaternion<PrimType_>> {
292  private:
295  public:
299  typedef PrimType_ Scalar;
301  typedef Eigen::Matrix<PrimType_,3,1> Imaginary;
303  typedef Eigen::Matrix<PrimType_,4,1> Vector4;
304 
307  : unitQuternion_(Implementation::Identity()) {
308  }
309 
311 
317  UnitQuaternion(Scalar w, Scalar x, Scalar y, Scalar z)
318  : unitQuternion_(w,x,y,z) {
319  KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, norm(), static_cast<Scalar>(1.0), static_cast<Scalar>(1e-2), "Input quaternion has not unit length.");
320  }
321 
327  UnitQuaternion(Scalar real, const Imaginary& imag)
328  : unitQuternion_(real,imag) {
329  KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, norm(), static_cast<Scalar>(1.0), static_cast<Scalar>(1e-2), "Input quaternion has not unit length.");
330  }
331 
336  UnitQuaternion(const Vector4& vector4)
337  : unitQuternion_(vector4(0),vector4(1),vector4(2),vector4(3)) {
338  KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, norm(), static_cast<Scalar>(1.0), static_cast<Scalar>(1e-2), "Input quaternion has not unit length.");
339  }
340 
342  explicit UnitQuaternion(const Quaternion<PrimType_>& other)
343  : unitQuternion_(other.toImplementation()) {
344  KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, norm(), static_cast<Scalar>(1.0), static_cast<Scalar>(1e-2), "Input quaternion has not unit length.");
345  }
346 
348 
351  explicit UnitQuaternion(const Implementation& other)
352  : unitQuternion_(other) {
353  KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, norm(), static_cast<Scalar>(1.0), static_cast<Scalar>(1e-2), "Input quaternion has not unit length.");
354  }
355 
357  this->w() = other.w();
358  this->x() = other.x();
359  this->y() = other.y();
360  this->z() = other.z();
361  return *this;
362  }
363 
364  template<typename PrimTypeIn_>
366 // uq = other.uq;
367  this->w() = static_cast<PrimType_>(other.w());
368  this->x() = static_cast<PrimType_>(other.x());
369  this->y() = static_cast<PrimType_>(other.y());
370  this->z() = static_cast<PrimType_>(other.z());
371  return *this;
372  }
373 
374  template<typename PrimTypeIn_>
376 // *this = (UnitQuaternion)quat;
377 // uq = other.template cast<PrimType_>();
378  this->w() = static_cast<PrimType_>(other.w());
379  this->x() = static_cast<PrimType_>(other.x());
380  this->y() = static_cast<PrimType_>(other.y());
381  this->z() = static_cast<PrimType_>(other.z());
382  KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, norm(), static_cast<Scalar>(1.0), 1e-2, "Input quaternion has not unit length.");
383  return *this;
384  }
385 
386 // UnitQuaternion<PrimType_> operator *(const UnitQuaternion<PrimType_>& other) {
387 // return UnitQuaternion<PrimType_>(this->uq * other.uq);
388 // }
389 //
390 // Quaternion<PrimType_> operator *(const Quaternion<PrimType_>& other) {
391 // return Quaternion<PrimType_>(this->uq * other);
392 // }
393 // bool operator ==(const UnitQuaternion<PrimType_>& other) {
394 // return this->uq == other.uq;
395 // }
396 //
397 // bool operator ==(const Quaternion<PrimType_>& other) {
398 // return this->uq == other.uq;
399 // }
400 
401  inline Scalar w() const {
402  return unitQuternion_.w();
403  }
404 
405  inline Scalar x() const {
406  return unitQuternion_.x();
407  }
408 
409  inline Scalar y() const {
410  return unitQuternion_.y();
411  }
412 
413  inline Scalar z() const {
414  return unitQuternion_.z();
415  }
416 
417  inline Scalar& w() { // todo: attention: no assertion for unitquaternions!
418  return unitQuternion_.w();
419  }
420 
421  inline Scalar& x() {
422  return unitQuternion_.x();
423  }
424 
425  inline Scalar& y() {
426  return unitQuternion_.y();
427  }
428 
429  inline Scalar& z() {
430  return unitQuternion_.z();
431  }
432 
433  inline Scalar real() const {
434  return unitQuternion_.w();
435  }
436 
437  inline Imaginary imaginary() const {
438  return Imaginary(unitQuternion_.x(),unitQuternion_.y(),unitQuternion_.z());
439  }
440 
441  inline Vector4 vector() const {
442  Vector4 vector4;
443  vector4 << w(), x(), y(), z();
444  return vector4;
445  }
446 
447 // using Base::operator*;
448 
449 // using UnitQuaternionBase<UnitQuaternion<PrimType_>>::conjugate;
450 
454  return UnitQuaternion(unitQuternion_.conjugated());
455  }
456 
460  unitQuternion_.conjugate();
461  return *this;
462  }
463 
464 // using Base::inverted;
465 // using Base::invert;
466 
467 // /*! \returns the inverse of the quaternion which is the conjugate for unit quaternions
468 // */
469 // UnitQuaternion inverse() const {
470 // return UnitQuaternion(Base::conjugate());
471 // }
472 
473  Scalar norm() const {
474  return unitQuternion_.norm();
475  }
476 
477  const Implementation& toImplementation() const {
478  return unitQuternion_.toImplementation();
479  }
480 
481  Implementation& toImplementation() {
482  return unitQuternion_.toImplementation();
483  }
484 
489  Eigen::Matrix<PrimType_,4,4> getQuaternionMatrix() {
490  Eigen::Matrix<PrimType_,4,4> Qleft;
491  Qleft(0,0) = w(); Qleft(0,1) = -x(); Qleft(0,2) = -y(); Qleft(0,3) = -z();
492  Qleft(1,0) = x(); Qleft(1,1) = w(); Qleft(1,2) = -z(); Qleft(1,3) = y();
493  Qleft(2,0) = y(); Qleft(2,1) = z(); Qleft(2,2) = w(); Qleft(2,3) = -x();
494  Qleft(3,0) = z(); Qleft(3,1) = -y(); Qleft(3,2) = x(); Qleft(3,3) = w();
495  return Qleft;
496  }
497 
502  Eigen::Matrix<PrimType_,4,4> getConjugateQuaternionMatrix() {
503  Eigen::Matrix<PrimType_,4,4> Qright;
504  Qright(0,0) = w(); Qright(0,1) = -x(); Qright(0,2) = -y(); Qright(0,3) = -z();
505  Qright(1,0) = x(); Qright(1,1) = w(); Qright(1,2) = z(); Qright(1,3) = -y();
506  Qright(2,0) = y(); Qright(2,1) = -z(); Qright(2,2) = w(); Qright(2,3) = x();
507  Qright(3,0) = z(); Qright(3,1) = y(); Qright(3,2) = -x(); Qright(3,3) = w();
508  return Qright;
509  }
510 };
511 
516 
517 
518 
519 } // namespace kindr
520 
Quaternion conjugated() const
Definition: Quaternion.hpp:118
Quaternion & operator()(const Quaternion< PrimTypeIn_ > &other)
Definition: Quaternion.hpp:147
Implementation of a Quaternion based on Eigen::Quaternion.
Definition: Quaternion.hpp:55
Scalar norm() const
Definition: Quaternion.hpp:222
Implementation & toImplementation()
Definition: Quaternion.hpp:166
Imaginary imaginary() const
Definition: Quaternion.hpp:212
Scalar w() const
Definition: Quaternion.hpp:176
Quaternion & operator=(const Quaternion< PrimType_ > &other)
Definition: Quaternion.hpp:129
Quaternion(const Base &other)
Definition: Quaternion.hpp:99
Scalar norm() const
Definition: Quaternion.hpp:473
const Implementation & toImplementation() const
Definition: Quaternion.hpp:477
Quaternion & conjugate()
Definition: Quaternion.hpp:124
Eigen::Matrix< PrimType_, 4, 1 > Vector4
quaternion as 4x1 matrix: [w; x; y; z]
Definition: Quaternion.hpp:66
Quaternion< PrimType_ > unitQuternion_
Definition: Quaternion.hpp:293
Scalar x() const
Definition: Quaternion.hpp:405
Vector4 vector() const
Definition: Quaternion.hpp:441
Eigen::Matrix< PrimType_, 4, 4 > getConjugateQuaternionMatrix()
Returns the quaternion matrix Qright: q*p = Qright(p)*q This function can be used to get the derivati...
Definition: Quaternion.hpp:502
Eigen::Matrix< PrimType_, 4, 1 > Vector4
quaternion as 4x1 matrix: [w; x; y; z]
Definition: Quaternion.hpp:303
Quaternion< PrimType_ >::Implementation Implementation
the implementation type, i.e., Eigen::Quaternion<>
Definition: Quaternion.hpp:297
Scalar x() const
Definition: Quaternion.hpp:180
Eigen::Quaternion< PrimType_ > Base
Definition: Quaternion.hpp:57
UnitQuaternion(const Implementation &other)
Constructor to create unit quaternion from Eigen::Quaternion.
Definition: Quaternion.hpp:351
UnitQuaternion< PrimType_ > toUnitQuaternion() const
Definition: Quaternion.hpp:243
UnitQuaternion conjugated() const
Definition: Quaternion.hpp:453
UnitQuaternion(Scalar real, const Imaginary &imag)
Constructor using real and imaginary part. In debug mode, an assertion is thrown if the quaternion ha...
Definition: Quaternion.hpp:327
Scalar z() const
Definition: Quaternion.hpp:188
#define KINDR_ASSERT_SCALAR_NEAR_DBG(exceptionType, A, B, PERCENT_TOLERANCE, MESSAGE)
Implementation of a unit quaternion based on Eigen::Quaternion.
Definition: Quaternion.hpp:40
Vector4 vector() const
Definition: Quaternion.hpp:216
const Implementation & toImplementation() const
Definition: Quaternion.hpp:169
UnitQuaternion< double > UnitQuaternionD
Unit quaternion using double.
Definition: Quaternion.hpp:513
PrimType_ Scalar
the scalar type, i.e., the type of the coefficients
Definition: Quaternion.hpp:299
Quaternion & normalize()
Definition: Quaternion.hpp:230
Scalar z() const
Definition: Quaternion.hpp:413
UnitQuaternion(const Quaternion< PrimType_ > &other)
Constructor to create unit quaternion from Quaternion.
Definition: Quaternion.hpp:342
Base class that defines the interface of a unit quaternion This class defines a generic interface for...
Scalar w() const
Definition: Quaternion.hpp:401
PrimType_ Scalar
the scalar type, i.e., the type of the coefficients
Definition: Quaternion.hpp:62
Implementation & toImplementation()
Definition: Quaternion.hpp:481
Scalar y() const
Definition: Quaternion.hpp:409
Quaternion & setZero()
Definition: Quaternion.hpp:235
Quaternion(Scalar w, Scalar x, Scalar y, Scalar z)
Constructor using four scalars.
Definition: Quaternion.hpp:79
Quaternion(Scalar real, const Imaginary &imag)
Constructor using real and imaginary part.
Definition: Quaternion.hpp:87
Quaternion normalized() const
Definition: Quaternion.hpp:226
UnitQuaternion(Scalar w, Scalar x, Scalar y, Scalar z)
Constructor to create unit quaternion from coefficients.
Definition: Quaternion.hpp:317
Base class that defines the interface of a quaternion This class defines a generic interface for a qu...
UnitQuaternion< float > UnitQuaternionF
Unit quaternion using float.
Definition: Quaternion.hpp:515
Eigen::Matrix< PrimType_, 4, 4 > getConjugateQuaternionMatrix()
Returns the quaternion matrix Qright: q*p = Qright(p)*q This function can be used to get the derivati...
Definition: Quaternion.hpp:264
Scalar y() const
Definition: Quaternion.hpp:184
Quaternion & invert()
Definition: Quaternion.hpp:111
UnitQuaternion & conjugate()
Definition: Quaternion.hpp:459
UnitQuaternion()
Default Constructor initializes the unit quaternion to identity.
Definition: Quaternion.hpp:306
Eigen::Matrix< PrimType_, 4, 4 > getQuaternionMatrix()
Returns the quaternion matrix Qleft: q*p = Qleft(q)*p This function can be used to get the derivative...
Definition: Quaternion.hpp:251
Imaginary imaginary() const
Definition: Quaternion.hpp:437
Base Implementation
the implementation type, i.e., Eigen::Quaternion<>
Definition: Quaternion.hpp:60
Quaternion< double > QuaternionD
Quaternion using double.
Definition: Quaternion.hpp:275
UnitQuaternionBase< UnitQuaternion< PrimType_ > > Base
Definition: Quaternion.hpp:294
Quaternion(const Vector4 &vector4)
Constructor using Eigen::Matrix<PrimType_,4,1>.
Definition: Quaternion.hpp:94
Eigen::Matrix< PrimType_, 4, 4 > getQuaternionMatrix()
Returns the quaternion matrix Qleft: q*p = Qleft(q)*p This function can be used to get the derivative...
Definition: Quaternion.hpp:489
Eigen::Matrix< PrimType_, 3, 1 > Imaginary
the imaginary type, i.e., Eigen::Quaternion<>
Definition: Quaternion.hpp:301
Scalar real() const
Definition: Quaternion.hpp:433
Quaternion inverted() const
Definition: Quaternion.hpp:105
Quaternion()
Default constructor creates a quaternion with all coefficients equal to zero.
Definition: Quaternion.hpp:69
UnitQuaternion(const Vector4 &vector4)
Constructor using Eigen::Matrix<PrimType_,4,1>. In debug mode, an assertion is thrown if the quaterni...
Definition: Quaternion.hpp:336
Eigen::Matrix< PrimType_, 3, 1 > Imaginary
the imaginary type, i.e., Eigen::Quaternion<>
Definition: Quaternion.hpp:64
Quaternion< float > QuaternionF
Quaternion using float.
Definition: Quaternion.hpp:277
Scalar real() const
Definition: Quaternion.hpp:208