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


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