posegraph.hh
Go to the documentation of this file.
1 /**********************************************************************
2  *
3  * This source code is part of the Tree-based Network Optimizer (TORO)
4  *
5  * TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss,
6  * Slawomir Grzonka, and Wolfram Burgard
7  *
8  * TORO is licences under the Common Creative License,
9  * Attribution-NonCommercial-ShareAlike 3.0
10  *
11  * You are free:
12  * - to Share - to copy, distribute and transmit the work
13  * - to Remix - to adapt the work
14  *
15  * Under the following conditions:
16  *
17  * - Attribution. You must attribute the work in the manner specified
18  * by the author or licensor (but not in any way that suggests that
19  * they endorse you or your use of the work).
20  *
21  * - Noncommercial. You may not use this work for commercial purposes.
22  *
23  * - Share Alike. If you alter, transform, or build upon this work,
24  * you may distribute the resulting work only under the same or
25  * similar license to this one.
26  *
27  * Any of the above conditions can be waived if you get permission
28  * from the copyright holder. Nothing in this license impairs or
29  * restricts the author's moral rights.
30  *
31  * TORO is distributed in the hope that it will be useful,
32  * but WITHOUT ANY WARRANTY; without even the implied
33  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
34  * PURPOSE.
35  **********************************************************************/
36 
43 #ifndef _TREEPOSEGRAPH_HXX_
44 #define _TREEPOSEGRAPH_HXX_
45 
46 #include <iostream>
47 #include <assert.h>
48 
49 #include <set>
50 #include <list>
51 #include <map>
52 #include <deque>
53 #include <vector>
54 #include <limits>
55 #include <algorithm>
56 
57 namespace AISNavigation{
58 
61 template <class E>
62 struct EVComparator{
66 
68  mode=CompareLevel;
69  }
70  inline bool operator() (const E& e1, const E& e2) const {
71  int o1=0, o2=0;
72  switch (mode){
73  case CompareLevel:
74  o1=e1->top->level;
75  o2=e2->top->level;
76  break;
77  case CompareLength:
78  o1=e1->length;
79  o2=e2->length;
80  break;
81  }
82  return o1<o2;
83  }
84 };
85 
90 template <class Ops>
92  typedef typename Ops::BaseType BaseType;
93  typedef typename Ops::PoseType Pose;
94  typedef typename Ops::RotationType Rotation;
95  typedef typename Ops::TranslationType Translation;
96  typedef typename Ops::TransformationType Transformation;
97  typedef typename Ops::CovarianceType Covariance;
98  typedef typename Ops::InformationType Information;
99  typedef typename Ops::ParametersType Parameters;
100 
101  struct Vertex;
102 
105  struct Edge{
109  int length;
110  Transformation transformation;
111  Information informationMatrix;
113  bool mark;
114 
115  double learningRate;
116  };
117 
119  typedef typename std::list< Edge* > EdgeList;
120  typedef typename std::map< int, Vertex* > VertexMap;
121  typedef typename std::set< Vertex* > VertexSet;
122  typedef typename std::map< Edge*, Edge* > EdgeMap;
123  typedef typename std::multiset< Edge*, EVComparator<Edge*> > EdgeSet;
124 
127  struct Vertex {
128 
129  // Graph-related elements
130  int id;
131  EdgeList edges;
133  // Tree-related elements
134  int level;
137  EdgeList children;
139  // Parameterization-related elements
140  Transformation transformation;
141  Pose pose;
142  Parameters parameters;
144  bool mark;
145  };
146 
148  Vertex* vertex(int id);
149 
151  const Vertex* vertex (int id) const;
152 
154  Edge* edge(int id1, int id2);
157  const Edge* edge(int id1, int id2) const;
158 
160  Vertex* addVertex(int id, const Pose& pose);
162  Vertex* removeVertex (int id);
163 
165  Edge* addEdge(Vertex* v1, Vertex* v2, const Transformation& t, const Information& i);
166 
168  Edge* removeEdge(Edge* eq);
169 
184  Edge* addIncrementalEdge(int id1, int id2, const Transformation& t, const Information& i);
185 
189  EdgeSet* affectedEdges(Vertex* v);
190 
191  EdgeSet* affectedEdges(VertexSet& vl);
192 
194  template <class Action>
195  void treeBreadthVisit(Action& act);
196 
198  template <class Action>
199  void treeDepthVisit(Action& act, Vertex *v);
200 
202  bool buildMST(int id);
203 
205  bool buildSimpleTree();
206 
208  void revertEdge(Edge* e);
209 
211  virtual void revertEdgeInfo(Edge* e) = 0;
212 
214  virtual void initializeFromParentEdge(Vertex* v) = 0;
215 
217  void clear();
218 
221  sortedEdges=0;
222  edgeCompareMode=EVComparator<Edge*>::CompareLevel;
223  }
224 
226  virtual ~TreePoseGraph();
227 
229  EdgeSet* sortEdges();
230 
232  int maxPathLength();
233 
235  int totalPathLength();
236 
238  void compressIndices();
239 
241  int maxIndex();
242 
245  bool sanityCheck();
246 
248  Vertex* root;
249 
251  VertexMap vertices;
252 
254  EdgeMap edges;
255 
260  EdgeSet* sortedEdges;
261 
262 protected:
263  void fillEdgeInfo(Edge* e);
264  void fillEdgesInfo();
265  EdgeCompareMode edgeCompareMode;
266 };
267 
268 //include the template implementation part
269 
270 #include "posegraph.hxx"
271 
272 }; //namespace AISNavigation
273 
274 #endif
Edge * addIncrementalEdge(int id1, int id2, const Transformation &t, const Information &i)
Definition: posegraph.hh:155
Definition of a vertex in the graph based on the template input from Ops.
Definition: posegraph.hh:127
Operations2D< double > ::BaseType BaseType
Definition: posegraph.hh:92
Ops::CovarianceType Covariance
Definition: posegraph.hh:97
EdgeCompareMode edgeCompareMode
Definition: posegraph.hh:265
bool operator()(const E &e1, const E &e2) const
Definition: posegraph.hh:70
Ops::RotationType Rotation
Definition: posegraph.hh:94
Ops::TransformationType Transformation
Definition: posegraph.hh:96
GLM_FUNC_DECL genType e()
A comparator class (struct) that compares the level of two vertices if edges.
Definition: posegraph.hh:62
Definition of an edge in the graph based on the template input from Ops.
Definition: posegraph.hh:105
std::map< int, Vertex *> VertexMap
Definition: posegraph.hh:120
Ops::ParametersType Parameters
Definition: posegraph.hh:99
std::list< Edge *> EdgeList
Definition: posegraph.hh:119
std::map< Edge *, Edge *> EdgeMap
Definition: posegraph.hh:122
EVComparator< Edge * >::CompareMode EdgeCompareMode
Definition: posegraph.hh:118
std::set< Vertex *> VertexSet
Definition: posegraph.hh:121
Ops::TranslationType Translation
Definition: posegraph.hh:95
The template class for representing an abstract tree without specifing the dimensionality of the exac...
Definition: posegraph.hh:91
Ops::InformationType Information
Definition: posegraph.hh:98
std::multiset< Edge *, EVComparator< Edge * > > EdgeSet
Definition: posegraph.hh:123


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:29