posegraph.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 _AIS_POSEGRAPH_HH_
00018 #define _AIS_POSEGRAPH_HH_
00019 
00020 #include <fstream>
00021 #include "graph.h"
00022 #include "dijkstra.h"
00023 #include <assert.h>
00024 
00025 namespace AISNavigation{
00026 
00027   template <typename T, typename I>
00028   struct PoseGraph : public Graph {
00029     typedef T TransformationType;
00030     typedef I InformationType;
00031     typedef typename TransformationType::TransformationVector TransformationVectorType;
00032 
00033     struct Vertex: public Graph::Vertex{
00034       friend struct PoseGraph;
00035       TransformationType transformation;
00036       TransformationType localTransformation;
00037       InformationType covariance;
00038       virtual ~Vertex();
00039       inline InformationType& A() const {return _A;}
00040       inline TransformationVectorType& b() const {return _b;}
00041       inline void backup() { assert(! _isBackup); _backupPose=transformation; _isBackup=true;}
00042       inline void restore(){ assert(_isBackup); transformation=_backupPose; _isBackup=false; }
00043       inline int& tempIndex() const { return _tempIndex; }
00044       inline bool fixed() const {return _fixed;}
00045       inline bool& fixed() {return _fixed;}
00046 
00047     protected:
00048       Vertex(int id=-1);
00049       mutable InformationType _A;
00050       mutable TransformationVectorType _b;
00051       mutable int _tempIndex;
00052       TransformationType _backupPose;
00053       bool _isBackup;
00054       bool _fixed;
00055     };
00056     
00057     struct Edge: public Graph::Edge {
00058       friend struct PoseGraph;
00059       bool direction(Vertex* from_, Vertex* to_) const; 
00060       const TransformationType& mean(bool direct=true) const;
00061       const InformationType& information(bool direct=true) const;
00062       const InformationType& covariance(bool direct=true) const;
00063       const double&    informationDet(bool direct=true) const;
00064       const double&    covarianceDet(bool direct=true) const;
00065       virtual bool revert();
00066       virtual void setAttributes(const TransformationType& m, const InformationType& i);
00067       double chi2() const;
00068       inline InformationType& AFromTo() const {return _AFromTo;}
00069     protected:
00070       Edge (Vertex* from, Vertex* to, const TransformationType& mean, const InformationType& information);
00071       TransformationType _mean;
00072       InformationType _information;
00073       InformationType _covariance;
00074       double _covDet;
00075       double _infoDet;
00076       
00077       TransformationType _rmean;
00078       InformationType _rinformation;
00079       InformationType _rcovariance;
00080       double _rcovDet;
00081       double _rinfoDet;
00082 
00083       mutable InformationType _AFromTo;
00084     };
00085 
00086     typedef std::set<Vertex*> VertexSet;
00087 
00088 
00089     struct PathLengthCostFunction: public Dijkstra::CostFunction{
00090       virtual double operator()(Graph::Edge* edge, Graph::Vertex* from, Graph::Vertex* to);
00091     };
00092 
00093     struct CovarianceDetCostFunction: public Dijkstra::CostFunction{
00094       virtual double operator()(Graph::Edge* edge, Graph::Vertex* from, Graph::Vertex* to);
00095     };
00096 
00097     
00098 
00099     inline Vertex* vertex(int id){
00100       return reinterpret_cast<Vertex*>(Graph::vertex(id));
00101     }
00102 
00103     inline const Vertex* vertex (int id) const{
00104       return reinterpret_cast<const Vertex*>(Graph::vertex(id));
00105     }
00106  
00107     virtual Vertex* addVertex(const int& k);
00108     virtual Vertex* addVertex(int id, const TransformationType& pose, const InformationType& information);
00109     virtual Edge*   addEdge(Vertex* from, Vertex* to, const TransformationType& mean, const InformationType& information);
00110     virtual void refineEdge(Edge* _e, const TransformationType& mean, const InformationType& information);
00111     //virtual ~PoseGraph();
00112     //virtual void load(std::istream& is, bool overrideCovariances=false, std::vector <Edge*> *orderedEdges=0);
00113     //virtual void save(std::ostream& os, const TransformationType& offset=TransformationType(), int type=0, bool onlyMarked=false) const;
00114 
00115     
00116     void propagateAlongDijkstraTree(PoseGraph<T,I>::Vertex* v, Dijkstra::AdjacencyMap& amap, bool covariance, bool transformation);
00117   };
00118 
00119   template < typename PG > 
00120   struct MotionJacobian {
00123     typename PG::InformationType state(const typename PG::TransformationType& t, const typename PG::TransformationType& m);
00124     typename PG::InformationType measurement(const typename PG::TransformationType& t, const typename PG::TransformationType& m);
00125   };
00126 
00127   template < typename PG > 
00128   struct TaylorTerms {
00129     void operator()(typename PG::TransformationType& fij, typename PG::InformationType& dfij_dxi, typename PG::InformationType& dfij_dxj, const typename PG::Edge& e);
00130   };
00131 
00132   template < typename PG > 
00133   struct Gradient {
00134     void operator()(typename PG::TransformationVectorType& fij, typename PG::InformationType& dfij_dxi, typename PG::InformationType& dfij_dxj, const typename PG::Edge& e);
00135   };
00136 
00137   template < typename PG > 
00138   struct LocalGradient {
00139     void operator()(typename PG::TransformationVectorType& fij, typename PG::InformationType& dfij_dxi, typename PG::InformationType& dfij_dxj, const typename PG::Edge& e);
00140   };
00141 
00142   template < typename PG > 
00143   struct ManifoldGradient {
00144     void operator()(typename PG::TransformationVectorType& fij, typename PG::InformationType& dfij_dxi, typename PG::InformationType& dfij_dxj, const typename PG::Edge& e);
00145   };
00146 
00151   template < typename PG > 
00152   struct TransformCovariance {
00153     void operator()(typename PG::InformationType& covariance, const typename PG::TransformationType& from, const typename PG::TransformationType& to);
00154   };
00155 
00156   template < typename PG > 
00157   struct PoseUpdate {
00158     void operator()(typename PG::TransformationType& t, typename PG::TransformationVectorType::BaseType* update);
00159   };
00160 
00161 } // end namespace
00162 
00163 #include "posegraph.hpp"
00164 
00165 #endif // _AIS_POSEGRAPH_HH_


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