Main MRPT website > C++ reference for MRPT 1.3.2
Quaternion.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2015, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 // $Id: Quaternion.h 171 2011-01-20 13:56:28Z kai_wurm $
10 
11 /**
12 * OctoMap:
13 * A probabilistic, flexible, and compact 3D mapping library for robotic systems.
14 * @author K. M. Wurm, A. Hornung, University of Freiburg, Copyright (C) 2009.
15 * @see http://octomap.sourceforge.net/
16 * License: New BSD License
17 */
18 
19 /*
20  * Copyright (c) 2009-2011, K. M. Wurm, A. Hornung, University of Freiburg
21  * All rights reserved.
22  *
23  * Redistribution and use in source and binary forms, with or without
24  * modification, are permitted provided that the following conditions are met:
25  *
26  * * Redistributions of source code must retain the above copyright
27  * notice, this list of conditions and the following disclaimer.
28  * * Redistributions in binary form must reproduce the above copyright
29  * notice, this list of conditions and the following disclaimer in the
30  * documentation and/or other materials provided with the distribution.
31  * * Neither the name of the University of Freiburg nor the names of its
32  * contributors may be used to endorse or promote products derived from
33  * this software without specific prior written permission.
34  *
35  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
36  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
37  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
38  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
39  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
40  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
41  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
42  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
43  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
44  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
45  * POSSIBILITY OF SUCH DAMAGE.
46  */
47 
48 #ifndef OCTOMATH_QUATERNION_H
49 #define OCTOMATH_QUATERNION_H
50 
51 #include "Vector3.h"
52 #include <mrpt/maps/link_pragmas.h> // For DLL export within mrpt-maps via the MAPS_IMPEXP macro
53 
54 #include <iostream>
55 #include <vector>
56 
57 
58 namespace octomath {
59 
60  /*!
61  * \brief This class represents a Quaternion.
62  *
63  * The Unit Quaternion is one possible representation of the
64  * attitude of an object in tree-dimensional space.
65  *
66  * This Quaternion class is implemented according to Diebel,
67  * James. Representing Attitude: Euler Angle, Unit Quaternions, and
68  * Rotation Vectors. Stanford University. 2006. - Technical Report.
69  */
70 
72 
73  public:
74 
75  /*!
76  * \brief Default constructor
77  *
78  * Constructs the (1,0,0,0) Unit Quaternion
79  * representing the identity rotation.
80  */
81  inline Quaternion() { u() = 1; x() = 0; y() = 0; z() = 0; }
82 
83  /*!
84  * \brief Copy constructor
85  */
86  Quaternion(const Quaternion& other);
87 
88  /*!
89  * \brief Constructor
90  *
91  * Constructs a Quaternion from four single
92  * values
93  */
94  Quaternion(float u, float x, float y, float z);
95 
96  /*!
97  * \brief Constructor
98  *
99  * @param other a vector containing euler angles
100  */
101  Quaternion(const Vector3& other);
102 
103  /*!
104  * \brief Constructor from Euler angles
105  *
106  * Constructs a Unit Quaternion from Euler angles / Tait Bryan
107  * angles (in radians) according to the 1-2-3 convention.
108  * @param roll phi/roll angle (rotation about x-axis)
109  * @param pitch theta/pitch angle (rotation about y-axis)
110  * @param yaw psi/yaw angle (rotation about z-axis)
111  */
112  Quaternion(double roll, double pitch, double yaw);
113 
114 
115 
116  //! Constructs a Unit Quaternion from a rotation angle and axis.
117  Quaternion(const Vector3& axis, double angle);
118 
119 
120  /*!
121  * \brief Conversion to Euler angles
122  *
123  * Converts the attitude represented by this to
124  * Euler angles (roll, pitch, yaw).
125  */
126  Vector3 toEuler() const;
127 
128  void toRotMatrix(std::vector <double>& rot_matrix_3_3) const;
129 
130 
131  inline const float& operator() (unsigned int i) const { return data[i]; }
132  inline float& operator() (unsigned int i) { return data[i]; }
133 
134  float norm () const;
135  Quaternion normalized () const;
136  Quaternion& normalize ();
137 
138 
139  void operator/= (float x);
140  Quaternion& operator= (const Quaternion& other);
141  bool operator== (const Quaternion& other) const;
142 
143  /*!
144  * \brief Quaternion multiplication
145  *
146  * Standard Quaternion multiplication which is not
147  * commutative.
148  * @return this * other
149  */
150  Quaternion operator* (const Quaternion& other) const;
151 
152  /*!
153  * \brief Quaternion multiplication with extended vector
154  *
155  * @return q * (0, v)
156  */
157  Quaternion operator* (const Vector3 &v) const;
158 
159  /*!
160  * \brief Quaternion multiplication with extended vector
161  *
162  * @return (0, v) * q
163  */
164  friend Quaternion operator* (const Vector3 &v, const Quaternion &q);
165 
166  /*!
167  * \brief Inversion
168  *
169  * @return A copy of this Quaterion inverted
170  */
171  inline Quaternion inv() const { return Quaternion(u(), -x(), -y(), -z()); }
172 
173 
174  /*!
175  * \brief Inversion
176  *
177  * Inverts this Quaternion
178  * @return a reference to this Quaternion
179  */
180  Quaternion& inv_IP();
181 
182  /*!
183  * \brief Rotate a vector
184  *
185  * Rotates a vector to the body fixed coordinate
186  * system according to the attitude represented by
187  * this Quaternion.
188  * @param v a vector represented in world coordinates
189  * @return v represented in body-fixed coordinates
190  */
191  Vector3 rotate(const Vector3 &v) const;
192 
193  inline float& u() { return data[0]; }
194  inline float& x() { return data[1]; }
195  inline float& y() { return data[2]; }
196  inline float& z() { return data[3]; }
197 
198  inline const float& u() const { return data[0]; }
199  inline const float& x() const { return data[1]; }
200  inline const float& y() const { return data[2]; }
201  inline const float& z() const { return data[3]; }
202 
203  std::istream& read(std::istream &s);
204  std::ostream& write(std::ostream &s) const;
205  std::istream& readBinary(std::istream &s);
206  std::ostream& writeBinary(std::ostream &s) const;
207 
208  protected:
209  float data[4];
210 
211  };
212 
213  //! user friendly output in format (u x y z)
214  std::ostream MAPS_IMPEXP & operator<<(std::ostream& s, const Quaternion& q);
215 
216 }
217 
218 #endif
Quaternion()
Default constructor.
Definition: Quaternion.h:81
OctoMap: A probabilistic, flexible, and compact 3D mapping library for robotic systems.
Definition: Pose6D.h:55
const float & x() const
Definition: Quaternion.h:199
std::ostream MAPS_IMPEXP & operator<<(std::ostream &s, const Pose6D &p)
user friendly output in format (x y z, u x y z) which is (translation, rotation)
bool operator==(const CArray< T, N > &x, const CArray< T, N > &y)
Definition: CArray.h:279
const float & y() const
Definition: Quaternion.h:200
void normalize(Scalar valMin, Scalar valMax)
Scales all elements such as the minimum & maximum values are shifted to the given values...
Quaternion inv() const
Inversion.
Definition: Quaternion.h:171
This class represents a Quaternion.
Definition: Quaternion.h:71
const float & z() const
Definition: Quaternion.h:201
const float & u() const
Definition: Quaternion.h:198
std::vector< T1 > operator*(const std::vector< T1 > &a, const std::vector< T2 > &b)
a*b (element-wise multiplication)
Definition: ops_vectors.h:59
CONTAINER::Scalar norm(const CONTAINER &v)
This class represents a three-dimensional vector.
Definition: Vector3.h:65



Page generated by Doxygen 1.8.9.1 for MRPT 1.3.2 SVN:Unversioned directory at Tue Dec 8 09:49:21 UTC 2015