Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00043 #ifndef _TREEPOSEGRAPH_HXX_
00044 #define _TREEPOSEGRAPH_HXX_
00045
00046 #include <iostream>
00047 #include <assert.h>
00048
00049 #include <set>
00050 #include <list>
00051 #include <map>
00052 #include <deque>
00053 #include <vector>
00054 #include <limits>
00055 #include <algorithm>
00056
00057 namespace AISNavigation{
00058
00061 template <class E>
00062 struct EVComparator{
00064 enum CompareMode {CompareLevel, CompareLength};
00065 CompareMode mode;
00066
00067 EVComparator(){
00068 mode=CompareLevel;
00069 }
00070 inline bool operator() (const E& e1, const E& e2){
00071 int o1=0, o2=0;
00072 switch (mode){
00073 case CompareLevel:
00074 o1=e1->top->level;
00075 o2=e2->top->level;
00076 break;
00077 case CompareLength:
00078 o1=e1->length;
00079 o2=e2->length;
00080 break;
00081 }
00082 return o1<o2;
00083 }
00084 };
00085
00090 template <class Ops>
00091 struct TreePoseGraph{
00092 typedef typename Ops::BaseType BaseType;
00093 typedef typename Ops::PoseType Pose;
00094 typedef typename Ops::RotationType Rotation;
00095 typedef typename Ops::TranslationType Translation;
00096 typedef typename Ops::TransformationType Transformation;
00097 typedef typename Ops::CovarianceType Covariance;
00098 typedef typename Ops::InformationType Information;
00099 typedef typename Ops::ParametersType Parameters;
00100
00101 struct Vertex;
00102
00105 struct Edge{
00106 Vertex* v1;
00107 Vertex* v2;
00108 Vertex* top;
00109 int length;
00110 Transformation transformation;
00111 Information informationMatrix;
00113 bool mark;
00114
00115 double learningRate;
00116 };
00117
00118 typedef typename EVComparator<Edge*>::CompareMode EdgeCompareMode;
00119 typedef typename std::list< Edge* > EdgeList;
00120 typedef typename std::map< int, Vertex* > VertexMap;
00121 typedef typename std::set< Vertex* > VertexSet;
00122 typedef typename std::map< Edge*, Edge* > EdgeMap;
00123 typedef typename std::multiset< Edge*, EVComparator<Edge*> > EdgeSet;
00124
00127 struct Vertex {
00128
00129
00130 int id;
00131 EdgeList edges;
00133
00134 int level;
00135 Vertex* parent;
00136 Edge* parentEdge;
00137 EdgeList children;
00139
00140 Transformation transformation;
00141 Pose pose;
00142 Parameters parameters;
00144 bool mark;
00145 };
00146
00148 Vertex* vertex(int id);
00149
00151 const Vertex* vertex (int id) const;
00152
00154 Edge* edge(int id1, int id2);
00155
00157 const Edge* edge(int id1, int id2) const;
00158
00160 Vertex* addVertex(int id, const Pose& pose);
00162 Vertex* removeVertex (int id);
00163
00165 Edge* addEdge(Vertex* v1, Vertex* v2, const Transformation& t, const Information& i);
00166
00168 Edge* removeEdge(Edge* eq);
00169
00184 Edge* addIncrementalEdge(int id1, int id2, const Transformation& t, const Information& i);
00185
00189 EdgeSet* affectedEdges(Vertex* v);
00190
00191 EdgeSet* affectedEdges(VertexSet& vl);
00192
00194 template <class Action>
00195 void treeBreadthVisit(Action& act);
00196
00198 template <class Action>
00199 void treeDepthVisit(Action& act, Vertex *v);
00200
00202 bool buildMST(int id);
00203
00205 bool buildSimpleTree();
00206
00208 void revertEdge(Edge* e);
00209
00211 virtual void revertEdgeInfo(Edge* e) = 0;
00212
00214 virtual void initializeFromParentEdge(Vertex* v) = 0;
00215
00217 void clear();
00218
00220 TreePoseGraph(){
00221 sortedEdges=0;
00222 edgeCompareMode=EVComparator<Edge*>::CompareLevel;
00223 }
00224
00226 virtual ~TreePoseGraph();
00227
00229 EdgeSet* sortEdges();
00230
00232 int maxPathLength();
00233
00235 int totalPathLength();
00236
00238 void compressIndices();
00239
00241 int maxIndex();
00242
00245 bool sanityCheck();
00246
00248 Vertex* root;
00249
00251 VertexMap vertices;
00252
00254 EdgeMap edges;
00255
00260 EdgeSet* sortedEdges;
00261
00262 protected:
00263 void fillEdgeInfo(Edge* e);
00264 void fillEdgesInfo();
00265 EdgeCompareMode edgeCompareMode;
00266 };
00267
00268
00269
00270 #include "posegraph.hxx"
00271
00272 };
00273
00274 #endif