transformation3.hh
Go to the documentation of this file.
00001 /**********************************************************************
00002  *
00003  * This source code is part of the Tree-based Network Optimizer (TORO)
00004  *
00005  * TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss, 
00006  *                         Slawomir Grzonka, and Wolfram Burgard
00007  *
00008  * TORO is licences under the Common Creative License,
00009  * Attribution-NonCommercial-ShareAlike 3.0
00010  *
00011  * You are free:
00012  *   - to Share - to copy, distribute and transmit the work
00013  *   - to Remix - to adapt the work
00014  *
00015  * Under the following conditions:
00016  *
00017  *   - Attribution. You must attribute the work in the manner specified
00018  *     by the author or licensor (but not in any way that suggests that
00019  *     they endorse you or your use of the work).
00020  *  
00021  *   - Noncommercial. You may not use this work for commercial purposes.
00022  *  
00023  *   - Share Alike. If you alter, transform, or build upon this work,
00024  *     you may distribute the resulting work only under the same or
00025  *     similar license to this one.
00026  *
00027  * Any of the above conditions can be waived if you get permission
00028  * from the copyright holder.  Nothing in this license impairs or
00029  * restricts the author's moral rights.
00030  *
00031  * TORO is distributed in the hope that it will be useful,
00032  * but WITHOUT ANY WARRANTY; without even the implied 
00033  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
00034  * PURPOSE.  
00035  **********************************************************************/
00036 
00037 #ifndef _TRANSFORMATION3_HXX_
00038 #define _TRANSFORMATION3_HXX_
00039 
00040 #include <assert.h>
00041 #include <cmath>
00042 #include "dmatrix.hh"
00043 
00044 namespace AISNavigation {
00045 
00046 template <class T>
00047 struct Vector3 {
00048   T elems[3] ;
00049 
00050   Vector3(T x, T y, T z) {elems[0]=x; elems[1]=y; elems[2]=z;}
00051   Vector3()              {elems[0]=0.; elems[1]=0.; elems[2]=0.;}
00052   Vector3(const DVector<T>& t){}
00053 
00054   // translational view
00055   inline const T& x()     const  {return elems[0];}
00056   inline const T& y()     const  {return elems[1];}
00057   inline const T& z()     const  {return elems[2];}
00058   inline T& x()            {return elems[0];}
00059   inline T& y()            {return elems[1];}
00060   inline T& z()            {return elems[2];}
00061 
00062   // rotational view
00063   inline const T& roll()   const  {return elems[0];}
00064   inline const T& pitch() const  {return elems[1];}
00065   inline const T& yaw()   const  {return elems[2];}
00066   inline T& roll()          {return elems[0];}
00067   inline T& pitch()        {return elems[1];}
00068   inline T& yaw()          {return elems[2];}
00069 
00070 };
00071 
00072 template <class T>
00073 struct Pose3 : public DVector<T>{
00074   Pose3();
00075   Pose3(const Vector3<T>& rot, const Vector3<T>& trans);
00076   Pose3(const T& x, const T& y, const T& z, const T& roll, const T& pitch, const T& yaw);
00077   Pose3(const DVector<T>& v): DVector<T>(v) {assert(v.dim()==6);} 
00078     
00079   inline operator const DVector<T>&  () {return (const DVector<T>)*this;}
00080   inline operator DVector<T>&        () {return *this;}
00081 
00082   inline const T& roll()  const  {return DVector<T>::elems[0];}
00083   inline const T& pitch() const  {return DVector<T>::elems[1];}
00084   inline const T& yaw()   const  {return DVector<T>::elems[2];}
00085   inline const T& x()     const  {return DVector<T>::elems[3];}
00086   inline const T& y()     const  {return DVector<T>::elems[4];}
00087   inline const T& z()     const  {return DVector<T>::elems[5];}
00088 
00089   inline T& roll()               {return DVector<T>::elems[0];}
00090   inline T& pitch()              {return DVector<T>::elems[1];}
00091   inline T& yaw()                {return DVector<T>::elems[2];}
00092   inline T& x()                  {return DVector<T>::elems[3];}
00093   inline T& y()                  {return DVector<T>::elems[4];}
00094   inline T& z()                  {return DVector<T>::elems[5];}
00095 
00096 };
00097 
00098 
00104 template <class T>
00105 struct Quaternion{
00106 
00110   Quaternion();
00111   
00115   Quaternion(const Vector3<T>& pose);
00116   
00120   Quaternion(const T _w, const T _x, const T _y, const T _z);
00121   
00125   Quaternion(const T _roll_x_phi, const T _pitch_y_theta, const T _yaw_z_psi);
00126   
00130   inline Quaternion<T> conjugated() const;
00131   
00135   inline Quaternion<T> normalized() const;
00136   
00140   inline Quaternion<T> inverse() const;
00141 
00142   /*construct a quaternion on the axis/angle representation*/
00143   inline Quaternion(const Vector3<T>& axis, const T& angle);
00144 
00151   inline Quaternion<T> rotateThisAlong (const Vector3<T>& axis, const T alpha) const;
00152   
00153         
00160   inline Quaternion<T> rotatePoint(const Quaternion& p) const;
00161   
00168   inline Vector3<T> rotatePoint(const Vector3<T>& p) const;
00169   
00176   inline Quaternion withRotation (const T alpha) const;
00177         
00182   inline Vector3<T> toAngles() const;
00183   
00184   inline Vector3<T> axis() const;
00185   inline T angle() const;
00186 
00187         
00191   inline T norm() const;
00192   
00196   inline T re() const;
00197   
00201   inline Vector3<T> im() const;
00202         
00203   T w,x,y,z;
00204 };
00205 
00206 
00207 
00208 template <class T> inline Quaternion<T> operator + (const Quaternion<T> & left, const Quaternion<T>& right);
00209 template <class T> inline Quaternion<T> operator - (const Quaternion<T> & left, const Quaternion<T>& right);
00210 template <class T> inline Quaternion<T> operator * (const Quaternion<T> & left, const Quaternion<T>& right);
00211 template <class T> inline Quaternion<T> operator * (const Quaternion<T> & left, const T scalar);
00212 template <class T> inline Quaternion<T> operator * (const T scalar, const Quaternion<T>& right);
00213 template <class T> std::ostream& operator << (std::ostream& os, const Quaternion<T>& q);
00214 
00215 template <class T> inline T innerproduct(const Quaternion<T>& left, const Quaternion<T>& right);
00216 template <class T> inline Quaternion<T> slerp(const Quaternion<T>& from, const Quaternion<T>& to, const T lambda);
00217 
00218 
00219 
00220 template <class T>
00221 struct Transformation3{
00222   Quaternion<T> rotationQuaternion;
00223   Vector3<T>    translationVector;
00224 
00225   Transformation3(){}
00226 
00227   inline static Transformation3<T> identity();
00228 
00229   Transformation3 (const Vector3<T>& trans, const Quaternion<T>& rot);
00230   Transformation3 (const Pose3<T>& v);
00231   Transformation3 (const T& x, const T& y, const T& z, const T& roll, const T& pitch, const T& yaw);
00232 
00233   inline Vector3<T> translation() const;
00234   inline Quaternion <T> rotation() const;
00235 
00236   inline Pose3<T> toPoseType() const;
00237   
00238   inline void setTranslation(const Vector3<T>& t);
00239   inline void setTranslation(const T& x, const T& y, const T& z);
00240 
00241   inline void setRotation(const Vector3<T>& r);
00242   inline void setRotation(const T& roll, const T& pitch, const T& yaw);
00243   inline void setRotation(const Quaternion<T>& q);
00244 
00245 
00246   inline Transformation3<T> inv() const;
00247   inline bool validRotation(const T& epsilon=0.001) const;
00248 
00249 };
00250 
00251 template <class T>
00252 inline Vector3<T> operator * (const Transformation3<T>& m, const Vector3<T>& v);
00253 
00254 template <class T>
00255 inline Transformation3<T> operator * (const Transformation3<T>& m1, const Transformation3<T>& m2);
00256 
00257 template <class T>
00258 struct Operations3D{
00259   typedef T                  BaseType;
00260   typedef Pose3<T>           PoseType;
00261   typedef Quaternion<T>      RotationType;
00262   typedef Vector3<T>         TranslationType;
00263   typedef Transformation3<T> TransformationType;
00264   typedef DMatrix<T>         CovarianceType;
00265   typedef DMatrix<T>         InformationType;
00266   typedef Transformation3<T> ParametersType;
00267 };
00268 
00269 } // namespace AISNavigation
00270 /**************************** IMPLEMENTATION ****************************/
00271 
00272 #include "transformation3.hxx"
00273 
00274 #endif
00275 


rtabmap
Author(s): Mathieu Labbe
autogenerated on Fri Aug 28 2015 12:51:42