treeoptimizer2.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 "treeoptimizer2.hh"
45 #include <fstream>
46 #include <sstream>
47 #include <string>
48 
49 typedef unsigned int uint;
50 using namespace std;
51 
52 namespace AISNavigation {
53 
54 //#define DEBUG(i) if (verboseLevel>i) cerr
55 
58  void perform(TreePoseGraph2::Vertex* v){
59  if (!v->parent){
60  v->parameters=TreePoseGraph2::Pose(0.,0.,0.);
61  return;
62  }
63  v->parameters=TreePoseGraph2::Pose(v->pose.x()-v->parent->pose.x(),
64  v->pose.y()-v->parent->pose.y(),
65  v->pose.theta()-v->parent->pose.theta());
66  }
67 };
68 
69 TreeOptimizer2::TreeOptimizer2():
70  iteration(1){
71  sortedEdges=0;
72 }
73 
75 }
76 
79  treeDepthVisit(pp, root);
80 }
81 
83  // compute the size of the preconditioning matrix
84  int sz=maxIndex()+1;
85  //DEBUG(1) << "Size= " << sz << endl;
86  M.resize(sz);
87  //DEBUG(1) << "allocating M(" << sz << ")" << endl;
88  iteration=1;
89 
90  // sorting edges
91  if (sortedEdges!=0){
92  delete sortedEdges;
93  sortedEdges=0;
94  }
96 }
97 
99  // compute the size of the preconditioning matrix
100  int sz=maxIndex()+1;
101  //DEBUG(1) << "Size= " << sz << endl;
102  M.resize(sz);
103  //DEBUG(1) << "allocating M(" << sz << ")" << endl;
104  iteration=1;
105 }
106 
108  gamma[0] = gamma[1] = gamma[2] = numeric_limits<double>::max();
109 
110  for (uint i=0; i<M.size(); i++)
111  M[i]=Pose(0.,0.,0.);
112 
113  int edgeCount=0;
114  for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
115  edgeCount++;
116  //if (! (edgeCount%10000))
117  // DEBUG(1) << "m";
118 
119  Edge* e=*it;
120  Transformation t=e->transformation;
121  InformationMatrix S=e->informationMatrix;
122 
124  R.values[0][0]=t.rotationMatrix[0][0];
125  R.values[0][1]=t.rotationMatrix[0][1];
126  R.values[0][2]=0;
127 
128  R.values[1][0]=t.rotationMatrix[1][0];
129  R.values[1][1]=t.rotationMatrix[1][1];
130  R.values[1][2]=0;
131 
132  R.values[2][0]=0;
133  R.values[2][1]=0;
134  R.values[2][2]=1;
135 
136  InformationMatrix W =R*S*R.transpose();
137 
138  Vertex* top=e->top;
139  for (int dir=0; dir<2; dir++){
140  Vertex* n = (dir==0)? e->v1 : e->v2;
141  while (n!=top){
142  uint i=n->id;
143  M[i].values[0]+=W.values[0][0];
144  M[i].values[1]+=W.values[1][1];
145  M[i].values[2]+=W.values[2][2];
146  gamma[0]=gamma[0]<W.values[0][0]?gamma[0]:W.values[0][0];
147  gamma[1]=gamma[1]<W.values[1][1]?gamma[1]:W.values[1][1];
148  gamma[2]=gamma[2]<W.values[2][2]?gamma[2]:W.values[2][2];
149  n=n->parent;
150  }
151  }
152  }
153 
154  if (verboseLevel>1){
155  for (uint i=0; i<M.size(); i++){
156  cerr << "M[" << i << "]=" << M[i].x() << " " << M[i].y() << " " << M[i].theta() <<endl;
157  }
158  }
159 }
160 
161 
163  iteration++;
164  int edgeCount=0;
165 
166  for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
167  edgeCount++;
168  //if (! (edgeCount%10000)) DEBUG(1) << "c";
169 
170  Edge* e=*it;
171  Vertex* top=e->top;
172 
173 
174  Vertex* v1=e->v1;
175  Vertex* v2=e->v2;
176 
177  double l=e->length;
178  //DEBUG(2) << "Edge: " << v1->id << " " << v2->id << ", top=" << top->id << ", length="<< l <<endl;
179 
180  Pose p1=getPose(v1, top);
181  Pose p2=getPose(v2, top);
182 
183  //DEBUG(2) << " p1=" << p1.x() << " " << p1.y() << " " << p1.theta() << endl;
184  //DEBUG(2) << " p2=" << p2.x() << " " << p2.y() << " " << p2.theta() << endl;
185 
186  Transformation et=e->transformation;
187  Transformation t1(p1);
188  Transformation t2(p2);
189 
190  Transformation t12=t1*et;
191 
192  Pose p12=t12.toPoseType();
193  //DEBUG(2) << " pt2=" << p12.x() << " " << p12.y() << " " << p12.theta() << endl;
194 
195  Pose r(p12.x()-p2.x(), p12.y()-p2.y(), p12.theta()-p2.theta());
196  double angle=r.theta();
197  angle=atan2(sin(angle),cos(angle));
198  r.theta()=angle;
199  //DEBUG(2) << " e=" << r.x() << " " << r.y() << " " << r.theta() << endl;
200 
201  InformationMatrix S=e->informationMatrix;
203  R.values[0][0]=t1.rotationMatrix[0][0];
204  R.values[0][1]=t1.rotationMatrix[0][1];
205  R.values[0][2]=0;
206 
207  R.values[1][0]=t1.rotationMatrix[1][0];
208  R.values[1][1]=t1.rotationMatrix[1][1];
209  R.values[1][2]=0;
210 
211  R.values[2][0]=0;
212  R.values[2][1]=0;
213  R.values[2][2]=1;
214 
215  InformationMatrix W=R*S*R.transpose();
216  Pose d=W*r*2.;
217 
218  //DEBUG(2) << " d=" << d.x() << " " << d.y() << " " << d.theta() << endl;
219 
220  assert(l>0);
221 
222  double alpha[3] = { 1./(gamma[0]*iteration), 1./(gamma[1]*iteration), 1./(gamma[2]*iteration) };
223 
224  double tw[3]={0.,0.,0.};
225  for (int dir=0; dir<2; dir++) {
226  Vertex* n = (dir==0)? v1 : v2;
227  while (n!=top){
228  uint i=n->id;
229  tw[0]+=1./M[i].values[0];
230  tw[1]+=1./M[i].values[1];
231  tw[2]+=1./M[i].values[2];
232  n=n->parent;
233  }
234  }
235 
236  double beta[3] = {l*alpha[0]*d.values[0], l*alpha[1]*d.values[1], l*alpha[2]*d.values[2]};
237  beta[0]=(fabs(beta[0])>fabs(r.values[0]))?r.values[0]:beta[0];
238  beta[1]=(fabs(beta[1])>fabs(r.values[1]))?r.values[1]:beta[1];
239  beta[2]=(fabs(beta[2])>fabs(r.values[2]))?r.values[2]:beta[2];
240 
241  //DEBUG(2) << " alpha=" << alpha[0] << " " << alpha[1] << " " << alpha[2] << endl;
242  //DEBUG(2) << " beta=" << beta[0] << " " << beta[1] << " " << beta[2] << endl;
243 
244  for (int dir=0; dir<2; dir++) {
245  Vertex* n = (dir==0)? v1 : v2;
246  double sign=(dir==0)? -1. : 1.;
247  while (n!=top){
248  uint i=n->id;
249  assert(M[i].values[0]>0);
250  assert(M[i].values[1]>0);
251  assert(M[i].values[2]>0);
252 
253  Pose delta( beta[0]/(M[i].values[0]*tw[0]), beta[1]/(M[i].values[1]*tw[1]), beta[2]/(M[i].values[2]*tw[2]));
254  delta=delta*sign;
255  //DEBUG(2) << " " << dir << ":" << i <<"," << n->parent->id << ":"
256  // << n->parameters.x() << " " << n->parameters.y() << " " << n->parameters.theta() << " -> ";
257 
258  n->parameters.x()+=delta.x();
259  n->parameters.y()+=delta.y();
260  n->parameters.theta()+=delta.theta();
261  //DEBUG(2) << n->parameters.x() << " " << n->parameters.y() << " " << n->parameters.theta()<< endl;
262  n=n->parent;
263  }
264  }
265  updatePoseChain(v1,top);
266  updatePoseChain(v2,top);
267 
268  //Pose pf1=v1->pose;
269  //Pose pf2=v2->pose;
270 
271  //DEBUG(2) << " pf1=" << pf1.x() << " " << pf1.y() << " " << pf1.theta() << endl;
272  //DEBUG(2) << " pf2=" << pf2.x() << " " << pf2.y() << " " << pf2.theta() << endl;
273  //DEBUG(2) << " en=" << p12.x()-pf2.x() << " " << p12.y()-pf2.y() << " " << p12.theta()-pf2.theta() << endl;
274  }
275 
276 }
277 
280  if (eset){
281  sortedEdges=eset;
282  }
283  if (iteration==1)
285  propagateErrors();
286  sortedEdges=temp;
287 }
288 
289 void TreeOptimizer2::updatePoseChain(Vertex* v, Vertex* top){
290  if (v!=top){
291  updatePoseChain(v->parent, top);
292  v->pose.x()=v->parent->pose.x()+v->parameters.x();
293  v->pose.y()=v->parent->pose.y()+v->parameters.y();
294  v->pose.theta()=v->parent->pose.theta()+v->parameters.theta();
295  return;
296  }
297 }
298 
300  Pose p(0,0,0);
301  Vertex* aux=v;
302  while (aux!=top){
303  p.x()+=aux->parameters.x();
304  p.y()+=aux->parameters.y();
305  p.theta()+=aux->parameters.theta();
306  aux=aux->parent;
307  }
308  p.x()+=aux->pose.x();
309  p.y()+=aux->pose.y();
310  p.theta()+=aux->pose.theta();
311  return p;
312 }
313 
314 
315 double TreeOptimizer2::error(const Edge* e) const{
316  const Vertex* v1=e->v1;
317  const Vertex* v2=e->v2;
318 
319  Pose p1=v1->pose;
320  Pose p2=v2->pose;
321 
322  //DEBUG(2) << " p1=" << p1.x() << " " << p1.y() << " " << p1.theta() << endl;
323  //DEBUG(2) << " p2=" << p2.x() << " " << p2.y() << " " << p2.theta() << endl;
324 
325  Transformation et=e->transformation;
326  Transformation t1(p1);
327  Transformation t2(p2);
328 
329  Transformation t12=t1*et;
330 
331  Pose p12=t12.toPoseType();
332  //DEBUG(2) << " pt2=" << p12.x() << " " << p12.y() << " " << p12.theta() << endl;
333 
334  Pose r(p12.x()-p2.x(), p12.y()-p2.y(), p12.theta()-p2.theta());
335  double angle=r.theta();
336  angle=atan2(sin(angle),cos(angle));
337  r.theta()=angle;
338  //DEBUG(2) << " e=" << r.x() << " " << r.y() << " " << r.theta() << endl;
339 
340  InformationMatrix S=e->informationMatrix;
342  R.values[0][0]=t1.rotationMatrix[0][0];
343  R.values[0][1]=t1.rotationMatrix[0][1];
344  R.values[0][2]=0;
345 
346  R.values[1][0]=t1.rotationMatrix[1][0];
347  R.values[1][1]=t1.rotationMatrix[1][1];
348  R.values[1][2]=0;
349 
350  R.values[2][0]=0;
351  R.values[2][1]=0;
352  R.values[2][2]=1;
353 
354  InformationMatrix W=R*S*R.transpose();
355 
356  Pose r1=W*r;
357  return r.x()*r1.x()+r.y()*r1.y()+r.theta()*r1.theta();
358 }
359 
360 double TreeOptimizer2::error() const{
361  double globalError=0.;
362  for (TreePoseGraph2::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
363  globalError+=error(it->second);
364  }
365  return globalError;
366 }
367 
368 }; //namespace AISNavigation
d
unsigned int uint
void iterate(TreePoseGraph2::EdgeSet *eset=0)
Defines the core optimizer class for 2D graphs which is a subclass of TreePoseGraph2.
T rotationMatrix[2][2]
the rotation matrix
A class to represent symmetric 3x3 matrices.
GLM_FUNC_DECL genType e()
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
GLM_FUNC_DECL genType sign(genType const &x)
SMatrix3< T > transpose() const
GLM_FUNC_DECL genType cos(genType const &angle)
GLM_FUNC_DECL genType sin(genType const &angle)
T values[3]
container for x, y, and theta
void updatePoseChain(Vertex *v, Vertex *top)
GLM_FUNC_QUALIFIER T atan2(T x, T y)
Arc tangent. Returns an angle whose tangent is y/x. The signs of x and y are used to determine what q...
unsigned int uint
Definition: posegraph2.cpp:53
const T & theta() const
Pose getPose(Vertex *v, Vertex *top)
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
const T & y() const
Operations2D< double >::PoseType Pose
Definition: posegraph2.hh:59
A class (struct) to compute the parameterization of the vertex v.
void perform(TreePoseGraph2::Vertex *v)
const T & x() const
2D Point (x,y) with orientation (theta)
std::multiset< Edge *, EVComparator< Edge * > > EdgeSet
Definition: posegraph.hh:123
A class to represent 2D transformations (rotation and translation)


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