transformation3.hh
Go to the documentation of this file.
1 /**********************************************************************
2  *
3  * This source code is part of the Tree-based Network Optimizer (TORO)
4  *
5  * TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss,
6  * Slawomir Grzonka, and Wolfram Burgard
7  *
8  * TORO is licences under the Common Creative License,
9  * Attribution-NonCommercial-ShareAlike 3.0
10  *
11  * You are free:
12  * - to Share - to copy, distribute and transmit the work
13  * - to Remix - to adapt the work
14  *
15  * Under the following conditions:
16  *
17  * - Attribution. You must attribute the work in the manner specified
18  * by the author or licensor (but not in any way that suggests that
19  * they endorse you or your use of the work).
20  *
21  * - Noncommercial. You may not use this work for commercial purposes.
22  *
23  * - Share Alike. If you alter, transform, or build upon this work,
24  * you may distribute the resulting work only under the same or
25  * similar license to this one.
26  *
27  * Any of the above conditions can be waived if you get permission
28  * from the copyright holder. Nothing in this license impairs or
29  * restricts the author's moral rights.
30  *
31  * TORO is distributed in the hope that it will be useful,
32  * but WITHOUT ANY WARRANTY; without even the implied
33  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
34  * PURPOSE.
35  **********************************************************************/
36 
37 #ifndef _TRANSFORMATION3_HXX_
38 #define _TRANSFORMATION3_HXX_
39 
40 #include <assert.h>
41 #include <cmath>
42 #include "dmatrix.hh"
43 
44 namespace AISNavigation {
45 
46 template <class T>
47 struct Vector3 {
48  T elems[3] ;
49 
50  Vector3(T x, T y, T z) {elems[0]=x; elems[1]=y; elems[2]=z;}
51  Vector3() {elems[0]=0.; elems[1]=0.; elems[2]=0.;}
52  Vector3(const DVector<T>& t){}
53 
54  // translational view
55  inline const T& x() const {return elems[0];}
56  inline const T& y() const {return elems[1];}
57  inline const T& z() const {return elems[2];}
58  inline T& x() {return elems[0];}
59  inline T& y() {return elems[1];}
60  inline T& z() {return elems[2];}
61 
62  // rotational view
63  inline const T& roll() const {return elems[0];}
64  inline const T& pitch() const {return elems[1];}
65  inline const T& yaw() const {return elems[2];}
66  inline T& roll() {return elems[0];}
67  inline T& pitch() {return elems[1];}
68  inline T& yaw() {return elems[2];}
69 
70 };
71 
72 template <class T>
73 struct Pose3 : public DVector<T>{
74  Pose3();
75  Pose3(const Vector3<T>& rot, const Vector3<T>& trans);
76  Pose3(const T& x, const T& y, const T& z, const T& roll, const T& pitch, const T& yaw);
77  Pose3(const DVector<T>& v): DVector<T>(v) {assert(v.dim()==6);}
78 
79  inline operator const DVector<T>& () {return (const DVector<T>)*this;}
80  inline operator DVector<T>& () {return *this;}
81 
82  inline const T& roll() const {return DVector<T>::elems[0];}
83  inline const T& pitch() const {return DVector<T>::elems[1];}
84  inline const T& yaw() const {return DVector<T>::elems[2];}
85  inline const T& x() const {return DVector<T>::elems[3];}
86  inline const T& y() const {return DVector<T>::elems[4];}
87  inline const T& z() const {return DVector<T>::elems[5];}
88 
89  inline T& roll() {return DVector<T>::elems[0];}
90  inline T& pitch() {return DVector<T>::elems[1];}
91  inline T& yaw() {return DVector<T>::elems[2];}
92  inline T& x() {return DVector<T>::elems[3];}
93  inline T& y() {return DVector<T>::elems[4];}
94  inline T& z() {return DVector<T>::elems[5];}
95 
96 };
97 
98 
104 template <class T>
105 struct Quaternion{
106 
110  Quaternion();
111 
115  Quaternion(const Vector3<T>& pose);
116 
120  Quaternion(const T _w, const T _x, const T _y, const T _z);
121 
125  Quaternion(const T _roll_x_phi, const T _pitch_y_theta, const T _yaw_z_psi);
126 
130  inline Quaternion<T> conjugated() const;
131 
135  inline Quaternion<T> normalized() const;
136 
140  inline Quaternion<T> inverse() const;
141 
142  /*construct a quaternion on the axis/angle representation*/
143  inline Quaternion(const Vector3<T>& axis, const T& angle);
144 
151  inline Quaternion<T> rotateThisAlong (const Vector3<T>& axis, const T alpha) const;
152 
153 
160  inline Quaternion<T> rotatePoint(const Quaternion& p) const;
161 
168  inline Vector3<T> rotatePoint(const Vector3<T>& p) const;
169 
176  inline Quaternion withRotation (const T alpha) const;
177 
182  inline Vector3<T> toAngles() const;
183 
184  inline Vector3<T> axis() const;
185  inline T angle() const;
186 
187 
191  inline T norm() const;
192 
196  inline T re() const;
197 
201  inline Vector3<T> im() const;
202 
203  T w,x,y,z;
204 };
205 
206 
207 
208 template <class T> inline Quaternion<T> operator + (const Quaternion<T> & left, const Quaternion<T>& right);
209 template <class T> inline Quaternion<T> operator - (const Quaternion<T> & left, const Quaternion<T>& right);
210 template <class T> inline Quaternion<T> operator * (const Quaternion<T> & left, const Quaternion<T>& right);
211 template <class T> inline Quaternion<T> operator * (const Quaternion<T> & left, const T scalar);
212 template <class T> inline Quaternion<T> operator * (const T scalar, const Quaternion<T>& right);
213 template <class T> std::ostream& operator << (std::ostream& os, const Quaternion<T>& q);
214 
215 template <class T> inline T innerproduct(const Quaternion<T>& left, const Quaternion<T>& right);
216 template <class T> inline Quaternion<T> slerp(const Quaternion<T>& from, const Quaternion<T>& to, const T lambda);
217 
218 
219 
220 template <class T>
221 struct Transformation3{
222  Quaternion<T> rotationQuaternion;
223  Vector3<T> translationVector;
224 
225  Transformation3(){}
226 
227  inline static Transformation3<T> identity();
228 
229  Transformation3 (const Vector3<T>& trans, const Quaternion<T>& rot);
230  Transformation3 (const Pose3<T>& v);
231  Transformation3 (const T& x, const T& y, const T& z, const T& roll, const T& pitch, const T& yaw);
232 
233  inline Vector3<T> translation() const;
234  inline Quaternion <T> rotation() const;
235 
236  inline Pose3<T> toPoseType() const;
237 
238  inline void setTranslation(const Vector3<T>& t);
239  inline void setTranslation(const T& x, const T& y, const T& z);
240 
241  inline void setRotation(const Vector3<T>& r);
242  inline void setRotation(const T& roll, const T& pitch, const T& yaw);
243  inline void setRotation(const Quaternion<T>& q);
244 
245 
246  inline Transformation3<T> inv() const;
247  inline bool validRotation(const T& epsilon=0.001) const;
248 
249 };
250 
251 template <class T>
252 inline Vector3<T> operator * (const Transformation3<T>& m, const Vector3<T>& v);
253 
254 template <class T>
255 inline Transformation3<T> operator * (const Transformation3<T>& m1, const Transformation3<T>& m2);
256 
257 template <class T>
258 struct Operations3D{
259  typedef T BaseType;
260  typedef Pose3<T> PoseType;
261  typedef Quaternion<T> RotationType;
262  typedef Vector3<T> TranslationType;
263  typedef Transformation3<T> TransformationType;
264  typedef DMatrix<T> CovarianceType;
265  typedef DMatrix<T> InformationType;
266  typedef Transformation3<T> ParametersType;
267 };
268 
269 } // namespace AISNavigation
270 /**************************** IMPLEMENTATION ****************************/
271 
272 #include "transformation3.hxx"
273 
274 #endif
275 
AISNavigation::Vector3::Vector3
Vector3(const DVector< T > &t)
Definition: transformation3.hh:52
AISNavigation::Pose3::pitch
const T & pitch() const
Definition: transformation3.hh:83
epsilon
double epsilon
AISNavigation::Vector3::y
const T & y() const
Definition: transformation3.hh:56
glm::axis
GLM_FUNC_DECL detail::tvec3< T, P > axis(detail::tquat< T, P > const &x)
AISNavigation::operator*
Vector2< T > operator*(const T &d, const Vector2< T > &v)
Definition: transformation2.hh:81
glm::angle
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
AISNavigation::Pose3::roll
const T & roll() const
Definition: transformation3.hh:82
AISNavigation::Vector3::yaw
const T & yaw() const
Definition: transformation3.hh:65
AISNavigation::Vector3::z
const T & z() const
Definition: transformation3.hh:57
AISNavigation::innerproduct
T innerproduct(const Quaternion< T > &left, const Quaternion< T > &right)
AISNavigation::Vector3::pitch
const T & pitch() const
Definition: transformation3.hh:64
AISNavigation::Pose3::yaw
const T & yaw() const
Definition: transformation3.hh:84
AISNavigation::Pose3::x
const T & x() const
Definition: transformation3.hh:85
AISNavigation::Pose3::Pose3
Pose3(const DVector< T > &v)
Definition: transformation3.hh:77
AISNavigation::Pose3::z
const T & z() const
Definition: transformation3.hh:87
AISNavigation::Vector3::x
const T & x() const
Definition: transformation3.hh:55
AISNavigation::slerp
Quaternion< T > slerp(const Quaternion< T > &from, const Quaternion< T > &to, const T lambda)
AISNavigation::Operations3D::TransformationType
Transformation3< T > TransformationType
Definition: transformation3.hh:263
DVector::dim
int dim() const
Definition: dmatrix.hh:32
AISNavigation::Pose3::y
const T & y() const
Definition: transformation3.hh:86
AISNavigation::operator+
Vector2< T > operator+(const Vector2< T > &v1, const Vector2< T > &v2)
Definition: transformation2.hh:100
glm::rotation
GLM_FUNC_DECL detail::tquat< T, P > rotation(detail::tvec3< T, P > const &orig, detail::tvec3< T, P > const &dest)
AISNavigation::Operations3D::ParametersType
Transformation3< T > ParametersType
Definition: transformation3.hh:266
AISNavigation::Vector3::roll
const T & roll() const
Definition: transformation3.hh:63
AISNavigation::operator-
Vector2< T > operator-(const Vector2< T > &v1, const Vector2< T > &v2)
Definition: transformation2.hh:107
glm::inverse
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:37:06