posegraph2d.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 POSEGRAPH_2D_HH
00018 #define POSEGRAPH_2D_HH
00019 
00020 #include "hogman_minimal/math/transformation.h"
00021 
00022 #include "posegraph.h"
00023 
00024 namespace AISNavigation{
00025 
00026   struct PoseGraph2D: public PoseGraph<Transformation2, Matrix3> {
00027 
00028     virtual void load(std::istream& is, bool overrideCovariances=false, std::vector <Edge*> *orderedEdges=0);
00029     virtual void save(std::ostream& os, const Transformation2& offset=Transformation2(), int type=0, bool onlyMarked=false) const;
00030 
00031     virtual void visualizeToStream(std::ostream& os) const;
00032 
00033     virtual void saveAsGnuplot(ostream& os, bool onlyMarked=false) const;
00034 
00035   };
00036 
00037   template <>
00038   struct MotionJacobian< PoseGraph<Transformation2, Matrix3 > >{
00039     Matrix3 state(const Transformation2& t, const Transformation2& movement){
00040       Matrix3 j;
00041       double dx=movement.translation().x();
00042       double dy=movement.translation().y();
00043       double theta=t.rotation();
00044       double s=sin(theta), c=cos(theta);
00045       j[0][0]=1.; j[0][1]=0.; j[0][2]=-s*dx-c*dy;
00046       j[1][0]=0.; j[1][1]=1.; j[1][2]=+c*dx-s*dy;
00047       j[2][0]=0.; j[2][1]=0.; j[2][2]=1;
00048       return j;
00049     }
00050     
00051     Matrix3 measurement(const Transformation2& t, const Transformation2& movement __attribute__((unused))){
00052       Matrix3 Ju;
00053       double s=sin(t.rotation()), c=cos(t.rotation());
00054       Ju[0][0]=c;  Ju[0][1]=-s; Ju[0][2]=0;
00055       Ju[1][0]=s; Ju[1][1]=c; Ju[1][2]=0;
00056       Ju[2][0]=0.; Ju[2][1]=0.; Ju[2][2]=1;
00057       return Ju;
00058     }
00059   };
00060 
00061   template <> 
00062   struct TaylorTerms< PoseGraph2D > {
00063     typedef PoseGraph2D PG;
00064 
00065     void operator()(PG::TransformationType& fij, PG::InformationType& dfij_dxi, PG::InformationType& dfij_dxj, const PG::Edge& e) {
00066       const PG::Vertex* vi = reinterpret_cast<const PG::Vertex*>(e.from());
00067       const PG::Vertex* vj = reinterpret_cast<const PG::Vertex*>(e.to());
00068       
00069       fij=vi->transformation.inverse()*vj->transformation;
00070       double thetai=vi->transformation.rotation();
00071       Vector2 dt=vj->transformation.translation()-vi->transformation.translation();
00072       double si=sin(thetai), ci=cos(thetai);
00073       
00074       dfij_dxi[0][0]=-ci;  dfij_dxi[0][1]=-si;  dfij_dxi[0][2]= -si*dt.x()+ci*dt.y();
00075       dfij_dxi[1][0]= si;  dfij_dxi[1][1]=-ci;  dfij_dxi[1][2]= -ci*dt.x()-si*dt.y();
00076       dfij_dxi[2][0]= 0;   dfij_dxi[2][1]=0;    dfij_dxi[2][2]= -1;
00077       
00078       dfij_dxj[0][0]= ci;  dfij_dxj[0][1]= si;  dfij_dxj[0][2]= 0;
00079       dfij_dxj[1][0]=-si;  dfij_dxj[1][1]= ci;  dfij_dxj[1][2]= 0;
00080       dfij_dxj[2][0]= 0;   dfij_dxj[2][1]=0;    dfij_dxj[2][2]= 1;
00081 
00082     }
00083   };
00084 
00085   template < > 
00086   struct Gradient < PoseGraph2D >{
00087     typedef PoseGraph2D PG;
00088     
00089     void operator()( PG::TransformationVectorType& eij,  PG::InformationType& deij_dxi,  PG::InformationType& deij_dxj,  const PG::Edge& e) {
00090 
00091       const PG::Vertex* vi = reinterpret_cast<const PG::Vertex*>(e.from());
00092       const PG::Vertex* vj = reinterpret_cast<const PG::Vertex*>(e.to());
00093       
00094       PG::TransformationType Tj=vi->transformation*e.mean();
00095       eij=vj->transformation.toVector();
00096       
00097       PG::TransformationVectorType pj=Tj.toVector();
00098       eij-=pj;
00099       
00100       double thetai=vi->transformation.rotation();
00101       Vector2 dt=e.mean().translation();
00102       double si=sin(thetai), ci=cos(thetai);
00103       
00104       deij_dxi[0][0]=-1;  deij_dxi[0][1]= 0;    deij_dxi[0][2]= -si*dt.x()+ci*dt.y();
00105       deij_dxi[1][0]= 0;  deij_dxi[1][1]=-1;    deij_dxi[1][2]= -ci*dt.x()-si*dt.y();
00106       deij_dxi[2][0]= 0;  deij_dxi[2][1]=0;     deij_dxi[2][2]= -1;
00107       
00108       deij_dxj[0][0]= 1;  deij_dxj[0][1]= 0;    deij_dxj[0][2]= 0;
00109       deij_dxj[1][0]= 0;  deij_dxj[1][1]= 1;    deij_dxj[1][2]= 0;
00110       deij_dxj[2][0]= 0;  deij_dxj[2][1]=0;     deij_dxj[2][2]= 1;
00111     }
00112 
00113   };
00114 
00115   template <> 
00116   struct LocalGradient < PoseGraph2D >{
00117     typedef PoseGraph2D PG;
00118 
00119     void operator()( PG::TransformationVectorType& eij,  PG::InformationType& deij_dxi,  PG::InformationType& deij_dxj,  const PG::Edge& e) {
00120       TaylorTerms<PG> taylorTerms;
00121       PG::TransformationType fij;
00122       taylorTerms(fij, deij_dxi, deij_dxj, e);
00123       PG::TransformationType rmean=e.mean(false);
00124       eij=(rmean*fij).toVector();
00125       Matrix3 z=rmean.toMatrix();
00126       z[0][2]=0;
00127       z[1][2]=0;
00128       deij_dxi = z * deij_dxi;
00129       deij_dxj = z * deij_dxj;
00130     }
00131   };
00132 
00133   template <> 
00134   struct ManifoldGradient < PoseGraph2D >{
00135     typedef PoseGraph2D PG;
00136 
00137     void operator()( PG::TransformationVectorType& eij,  PG::InformationType& deij_dxi,  PG::InformationType& deij_dxj,  const PG::Edge& e) {
00138       TaylorTerms<PG> taylorTerms;
00139       PG::TransformationType fij;
00140       taylorTerms(fij, deij_dxi, deij_dxj, e);
00141       PG::TransformationType rmean=e.mean(false);
00142       eij=(rmean*fij).toVector();
00143       Matrix3 z=rmean.toMatrix();
00144       z[0][2]=0;
00145       z[1][2]=0;
00146       deij_dxi = z * deij_dxi;
00147       deij_dxj = z * deij_dxj;
00148     }
00149   };
00150 
00151   template <> 
00152   struct TransformCovariance<PoseGraph2D> {
00153     typedef PoseGraph2D PG;
00154     void operator()(PG::InformationType& covariance, const PG::TransformationType& t, const PG::TransformationType& )
00155     {
00156       PG::InformationType J = t.inverse().toMatrix();
00157       J[0][2]=0.;
00158       J[1][2]=0.;
00159       J[2][0]=0.;
00160       J[2][1]=0.;
00161       J[2][2]=1.;
00162       covariance = J * covariance * J.transpose();
00163     }
00164   };
00165 
00166   template <> 
00167   struct PoseUpdate<PoseGraph2D> {
00168     typedef PoseGraph2D PG;
00169     void operator()(PG::TransformationType& t, PG::TransformationVectorType::BaseType* update)
00170     {
00171       PG::TransformationVectorType p = t.toVector();
00172       for (int k=0; k<3; k++){
00173         if (k==2 && fabs(update[k])>M_PI){
00174           update[k] = update[k]>0?M_PI:-M_PI;
00175         }
00176         p[k]+=update[k];
00177       }
00178       t = PG::TransformationType::fromVector(p);
00179     }
00180   };
00181 
00182 }
00183 
00184 #endif


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