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


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:22