treeoptimizer3.cpp
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 
44 #include "treeoptimizer3.hh"
45 #include <fstream>
46 #include <sstream>
47 #include <string>
48 
49 using namespace std;
50 
51 namespace AISNavigation {
52 
53 //#define DEBUG(i) if (verboseLevel>i) cerr
54 
55 
56 TreeOptimizer3::TreeOptimizer3(){
57  restartOnDivergence=false;
58  sortedEdges=0;
59  mpl=-1;
60  edgeCompareMode=EVComparator<Edge*>::CompareLevel;
61 }
62 
63 
64 TreeOptimizer3::~TreeOptimizer3(){
65 }
66 
67 void TreeOptimizer3::initializeTreeParameters(){
69  treeDepthVisit(pp,root);
70 }
71 
72 
73 void TreeOptimizer3::iterate(TreePoseGraph3::EdgeSet* eset, bool noPreconditioner){
74  TreePoseGraph3::EdgeSet* temp=sortedEdges;
75  if (eset){
76  sortedEdges=eset;
77  }
78 
79  if (noPreconditioner)
80  propagateErrors(false);
81  else {
82  if (iteration==1)
83  computePreconditioner();
84  propagateErrors(true);
85  }
86  sortedEdges=temp;
87 
88  onRestartBegin();
89  if (restartOnDivergence){
90  double mte, ate;
91  double mre, are;
92  error(&mre, &mte, &are, &ate);
93  maxTranslationalErrors.push_back(mte);
94  maxRotationalErrors.push_back(mre);
95  int interval=3;
96  if ((int)maxRotationalErrors.size()>=interval){
97  uint s=(uint)maxRotationalErrors.size();
98  double re0 = maxRotationalErrors[s-interval];
99  double re1 = maxRotationalErrors[s-1];
100 
101  if ((re1-re0)>are || sqrt(re1)>0.99*M_PI){
102  double rg=rotGain;
103  if (sqrt(re1)>M_PI/4){
104  cerr << "RESTART!!!!! : Angular wraparound may be occourring" << endl;
105  cerr << " err=" << re0 << " -> " << re1 << endl;
106  cerr << "Restarting optimization and reducing the rotation factor" << endl;
107  cerr << rg << " -> ";
108  initializeOnTree();
109  initializeTreeParameters();
110  initializeOptimization();
111  error(&mre, &mte);
112  maxTranslationalErrors.push_back(mte);
113  maxRotationalErrors.push_back(mre);
114  rg*=0.1;
115  rotGain=rg;
116  cerr << rotGain << endl;
117  }
118  else {
119  cerr << "decreasing angular gain" << rotGain*0.1 << endl;
120  rotGain*=0.1;
121  }
122  }
123  }
124  }
125  onRestartDone();
126 }
127 
128 
129 void TreeOptimizer3::recomputeTransformations(Vertex*v, Vertex* top){
130  if (v==top)
131  return;
132  recomputeTransformations(v->parent, top);
133  v->transformation=v->parent->transformation*v->parameters;
134 }
135 
136 
137 void TreeOptimizer3::recomputeParameters(Vertex*v, Vertex* top){
138  while (v!=top){
139  v->parameters=v->parent->transformation.inv()*v->transformation;
140  v=v->parent;
141  }
142 }
143 
144 
145 TreeOptimizer3::Transformation TreeOptimizer3::getPose(Vertex*v, Vertex* top){
146  Transformation t(0.,0.,0.,0.,0.,0.);
147  if (v==top)
148  return v->transformation;
149  while (v!=top){
150  t=v->parameters*t;
151  v=v->parent;
152  }
153  return top->transformation*t;
154 }
155 
156 TreeOptimizer3::Rotation TreeOptimizer3::getRotation(Vertex*v, Vertex* top){
157  Rotation r(0.,0.,0.);
158  if (v==top)
159  return v->transformation.rotation();
160  while (v!=top){
161  r=v->parameters.rotation()*r;
162  v=v->parent;
163  }
164  return top->transformation.rotation()*r;
165 }
166 
167 
168 
169 double TreeOptimizer3::error(const Edge* e) const{
170  const Vertex* v1=e->v1;
171  const Vertex* v2=e->v2;
172 
173  Transformation et=e->transformation;
174  Transformation t1=v1->transformation;
175  Transformation t2=v2->transformation;
176 
177  Transformation t12=(t1*et)*t2.inv();
178  Pose p12=t12.toPoseType();
179 
180  Pose ps=e->informationMatrix*p12;
181  double err=p12*ps;
182  //DEBUG(100) << "e(" << v1->id << "," << v2->id << ")" << err << endl;
183  return err;
184 
185 }
186 
187 double TreeOptimizer3::traslationalError(const Edge* e) const{
188  const Vertex* v1=e->v1;
189  const Vertex* v2=e->v2;
190 
191  Transformation et=e->transformation;
192  Transformation t1=v1->transformation;
193  Transformation t2=v2->transformation;
194 
195  Translation t12=(t2.inv()*(t1*et)).translation();
196  return t12*t12;;
197 
198 }
199 
200 double TreeOptimizer3::rotationalError(const Edge* e) const{
201  const Vertex* v1=e->v1;
202  const Vertex* v2=e->v2;
203 
204  Rotation er=e->transformation.rotation();
205  Rotation r1=v1->transformation.rotation();
206  Rotation r2=v2->transformation.rotation();
207 
208  Rotation r12=r2.inverse()*(r1*er);
209  double r=r12.angle();
210  return r*r;
211 }
212 
213 
214 double TreeOptimizer3::loopError(const Edge* e) const{
215  double err=0;
216  const Vertex* v=e->v1;
217  while (v!=e->top){
218  err+=error(v->parentEdge);
219  v=v->parent;
220  }
221  v=e->v2;
222  while (v==e->top){
223  err+=error(v->parentEdge);
224  v=v->parent;
225  }
226  if (e->v2->parentEdge!=e && e->v1->parentEdge!=e)
227  err+=error(e);
228  return err;
229 }
230 
231 double TreeOptimizer3::loopRotationalError(const Edge* e) const{
232  double err=0;
233  const Vertex* v=e->v1;
234  while (v!=e->top){
235  err+=rotationalError(v->parentEdge);
236  v=v->parent;
237  }
238  v=e->v2;
239  while (v!=e->top){
240  err+=rotationalError(v->parentEdge);
241  v=v->parent;
242  }
243  if (e->v2->parentEdge!=e && e->v1->parentEdge!=e)
244  err+=rotationalError(e);
245  return err;
246 }
247 
248 
249 double TreeOptimizer3::error(double* mre, double* mte, double* are, double* ate, TreePoseGraph3::EdgeSet* eset) const{
250  double globalRotError=0.;
251  double maxRotError=0;
252  double globalTrasError=0.;
253  double maxTrasError=0;
254 
255  int c=0;
256  if (! eset){
257  for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
258  double re=rotationalError(it->second);
259  globalRotError+=re;
260  maxRotError=maxRotError>re?maxRotError:re;
261  double te=traslationalError(it->second);
262  globalTrasError+=te;
263  maxTrasError=maxTrasError>te?maxTrasError:te;
264  c++;
265  }
266  } else {
267  for (TreePoseGraph3::EdgeSet::const_iterator it=eset->begin(); it!=eset->end(); it++){
268  const TreePoseGraph3::Edge* edge=*it;
269  double re=rotationalError(edge);
270  globalRotError+=re;
271  maxRotError=maxRotError>re?maxRotError:re;
272  double te=traslationalError(edge);
273  globalTrasError+=te;
274  maxTrasError=maxTrasError>te?maxTrasError:te;
275  c++;
276  }
277  }
278 
279  if (mte)
280  *mte=maxTrasError;
281  if (mre)
282  *mre=maxRotError;
283  if (ate)
284  *ate=globalTrasError/c;
285  if (are)
286  *are=globalRotError/c;
287  return globalRotError+globalTrasError;
288 }
289 
290 
291 
292 void TreeOptimizer3::initializeOptimization(EdgeCompareMode mode){
293  edgeCompareMode=mode;
294  // compute the size of the preconditioning matrix
295  int sz=maxIndex()+1;
296  //DEBUG(1) << "Size= " << sz << endl;
297  M.resize(sz);
298 
299  //DEBUG(1) << "allocating M(" << sz << ")" << endl;
300  iteration=1;
301 
302  // sorting edges
303  if (sortedEdges!=0){
304  delete sortedEdges;
305  sortedEdges=0;
306  }
307  sortedEdges=sortEdges();
308  mpl=maxPathLength();
309  rotGain=1.;
310  trasGain=1.;
311 }
312 
313 void TreeOptimizer3::initializeOnlineIterations(){
314  int sz=maxIndex()+1;
315  //DEBUG(1) << "Size= " << sz << endl;
316  M.resize(sz);
317  //DEBUG(1) << "allocating M(" << sz << ")" << endl;
318  iteration=1;
319  maxRotationalErrors.clear();
320  maxTranslationalErrors.clear();
321  rotGain=1.;
322  trasGain=1.;
323 }
324 
325 void TreeOptimizer3::initializeOnlineOptimization(EdgeCompareMode mode){
326  edgeCompareMode=mode;
327  // compute the size of the preconditioning matrix
328  clear();
329  Vertex* v0=addVertex(0,Pose(0,0,0,0,0,0));
330  root=v0;
331  v0->parameters=Transformation(v0->pose);
332  v0->parentEdge=0;
333  v0->parent=0;
334  v0->level=0;
335  v0->transformation=Transformation(TreePoseGraph3::Pose(0,0,0,0,0,0));
336 }
337 
338 void TreeOptimizer3::onStepStart(Edge* e){
339  //DEBUG(5) << "entering edge" << e << endl;
340 }
341 void TreeOptimizer3::onStepFinished(Edge* e){
342  //DEBUG(5) << "exiting edge" << e << endl;
343 }
344 
345 void TreeOptimizer3::onIterationStart(int iteration){
346  //DEBUG(5) << "entering iteration " << iteration << endl;
347 }
348 
349 void TreeOptimizer3::onIterationFinished(int iteration){
350  //DEBUG(5) << "exiting iteration " << iteration << endl;
351 }
352 
353 void TreeOptimizer3::onRestartBegin(){}
354 void TreeOptimizer3::onRestartDone(){}
355 bool TreeOptimizer3::isDone(){
356  return false;
357 }
358 
359 
360 }; //namespace AISNavigation
root
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
unsigned int uint
Definition: type_int.hpp:170
GLM_FUNC_DECL genType e()
Transformation3< T > inv() const
A comparator class (struct) that compares the level of two vertices if edges.
Definition: posegraph.hh:62
Defines the core optimizer class for 3D graphs which is a subclass of TreePoseGraph3.
unsigned int uint
Definition: posegraph2.cpp:53
A class (struct) to compute the parameterization of the vertex v.
Definition: posegraph3.hh:118
Quaternion< T > inverse() const
Pose3< T > toPoseType() const


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:38:58