transformation.h
Go to the documentation of this file.
00001 // HOG-Man - Hierarchical Optimization for Pose Graphs on Manifolds
00002 // Copyright (C) 2010 G. Grisetti, R. Kümmerle, C. Stachniss
00003 // 
00004 // HOG-Man is free software: you can redistribute it and/or modify
00005 // it under the terms of the GNU Lesser General Public License as published
00006 // by the Free Software Foundation, either version 3 of the License, or
00007 // (at your option) any later version.
00008 // 
00009 // HOG-Man is distributed in the hope that it will be useful,
00010 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 // GNU Lesser General Public License for more details.
00013 // 
00014 // You should have received a copy of the GNU Lesser General Public License
00015 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016 
00017 #ifndef _TRANSFORMATION_H_
00018 #define _TRANSFORMATION_H_
00019 #include "rotation_matrix.h"
00020 #include "angle.h"
00021 #include "quaternion.h"
00022 #include "axis_angle.h"
00023 
00026 
00034 template <typename Rotation>
00035 struct _Transformation {
00036   typedef typename Rotation::BaseType BaseType;
00037   typedef Rotation RotationType;
00038   typedef _Vector<Rotation::Dimension, BaseType> TranslationType;
00039   typedef _Matrix<Rotation::Dimension+1, Rotation::Dimension+1, BaseType> TransformationMatrix;
00040   typedef _Vector<Rotation::Dimension+Rotation::Angles, BaseType> TransformationVector;
00041   
00042   _Transformation() {
00043     _translation.fill(0.0);//=TranslationType()*BaseType(0.);
00044   }
00045 
00046   _Transformation(const TranslationType& t, const Rotation& r) {
00047     _translation=t;
00048     _rotation=r;
00049   }
00050 
00051   TranslationType& translation() {return _translation;}
00052 
00053   const TranslationType& translation() const {return _translation;}
00054 
00055   Rotation& rotation() {return _rotation;}
00056 
00057   const Rotation& rotation() const {return _rotation;}
00058 
00059   _Transformation<Rotation> operator  * (const _Transformation<Rotation>& t) const {
00060     return _Transformation<Rotation>(translation()+rotation()*t.translation(), rotation()*t.rotation());
00061   }
00062 
00063   _Transformation<Rotation>& operator *= (const _Transformation<Rotation>& t) {
00064     _translation+=rotation()*t.translation();
00065     _rotation*=t.rotation();
00066     return *this;
00067   }
00068   
00069   _Transformation<Rotation> inverse() const {
00070     RotationType _inverse=_rotation.inverse();
00071     return _Transformation<Rotation>(_inverse*(_translation*BaseType(-1.)), _inverse);
00072   }
00073 
00074   TranslationType operator* (const TranslationType& t) const {
00075     return TranslationType(translation()+rotation()*t);
00076   }
00077 
00078   TransformationMatrix toMatrix() const {
00079     TransformationMatrix m= TransformationMatrix::eye(1.);
00080     _Matrix <Rotation::Dimension, Rotation::Dimension, BaseType> r = _rotation.rotationMatrix();
00081     for (int i=0; i<r.rows(); i++)
00082       for (int j=0; j<r.cols(); j++)
00083         m[i][j]=r[i][j];
00084     for (int i=0; i<_translation.size(); i++)
00085       m[i][m.cols()-1]=_translation[i];
00086     return m;
00087   }
00088 
00089   TransformationVector toVector() const {
00090     TransformationVector v;
00091     for (int i=0; i<_translation.size(); i++)
00092       v[i]=_translation[i];
00093     _Vector<RotationType::Angles, BaseType> a=_rotation.angles();
00094     for (int i=0; i<a.size(); i++)
00095       v[i+RotationType::Dimension]=a[i];
00096     return v;
00097   }
00098 
00099   static _Transformation<Rotation> fromVector(const TransformationVector& v) {
00100     TranslationType translation_;
00101     _Vector<RotationType::Angles, BaseType> a;
00102     for (int i=0; i<RotationType::Dimension; i++)
00103       translation_[i]=v[i];
00104     for (int i=0; i<a.size(); i++)
00105       a[i]=v[i+RotationType::Dimension];
00106     RotationType rotation_(a);
00107     return _Transformation<Rotation>(translation_,rotation_);
00108   }
00109 
00110   TranslationType _translation;
00111   RotationType    _rotation;
00112 };
00113 
00114 typedef _Transformation< RotationMatrix2f> _Transformation2rf;
00115 typedef _Transformation< Anglef> _Transformation2f;
00116 typedef _Transformation< RotationMatrix2> _Transformation2r;
00117 typedef _Transformation< Angle> _Transformation2;
00118 
00119 typedef _Transformation< RotationMatrix3f> Transformation3rf;
00120 typedef _Transformation< Quaternionf>      Transformation3f;
00121 typedef _Transformation< AxisAnglef>       Transformation3af;
00122 
00123 typedef _Transformation< Quaternion>      Transformation3;
00124 typedef _Transformation< RotationMatrix3> Transformation3r;
00125 typedef _Transformation< AxisAngle>       Transformation3a;
00126 
00127 typedef _Transformation< Angle> Transformation2;
00128 
00130 
00131 // creation methods for transformation2
00132 
00133 
00134 
00135 template <typename Base>
00136 _Transformation< _Quaternion<Base>  > manifold2Transformation(const typename _Transformation< _Quaternion<Base>  >::TransformationVector& v) {
00137   _Vector<3, Base> t;
00138   t[0] = v[0];
00139   t[1] = v[1];
00140   t[2] = v[2];
00141   _AxisAngle<Base> a;
00142   a[0]=v[3];
00143   a[1]=v[4];
00144   a[2]=v[5];
00145   return _Transformation< _Quaternion<Base>  >(t,a.quaternion());
00146 }
00147 
00148 template <typename Base> 
00149 typename _Transformation< _Quaternion<Base>  >::TransformationVector transformation2manifold(const _Transformation< _Quaternion<Base>  >& t) {
00150   typename _Transformation< _Quaternion<Base>  >::TransformationVector v;
00151   v[0] = t.transformation()[0];
00152   v[1] = t.transformation()[1];
00153   v[2] = t.transformation()[2];
00154   _AxisAngle<Base> a(t.rotation());
00155   v[3]=a[3];
00156   v[4]=a[4];
00157   v[5]=a[5];
00158   return v;
00159 }
00160 
00161 
00162 
00163 #endif


hogman_minimal
Author(s): Maintained by Juergen Sturm
autogenerated on Mon Oct 6 2014 00:06:59