Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
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
00112
00113
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 }
00162
00163 #include "posegraph.hpp"
00164
00165 #endif // _AIS_POSEGRAPH_HH_