posegraph2.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 
43 #include "posegraph2.hh"
44 #include <fstream>
45 #include <sstream>
46 #include <string>
47 
48 using namespace std;
49 
50 namespace AISNavigation {
51 
52 
53 typedef unsigned int uint;
54 #define LINESIZE 81920
55 
56 
57 //#define DEBUG(i) if (verboseLevel>i) cerr
58 
59 
60 bool TreePoseGraph2::load(const char* filename, bool overrideCovariances){
61  clear();
62  ifstream is(filename);
63  if (!is)
64  return false;
65 
66  while(is){
67  char buf[LINESIZE];
68  is.getline(buf,LINESIZE);
69  istringstream ls(buf);
70  string tag;
71  ls >> tag;
72 
73  if (tag=="VERTEX" || tag=="VERTEX2"){
74  int id;
75  Pose p;
76  ls >> id >> p.x() >> p.y() >> p.theta();
77  addVertex(id,p);
78  //DEBUG(2) << "V " << id << endl;
79 
80  }
81 
82  if (tag=="EDGE" || tag=="EDGE2"){
83  int id1, id2;
84  Pose p;
86  ls >> id1 >> id2 >> p.x() >> p.y() >> p.theta();
87  if (overrideCovariances){
88  m.values[0][0]=1; m.values[1][1]=1; m.values[2][2]=1;
89  m.values[0][1]=0; m.values[0][2]=0; m.values[1][2]=0;
90  } else {
91  ls >> m.values[0][0] >> m.values[0][1] >> m.values [1][1]
92  >> m.values[2][2] >> m.values[0][2] >> m.values [1][2];
93  }
94  m.values[1][0]=m.values[0][1];
95  m.values[2][0]=m.values[0][2];
96  m.values[2][1]=m.values[1][2];
97  TreePoseGraph2::Vertex* v1=vertex(id1);
98  TreePoseGraph2::Vertex* v2=vertex(id2);
99  Transformation t(p);
100  addEdge(v1, v2,t ,m);
101  //DEBUG(2) << "E " << id1 << " " << id2 << endl;
102  }
103  }
104  return true;
105 }
106 
107 bool TreePoseGraph2::loadEquivalences(const char* filename){
108  ifstream is(filename);
109  if (!is)
110  return false;
111  EdgeList suppressed;
112  uint equivCount=0;
113  while (is){
114  char buf[LINESIZE];
115  is.getline(buf, LINESIZE);
116  istringstream ls(buf);
117  string tag;
118  ls >> tag;
119  if (tag=="EQUIV"){
120  int id1, id2;
121  ls >> id1 >> id2;
122  Edge* e=edge(id1,id2);
123  if (!e)
124  e=edge(id2,id1);
125  if (e){
126  suppressed.push_back(e);
127  equivCount++;
128  }
129  }
130  }
131  for (EdgeList::iterator it=suppressed.begin(); it!=suppressed.end(); it++){
132  Edge* e=*it;
133  if (e->v1->id > e->v2->id)
134  revertEdge(e);
135  collapseEdge(e);
136  }
137  for (TreePoseGraph2::VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
138  Vertex* v=it->second;
139  v->edges.clear();
140  }
141  for (TreePoseGraph2::EdgeMap::iterator it=edges.begin(); it!=edges.end(); it++){
142  TreePoseGraph2::Edge * e=it->second;
143  e->v1->edges.push_back(e);
144  e->v2->edges.push_back(e);
145  }
146  return true;
147 }
148 
149 bool TreePoseGraph2::saveGnuplot(const char* filename){
150  ofstream os(filename);
151  if (!os)
152  return false;
153 
154  for (TreePoseGraph2::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
155  const TreePoseGraph2::Edge * e=it->second;
156  const Vertex* v1=e->v1;
157  const Vertex* v2=e->v2;
158 
159  os << v1->pose.x() << " " << v1->pose.y() << " " << v1->pose.theta() << endl;
160  os << v2->pose.x() << " " << v2->pose.y() << " " << v2->pose.theta() << endl;
161  os << endl;
162  }
163  return true;
164 
165 }
166 
167 bool TreePoseGraph2::save(const char* filename){
168  ofstream os(filename);
169  if (!os)
170  return false;
171 
172  for (TreePoseGraph2::VertexMap::const_iterator it=vertices.begin(); it!=vertices.end(); it++){
173  const TreePoseGraph2::Vertex* v=it->second;
174  os << "VERTEX "
175  << v->id << " "
176  << v->pose.x() << " "
177  << v->pose.y() << " "
178  << v->pose.theta()<< endl;
179  }
180  for (TreePoseGraph2::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
181  const TreePoseGraph2::Edge * e=it->second;
182  os << "EDGE " << e->v1->id << " " << e->v2->id << " ";
183  Pose p=e->transformation.toPoseType();
184  os << p.x() << " " << p.y() << " " << p.theta() << " ";
185  os << e->informationMatrix.values[0][0] << " "
186  << e->informationMatrix.values[0][1] << " "
187  << e->informationMatrix.values[1][1] << " "
188  << e->informationMatrix.values[2][2] << " "
189  << e->informationMatrix.values[0][2] << " "
190  << e->informationMatrix.values[1][2] << endl;
191  }
192  return true;
193 }
194 
197 struct IdPrinter{
198  IdPrinter(std::ostream& _os):os(_os){}
199  std::ostream& os;
200  void perform(TreePoseGraph2::Vertex* v){
201  std::cout << "(" << v->id << "," << v->level << ")" << endl;
202  }
203 };
204 
205 void TreePoseGraph2::printDepth( std::ostream& os ){
206  IdPrinter ip(os);
207  treeDepthVisit(ip, root);
208 }
209 
210 void TreePoseGraph2::printWidth( std::ostream& os ){
211  IdPrinter ip(os);
212  treeBreadthVisit(ip);
213 }
214 
219  void perform(TreePoseGraph2::Vertex* v){
220  if (!v->parent)
221  return;
222  TreePoseGraph2::Transformation tParent(v->parent->pose);
223  TreePoseGraph2::Transformation tNode=tParent*v->parentEdge->transformation;
224 
225  //cerr << "EDGE(" << v->parentEdge->v1->id << "," << v->parentEdge->v2->id <<"): " << endl;
226  //Pose pParent=v->parent->pose;
227  //cerr << " p=" << pParent.x() << "," << pParent.y() << "," << pParent.theta() << endl;
228  //Pose pEdge=v->parentEdge->transformation.toPoseType();
229  //cerr << " m=" << pEdge.x() << "," << pEdge.y() << "," << pEdge.theta() << endl;
230  //Pose pNode=tNode.toPoseType();
231  //cerr << " n=" << pNode.x() << "," << pNode.y() << "," << pNode.theta() << endl;
232 
233  assert(v->parentEdge->v1==v->parent);
234  assert(v->parentEdge->v2==v);
235  v->pose=tNode.toPoseType();
236  }
237 };
238 
239 void TreePoseGraph2::initializeOnTree(){
240  PosePropagator pp;
241  treeDepthVisit(pp, root);
242 }
243 
244 
245 void TreePoseGraph2::printEdgesStat(std::ostream& os){
246  for (TreePoseGraph2::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
247  const TreePoseGraph2::Edge * e=it->second;
248  os << "EDGE " << e->v1->id << " " << e->v2->id << " ";
249  Pose p=e->transformation.toPoseType();
250  os << p.x() << " " << p.y() << " " << p.theta() << " ";
251  os << e->informationMatrix.values[0][0] << " "
252  << e->informationMatrix.values[0][1] << " "
253  << e->informationMatrix.values[1][1] << " "
254  << e->informationMatrix.values[2][2] << " "
255  << e->informationMatrix.values[0][2] << " "
256  << e->informationMatrix.values[1][2] << endl;
257  os << " top=" << e->top->id << " length=" << e->length << endl;
258  }
259 }
260 
261 void TreePoseGraph2::revertEdgeInfo(Edge* e){
262  Transformation it=e->transformation.inv();
264  R.values[0][0]=e->transformation.rotationMatrix[0][0];
265  R.values[0][1]=e->transformation.rotationMatrix[0][1];
266  R.values[0][2]=0;
267 
268  R.values[1][0]=e->transformation.rotationMatrix[1][0];
269  R.values[1][1]=e->transformation.rotationMatrix[1][1];
270  R.values[1][2]=0;
271 
272  R.values[2][0]=0;
273  R.values[2][1]=0;
274  R.values[2][2]=1;
275 
276  InformationMatrix IM=R.transpose()*e->informationMatrix*R;
277 
278 
279  //Pose np=e->transformation.toPoseType();
280 
281  //Pose ip=it.toPoseType();
282 
283  //Transformation tc=it*e->transformation;
284  //Pose pc=tc.toPoseType();
285 
286  e->transformation=it;
287  e->informationMatrix=IM;
288 };
289 
290 void TreePoseGraph2::initializeFromParentEdge(Vertex* v){
291  Transformation tp=Transformation(v->parent->pose)*v->parentEdge->transformation;
292  v->transformation=tp;
293  v->pose=tp.toPoseType();
294  v->parameters=v->pose;
295  v->parameters.x()-=v->parent->pose.x();
296  v->parameters.y()-=v->parent->pose.y();
297  v->parameters.theta()-=v->parent->pose.theta();
298  v->parameters.theta()=atan2(sin(v->parameters.theta()), cos(v->parameters.theta()));
299 }
300 
301 void TreePoseGraph2::collapseEdge(Edge* e){
302  EdgeMap::iterator ie_it=edges.find(e);
303  if (ie_it==edges.end())
304  return;
305  //VertexMap::iterator it1=vertices.find(e->v1->id);
306  //VertexMap::iterator it2=vertices.find(e->v2->id);
307  assert(vertices.find(e->v1->id)!=vertices.end());
308  assert(vertices.find(e->v2->id)!=vertices.end());
309 
310  Vertex* v1=e->v1;
311  Vertex* v2=e->v2;
312 
313 
314  // all the edges of v2 become outgoing
315  for (EdgeList::iterator it=v2->edges.begin(); it!=v2->edges.end(); it++){
316  if ( (*it)->v1!=v2 )
317  revertEdge(*it);
318  }
319 
320  // all the edges of v1 become outgoing
321  for (EdgeList::iterator it=v1->edges.begin(); it!=v1->edges.end(); it++){
322  if ( (*it)->v1!=v1 )
323  revertEdge(*it);
324  }
325 
326  assert(e->v1==v1);
327 
328  InformationMatrix I12=e->informationMatrix;
329  CovarianceMatrix C12=I12.inv();
330  Transformation T12=e->transformation;
331  //Pose p12=T12.toPoseType();
332 
333  //Transformation iT12=T12.inv();
334 
335  //compute the marginal information of the nodes in the path v1-v2-v*
336  for (EdgeList::iterator it2=v2->edges.begin(); it2!=v2->edges.end(); it2++){
337  Edge* e2=*it2;
338  if (e2->v1==v2){ //edge leaving v2
339  //Transformation T2x=e2->transformation;
340  //Pose p2x=T2x.toPoseType();
341  InformationMatrix I2x=e2->informationMatrix;
342  CovarianceMatrix C2x=I2x.inv();
343 
344  //compute the estimate of the vertex based on the path v1-v2-vx
345 
346  //Transformation tr=iT12*T2x;
347 
348  //InformationMatrix R;
349  //R.values[0][0]=tr.rotationMatrix[0][0];
350  //R.values[0][1]=tr.rotationMatrix[0][1];
351  //R.values[0][2]=0;
352 
353  //R.values[1][0]=tr.rotationMatrix[1][0];
354  //R.values[1][1]=tr.rotationMatrix[1][1];
355  //R.values[1][2]=0;
356 
357  //R.values[2][0]=0;
358  //R.values[2][1]=0;
359  //R.values[2][2]=1;
360 
361  //CovarianceMatrix CM=R.transpose()*C2x*R;
362 
363 
364  Transformation T1x_pred=T12*e2->transformation;
365  Covariance C1x_pred=C12+C2x;
366  InformationMatrix I1x_pred=C1x_pred.inv();
367 
368  e2->transformation=T1x_pred;
369  e2->informationMatrix=I1x_pred;
370  }
371  }
372 
373  //all the edges leaving v1 and leaving v2 and leading to the same point are merged
374  std::list<Transformation> tList;
375  std::list<InformationMatrix> iList;
376  std::list<Vertex*> vList;
377 
378  //others are transformed and added to v1
379  for (EdgeList::iterator it2=v2->edges.begin(); it2!=v2->edges.end(); it2++){
380  Edge* e1x=0;
381  Edge* e2x=0;
382  if ( ((*it2)->v1!=v1)){
383  e2x=*it2;
384  for (EdgeList::iterator it1=v1->edges.begin(); it1!=v1->edges.end(); it1++){
385  if ((*it1)->v2==(*it2)->v2)
386  e1x=*it1;
387  }
388 
389  }
390  if (e1x && e2x){
391  Transformation t1x=e1x->transformation;
392  InformationMatrix I1x=e1x->informationMatrix;
393  Pose p1x=t1x.toPoseType();
394 
395  Transformation t2x=e2x->transformation;
396  InformationMatrix I2x=e2x->informationMatrix;;
397  Pose p2x=t2x.toPoseType();
398 
399  InformationMatrix IM=I1x+I2x;
400  CovarianceMatrix CM=IM.inv();
401  InformationMatrix scale1=CM*I1x;
402  InformationMatrix scale2=CM*I2x;
403 
404 
405  Pose p1=scale1*p1x;
406  Pose p2=scale2*p2x;
407 
408 
409  //need to recover the angles in a decent way.
410  double s=scale1.values[2][2]*sin(p1x.theta())+ scale2.values[2][2]*sin(p2x.theta());
411  double c=scale1.values[2][2]*cos(p1x.theta())+ scale2.values[2][2]*cos(p2x.theta());
412 
413  //DEBUG(2) << "p1x= " << p1x.x() << " " << p1x.y() << " " << p1x.theta() << endl;
414  //DEBUG(2) << "p1x_pred= " << p2x.x() << " " << p2x.y() << " " << p2x.theta() << endl;
415 
416  Pose pFinal(p1.x()+p2.x(), p1.y()+p2.y(), atan2(s,c));
417  //DEBUG(2) << "p1x_final= " << pFinal.x() << " " << pFinal.y() << " " << pFinal.theta() << endl;
418 
419  e1x->transformation=Transformation(pFinal);
420  e1x->informationMatrix=IM;
421  }
422  if (!e1x && e2x){
423  tList.push_back(e2x->transformation);
424  iList.push_back(e2x->informationMatrix);
425  vList.push_back(e2x->v2);
426  }
427  }
428  removeVertex(v2->id);
429 
430  std::list<Transformation>::iterator t=tList.begin();
431  std::list<InformationMatrix>::iterator i=iList.begin();
432  std::list<Vertex*>::iterator v=vList.begin();
433  while (i!=iList.end()){
434  addEdge(v1,*v,*t,*i);
435  i++;
436  t++;
437  v++;
438  }
439 }
440 
441 }; //namespace AISNavigation
root
Transformation2< T > inv() const
A class to represent symmetric 3x3 matrices.
GLM_FUNC_DECL genType e()
static const float vertices[]
Definition: quad.cpp:39
Defines the graph of 2D poses, with specific functionalities such as loading, saving, merging constraints, and etc.
SMatrix3< T > transpose() const
GLM_FUNC_DECL genType cos(genType const &angle)
GLM_FUNC_DECL genType sin(genType const &angle)
SMatrix3< T > inv() const
#define LINESIZE
Definition: posegraph2.cpp:54
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
const T & y() const
void perform(TreePoseGraph2::Vertex *v)
Definition: posegraph2.cpp:219
void perform(TreePoseGraph2::Vertex *v)
Definition: posegraph2.cpp:200
IdPrinter(std::ostream &_os)
Definition: posegraph2.cpp:198
const T & x() const
2D Point (x,y) with orientation (theta)
A class (struct) for realizing the pose update of the individual nodes. Assumes the correct order of ...
Definition: posegraph2.cpp:218
A class to represent 2D transformations (rotation and translation)
A class (struct) used to print vertex information to a stream. Needed for debugging.
Definition: posegraph2.cpp:197


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:59