treeoptimizer3_iteration.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 
37 #include "treeoptimizer3.hh"
38 #include <fstream>
39 #include <string>
40 
41 using namespace std;
42 
43 namespace AISNavigation {
44 
45 //#define DEBUG(i) if (verboseLevel>i) cerr
46 
47 //helper functions. Should I explain :-)?
48 inline double max3( const double& a, const double& b, const double& c){
49  double m=a>b?a:b;
50  return m>c?m:c;
51 }
52 inline double min3( const double& a, const double& b, const double& c){
53  double m=a<b?a:b;
54  return m<c?m:c;
55 }
56 
57 
58 struct NodeInfo{
59  TreeOptimizer3::Vertex* n;
62  int direction;
63  TreeOptimizer3::Transformation transformation;
64  TreeOptimizer3::Transformation parameters;
65  NodeInfo(TreeOptimizer3::Vertex* v=0, double tw=0, double rw=0, int dir=0,
66  TreeOptimizer3::Transformation t=TreeOptimizer3::Transformation(0,0,0,0,0,0),
67  TreeOptimizer3::Parameters p=TreeOptimizer3::Transformation(0,0,0,0,0,0)){
68  n=v;
69  translationalWeight=tw;
70  rotationalWeight=rw;
71  direction=dir;
72  transformation=t;
73  parameters=p;
74  }
75 };
76 
77 typedef std::vector<NodeInfo> NodeInfoVector;
78 
79 
80 /********************************** Preconditioned and unpreconditioned error distribution ************************************/
81 
82 
83 
84 
85 
86 void TreeOptimizer3::computePreconditioner(){
87  for (uint i=0; i<M.size(); i++){
88  M[i][0]=0;
89  M[i][1]=0;
90  }
91  gamma[0] = gamma[1] = numeric_limits<double>::max();
92 
93  int edgeCount=0;
94  for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
95  edgeCount++;
96  //if (! (edgeCount%1000))
97  // DEBUG(1) << "m";
98 
99  Edge* e=*it;
100  //Transformation t=e->transformation;
101  InformationMatrix W=e->informationMatrix;
102 
103  Vertex* top=e->top;
104  for (int dir=0; dir<2; dir++){
105  Vertex* n = (dir==0)? e->v1 : e->v2;
106  while (n!=top){
107  uint i=n->id;
108  double rW=min3(W[0][0], W[1][1], W[2][2]);
109  double tW=min3(W[3][3], W[4][4], W[5][5]);
110  M[i][0]+=rW;
111  M[i][1]+=tW;
112  gamma[0]=gamma[0]<rW?gamma[0]:rW;
113  gamma[1]=gamma[1]<tW?gamma[1]:tW;
114  n=n->parent;
115  }
116  }
117 
118  }
119 
120  if (verboseLevel>1){
121  for (uint i=0; i<M.size(); i++){
122  cerr << "M[" << i << "]=" << M[i][0] << " " << M[i][1] << endl;
123  }
124  }
125 }
126 
127 
128 void TreeOptimizer3::propagateErrors(bool usePreconditioner){
129  iteration++;
130  int edgeCount=0;
131  // this is the workspace for computing the paths without
132  // bothering too much the memory allocation
133  static NodeInfoVector path;
134  path.resize(edges.size()+1);
135  static Rotation zero(0.,0.,0.);
136 
137  onIterationStart(iteration);
138  for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
139  edgeCount++;
140  //if (! (edgeCount%1000))
141  // DEBUG(1) << "c";
142 
143  if (isDone())
144  return;
145 
146  Edge* e=*it;
147  Vertex* top=e->top;
148  Vertex* v1=e->v1;
149  Vertex* v2=e->v2;
150  int l=e->length;
151  onStepStart(e);
152 
153  recomputeTransformations(v1,top);
154  recomputeTransformations(v2,top);
155  //DEBUG(2) << "Edge: " << v1->id << " " << v2->id << ", top=" << top->id << ", length="<< l <<endl;
156 
157  //BEGIN: Path and weight computation
158  int pc=0;
159  Vertex* aux=v1;
160  double totTW=0, totRW=0;
161  while(aux!=top){
162  int index=aux->id;
163  double tw=1./(double)l, rw=1./(double)l;
164  if (usePreconditioner){
165  tw=1./M[index][0];
166  rw=1./M[index][1];
167  }
168  totTW+=tw;
169  totRW+=rw;
170  path[pc++]=NodeInfo(aux,tw,rw,-1,aux->transformation, aux->parameters);
171  aux=aux->parent;
172  }
173  int topIndex=pc;
174  path[pc++]=NodeInfo(top,0.,0.,0, top->transformation, top->parameters);
175  pc=l;
176  aux=v2;
177  while(aux!=top){
178  int index=aux->id;
179  double tw=1./l, rw=1./l;
180  if (usePreconditioner){
181  tw=1./M[index][0];
182  rw=1./M[index][1];
183  }
184  totTW+=tw;
185  totRW+=rw;
186  path[pc--]=NodeInfo(aux,tw,rw,1,aux->transformation, aux->parameters);
187  aux=aux->parent;
188  }
189 
190  //store the transformations relative to the top node
191  //Transformation topTransformation=top->transformation;
192  //Transformation topParameters=top->parameters;
193 
194  //END: Path and weight computation
195 
196 
197 
198  //BEGIN: Rotational Error
199  Rotation r1=getRotation(v1, top);
200  Rotation r2=getRotation(v2, top);
201  Rotation re=e->transformation.rotation();
202  Rotation rR=r2.inverse()*(r1*re);
203 
204  double rotationFactor=(usePreconditioner)?
205  sqrt(double(l))* min3(e->informationMatrix[0][0],
206  e->informationMatrix[1][1],
207  e->informationMatrix[2][2])/
208  ( gamma[0]* (double)iteration ):
209  sqrt(double(l))*rotGain/(double)iteration;
210 
211 // double rotationFactor=(usePreconditioner)?
212 // sqrt(double(l))*rotGain/
213 // ( gamma[0]* (double)iteration * min3(e->informationMatrix[0][0],
214 // e->informationMatrix[1][1],
215 // e->informationMatrix[2][2])):
216 // sqrt(double(l))*rotGain/(double)iteration;
217 
218  if (rotationFactor>1)
219  rotationFactor=1;
220 
221  Rotation totalRotation = path[l].transformation.rotation() * rR * path[l].transformation.rotation().inverse();
222 
223  Translation axis = totalRotation.axis();
224  double angle=totalRotation.angle();
225 
226  double cw=0;
227  for (int i= 1; i<=topIndex; i++){
228  cw+=path[i-1].rotationalWeight/totRW;
229  Rotation R=path[i].transformation.rotation();
230  Rotation B(axis, angle*cw*rotationFactor);
231  R= B*R;
232  path[i].transformation.setRotation(R);
233  }
234  for (int i= topIndex+1; i<=l; i++){
235  cw+=path[i].rotationalWeight/totRW;
236  Rotation R=path[i].transformation.rotation();
237  Rotation B(axis, angle*cw*rotationFactor);
238  R= B*R;
239  path[i].transformation.setRotation(R);
240  }
241 
242  //recompute the parameters based on the transformation
243  for (int i=0; i<topIndex; i++){
244  Vertex* n=path[i].n;
245  n->parameters.setRotation(path[i+1].transformation.rotation().inverse()*path[i].transformation.rotation());
246  }
247  for (int i= topIndex+1; i<=l; i++){
248  Vertex* n=path[i].n;
249  n->parameters.setRotation(path[i-1].transformation.rotation().inverse()*path[i].transformation.rotation());
250  }
251 
252  //END: Rotational Error
253 
254 
255  //now spread the parameters
256  recomputeTransformations(v1,top);
257  recomputeTransformations(v2,top);
258 
259  //BEGIN: Translational Error
260  //Translation topTranslation=top->transformation.translation();
261 
262  Transformation tr12=v1->transformation*e->transformation;
263  Translation tR=tr12.translation()-v2->transformation.translation();
264 
265 
266 // double translationFactor=(usePreconditioner)?
267 // trasGain*l/( gamma[1]* (double)iteration * min3(e->informationMatrix[3][3],
268 // e->informationMatrix[4][4],
269 // e->informationMatrix[5][5])):
270 // trasGain*l/(double)iteration;
271 
272  double translationFactor=(usePreconditioner)?
273  trasGain*l*min3(e->informationMatrix[3][3],
274  e->informationMatrix[4][4],
275  e->informationMatrix[5][5])/( gamma[1]* (double)iteration):
276  trasGain*l/(double)iteration;
277 
278  if (translationFactor>1)
279  translationFactor=1;
280  Translation dt=tR*translationFactor;
281 
282  //left wing
283  double lcum=0;
284  for (int i=topIndex-1; i>=0; i--){
285  Vertex* n=path[i].n;
286  lcum-=(usePreconditioner) ? path[i].translationalWeight/totTW : 1./(double)l;
287  double fraction=lcum;
288  Translation offset= dt*fraction;
289  Translation T=n->transformation.translation()+offset;
290  n->transformation.setTranslation(T);
291  }
292  //right wing
293 
294  double rcum=0;
295  for (int i=topIndex+1; i<=l; i++){
296  Vertex* n=path[i].n;
297  rcum+=(usePreconditioner) ? path[i].translationalWeight/totTW : 1./(double)l;
298  double fraction=rcum;
299  Translation offset= dt*fraction;
300  Translation T=n->transformation.translation()+offset;
301  n->transformation.setTranslation(T);
302  }
303  assert(fabs(lcum+rcum)-1<1e-6);
304 
305  recomputeParameters(v1, top);
306  recomputeParameters(v2, top);
307 
308  //END: Translational Error
309 
310  onStepFinished(e);
311 
312  if (verboseLevel>2){
313  Rotation newRotResidual=v2->transformation.rotation().inverse()*(v1->transformation.rotation()*re);
314  Translation newRotResidualAxis=newRotResidual.axis();
315  double newRotResidualAngle=newRotResidual.angle();
316  Translation rotResidualAxis=rR.axis();
317  double rotResidualAngle=rR.angle();
318  Translation newTransResidual=(v1->transformation*e->transformation).translation()-v2->transformation.translation();
319 
320  cerr << "RotationalFraction: " << rotationFactor << endl;
321  cerr << "Rotational residual: "
322  << " axis " << rotResidualAxis.x() << "\t" << rotResidualAxis.y() << "\t" << rotResidualAxis.z() << " --> "
323  << " -> " << newRotResidualAxis.x() << "\t" << newRotResidualAxis.y() << "\t" << newRotResidualAxis.z() << endl;
324  cerr << " angle " << rotResidualAngle << "\t" << newRotResidualAngle << endl;
325 
326  cerr << "Translational Fraction: " << translationFactor << endl;
327  cerr << "Translational Residual" << endl;
328  cerr << " " << tR.x() << "\t" << tR.y() << "\t" << tR.z() << endl;
329  cerr << " " << newTransResidual.x() << "\t" << newTransResidual.y() << "\t" << newTransResidual.z() << endl;
330  }
331 
332  if (verboseLevel>101){
333  char filename [1000];
334  sprintf(filename, "po-%02d-%03d-%03d-.dat", iteration, v1->id, v2->id);
335  recomputeAllTransformations();
336  saveGnuplot(filename);
337  }
338  }
339  onIterationFinished(iteration);
340 }
341 
342 };//namespace AISNavigation
Vector3< T > axis() const
TreeOptimizer3::Vertex * n
double max3(const double &a, const double &b, const double &c)
const T & y() const
NodeInfo(TreeOptimizer3::Vertex *v=0, double tw=0, double rw=0, int dir=0, TreeOptimizer3::Transformation t=TreeOptimizer3::Transformation(0, 0, 0, 0, 0, 0), TreeOptimizer3::Parameters p=TreeOptimizer3::Transformation(0, 0, 0, 0, 0, 0))
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
GLM_FUNC_DECL detail::tvec3< T, P > axis(detail::tquat< T, P > const &x)
GLM_FUNC_DECL genType e()
TreeOptimizer3::Transformation parameters
TreeOptimizer3::Transformation transformation
Vector3< T > translation() const
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
Defines the core optimizer class for 3D graphs which is a subclass of TreePoseGraph3.
const T & z() const
Quaternion< T > inverse() const
const T & x() const
std::vector< NodeInfo > NodeInfoVector
unsigned int uint
Definition: posegraph2.cpp:53
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
GLM_FUNC_DECL genType zero()
double min3(const double &a, const double &b, const double &c)


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:37:06