posegraph3.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  * * Any of the above conditions can be waived if you get permission
27  * from the copyright holder. Nothing in this license impairs or
28  * restricts the author's moral rights.
29  *
30  * TORO is distributed in the hope that it will be useful,
31  * but WITHOUT ANY WARRANTY; without even the implied
32  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
33  * PURPOSE.
34  **********************************************************************/
35 
42 #include "posegraph3.hh"
43 #include <fstream>
44 #include <sstream>
45 #include <string>
46 
47 using namespace std;
48 
49 namespace AISNavigation {
50 
51 #define LINESIZE 81920
52 
53 
54 //#define DEBUG(i) if (verboseLevel>i) cerr
55 
56 
57 bool TreePoseGraph3::load(const char* filename, bool overrideCovariances, bool twoDimensions){
58  clear();
59  ifstream is(filename);
60  if (!is)
61  return false;
62 
63  while(is){
64  char buf[LINESIZE];
65  is.getline(buf,LINESIZE);
66  istringstream ls(buf);
67  string tag;
68  ls >> tag;
69 
70  if (twoDimensions){
71  if (tag=="VERTEX"){
72  int id;
73  Pose p(0.,0.,0.,0.,0.,0.);
74  ls >> id >> p.x() >> p.y() >> p.yaw();
75  TreePoseGraph3::Vertex* v=addVertex(id,p);
76  if (v){
77  v->transformation=Transformation(p);
78  }
79  }
80  } else {
81  if (tag=="VERTEX3"){
82  int id;
83  Pose p;
84  ls >> id >> p.x() >> p.y() >> p.z() >> p.roll() >> p.pitch() >> p.yaw();
85  TreePoseGraph3::Vertex* v=addVertex(id,p);
86  if (v){
87  v->transformation=Transformation(p);
88  }
89  }
90  }
91  }
92  is.clear(); /* clears the end-of-file and error flags */
93  is.seekg(0, ios::beg);
94 
95  //bool edgesOk=true;
96  while(is){
97  char buf[LINESIZE];
98  is.getline(buf,LINESIZE);
99  istringstream ls(buf);
100  string tag;
101  ls >> tag;
102 
103  if (twoDimensions){
104  if (tag=="EDGE"){
105  int id1, id2;
106  Pose p(0.,0.,0.,0.,0.,0.);
108  ls >> id1 >> id2 >> p.x() >> p.y() >> p.yaw();
109  m=DMatrix<double>::I(6);
110  if (! overrideCovariances){
111  ls >> m[0][0] >> m[0][1] >> m[1][1] >> m[2][2] >> m[0][2] >> m[1][2];
112  m[2][0]=m[0][2]; m[2][1]=m[1][2]; m[1][0]=m[0][1];
113  }
114 
115  TreePoseGraph3::Vertex* v1=vertex(id1);
116  TreePoseGraph3::Vertex* v2=vertex(id2);
117  Transformation t(p);
118  if (!addEdge(v1, v2,t ,m)){
119  cerr << "Fatal, attempting to insert an edge between non existing nodes, skipping";
120  cerr << "edge=" << id1 <<" -> " << id2 << endl;
121  //edgesOk=false;
122  }
123  }
124  } else {
125  if (tag=="EDGE3"){
126  int id1, id2;
127  Pose p;
129  ls >> id1 >> id2 >> p.x() >> p.y() >> p.z() >> p.roll() >> p.pitch() >> p.yaw();
130  m=DMatrix<double>::I(6);
131  if (! overrideCovariances){
132  for (int i=0; i<6; i++)
133  for (int j=i; j<6; j++)
134  ls >> m[i][j];
135  }
136  TreePoseGraph3::Vertex* v1=vertex(id1);
137  TreePoseGraph3::Vertex* v2=vertex(id2);
138  Transformation t(p);
139  if (!addEdge(v1, v2,t ,m)){
140  cerr << "Fatal, attempting to insert an edge between non existing nodes, skipping";
141  cerr << "edge=" << id1 <<" -> " << id2 << endl;
142  //edgesOk=false;
143  }
144  }
145  }
146  }
147  return true;
148  //return edgesOk;
149 }
150 
151 bool TreePoseGraph3::loadEquivalences(const char* filename){
152  ifstream is(filename);
153  if (!is)
154  return false;
155  EdgeList suppressed;
156  uint equivCount=0;
157  while (is){
158  char buf[LINESIZE];
159  is.getline(buf, LINESIZE);
160  istringstream ls(buf);
161  string tag;
162  ls >> tag;
163  if (tag=="EQUIV"){
164  int id1, id2;
165  ls >> id1 >> id2;
166  Edge* e=edge(id1,id2);
167  if (!e)
168  e=edge(id2,id1);
169  if (e){
170  suppressed.push_back(e);
171  equivCount++;
172  }
173  }
174  }
175  for (EdgeList::iterator it=suppressed.begin(); it!=suppressed.end(); it++){
176  Edge* e=*it;
177  if (e->v1->id > e->v2->id)
178  revertEdge(e);
179  collapseEdge(e);
180  }
181  return true;
182 }
183 
184 bool TreePoseGraph3::saveGnuplot(const char* filename){
185  ofstream os(filename);
186  if (!os)
187  return false;
188  for (TreePoseGraph3::VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
189  TreePoseGraph3::Vertex* v=it->second;
190  v->pose=v->transformation.toPoseType();
191  }
192  for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
193  const TreePoseGraph3::Edge * e=it->second;
194  const Vertex* v1=e->v1;
195  const Vertex* v2=e->v2;
196 
197  os << v1->pose.x() << " " << v1->pose.y() << " " << v1->pose.z() << " "
198  << v1->pose.roll() << " " << v1->pose.pitch() << " " << v1->pose.yaw() <<endl;
199  os << v2->pose.x() << " " << v2->pose.y() << " " << v2->pose.z() << " "
200  << v2->pose.roll() << " " << v2->pose.pitch() << " " << v2->pose.yaw() <<endl;
201  os << endl << endl;
202  }
203  return true;
204 
205 }
206 
207 bool TreePoseGraph3::save(const char* filename){
208  ofstream os(filename);
209  if (!os)
210  return false;
211 
212  for (TreePoseGraph3::VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
213  TreePoseGraph3::Vertex* v=it->second;
214  v->pose=v->transformation.toPoseType();
215 
216  os << "VERTEX3 "
217  << v->id << " "
218  << v->pose.x() << " "
219  << v->pose.y() << " "
220  << v->pose.z() << " "
221  << v->pose.roll() << " "
222  << v->pose.pitch() << " "
223  << v->pose.yaw() << endl;
224  }
225  for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
226  const TreePoseGraph3::Edge * e=it->second;
227  os << "EDGE3 " << e->v1->id << " " << e->v2->id << " ";
228  Pose p=e->transformation.toPoseType();
229  os << p.x() << " " << p.y() << " " << p.z() << " " << p.roll() << " " << p.pitch() << " " << p.yaw() << " ";
230  for (int i=0; i<6; i++)
231  for (int j=i; j<6; j++)
232  os << e->informationMatrix[i][j] << " ";
233  os << endl;
234  }
235  return true;
236 }
237 
240 struct IdPrinter{
241  IdPrinter(std::ostream& _os):os(_os){}
242  std::ostream& os;
243  void perform(TreePoseGraph3::Vertex* v){
244  std::cout << "(" << v->id << "," << v->level << ")" << endl;
245  }
246 };
247 
248 void TreePoseGraph3::printDepth( std::ostream& os ){
249  IdPrinter ip(os);
250  treeDepthVisit(ip, root);
251 }
252 
253 void TreePoseGraph3::printWidth( std::ostream& os ){
254  IdPrinter ip(os);
255  treeBreadthVisit(ip);
256 }
257 
262 struct PosePropagator{
263  void perform(TreePoseGraph3::Vertex* v){
264  if (!v->parent)
265  return;
266  TreePoseGraph3::Transformation tParent(v->parent->transformation);
267  TreePoseGraph3::Transformation tNode=tParent*v->parentEdge->transformation;
268 
269  assert(v->parentEdge->v1==v->parent);
270  assert(v->parentEdge->v2==v);
271  v->transformation=tNode;
272  }
273 };
274 
275 void TreePoseGraph3::initializeOnTree(){
276  PosePropagator pp;
277  treeDepthVisit(pp, root);
278 }
279 
280 
281 void TreePoseGraph3::printEdgesStat(std::ostream& os){
282  for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
283  const TreePoseGraph3::Edge * e=it->second;
284  os << "EDGE " << e->v1->id << " " << e->v2->id << " ";
285  Pose p=e->transformation.toPoseType();
286  os << p.x() << " " << p.y() << " " << p.z() << " " << p.roll() << " " << p.pitch() << " " << p.yaw() << endl;
287  os << " top=" << e->top->id << " length=" << e->length << endl;
288  }
289 }
290 
291 void TreePoseGraph3::revertEdgeInfo(Edge* e){
292  // here we assume uniform covariances, and we neglect the transofrmation
293  // induced by the Jacobian when reverting the link
294  e->transformation=e->transformation.inv();
295 };
296 
297 void TreePoseGraph3::initializeFromParentEdge(Vertex* v){
298  Transformation tp=Transformation(v->parent->pose)*v->parentEdge->transformation;
299  v->transformation=tp;
300  v->pose=tp.toPoseType();
301  v->parameters=v->parentEdge->transformation;
302 }
303 
304 
305 void TreePoseGraph3::collapseEdge(Edge* e){
306  Vertex* v1=e->v1;
307  Vertex* v2=e->v2;
308 
309  // all the edges of v2 become outgoing
310  for (EdgeList::iterator it=v2->edges.begin(); it!=v2->edges.end(); it++){
311  if ( (*it)->v1!=v2 )
312  revertEdge(*it);
313  }
314 
315  // all the edges of v1 become outgoing
316  for (EdgeList::iterator it=v1->edges.begin(); it!=v1->edges.end(); it++){
317  if ( (*it)->v1!=v1 )
318  revertEdge(*it);
319  }
320 
321  assert(e->v1==v1);
322 
323  InformationMatrix I12=e->informationMatrix;
324  CovarianceMatrix C12=I12.inv();
325  Transformation T12=e->transformation;
326  Pose p12=T12.toPoseType();
327 
328  //Transformation iT12=T12.inv();
329 
330  //compute the marginal information of the nodes in the path v1-v2-v*
331  for (EdgeList::iterator it2=v2->edges.begin(); it2!=v2->edges.end(); it2++){
332  Edge* e2=*it2;
333  if (e2->v1==v2){ //edge leaving v2
334  Transformation T2x=e2->transformation;
335  Pose p2x=T2x.toPoseType();
336  InformationMatrix I2x=e2->informationMatrix;
337  CovarianceMatrix C2x=I2x.inv();
338 
339  //compute the estimate of the vertex based on the path v1-v2-vx
340 
341  //Transformation tr=iT12*T2x;
342 
343  CovarianceMatrix CM=C2x;
344 
345 
346  Transformation T1x_pred=T12*e2->transformation;
347  Covariance C1x_pred=C12+C2x;
348  InformationMatrix I1x_pred=C1x_pred.inv();
349 
350  e2->transformation=T1x_pred;
351  e2->informationMatrix=I1x_pred;
352  }
353  }
354 
355  //all the edges leaving v1 and leaving v2 and leading to the same point are merged
356  std::list<Transformation> tList;
357  std::list<InformationMatrix> iList;
358  std::list<Vertex*> vList;
359 
360  //others are transformed and added to v1
361  for (EdgeList::iterator it2=v2->edges.begin(); it2!=v2->edges.end(); it2++){
362  Edge* e1x=0;
363  Edge* e2x=0;
364  if ( ((*it2)->v1!=v1)){
365  e2x=*it2;
366  for (EdgeList::iterator it1=v1->edges.begin(); it1!=v1->edges.end(); it1++){
367  if ((*it1)->v2==(*it2)->v2)
368  e1x=*it1;
369  }
370 
371  }
372  // FIXME
373  // edges leading to the same node are ignored
374  // should be merged
375  if (e1x && e2x){
376  // here goes something for mergin the constraints, according to the information matrices.
377  // in 3D it is a nightmare, so i postpone this, and i simply ignore the redundant constraints.
378  // the resultng system is overconfident
379  }
380  if (!e1x && e2x){
381  tList.push_back(e2x->transformation);
382  iList.push_back(e2x->informationMatrix);
383  vList.push_back(e2x->v2);
384  }
385  }
386  removeVertex(v2->id);
387 
388  std::list<Transformation>::iterator t=tList.begin();
389  std::list<InformationMatrix>::iterator i=iList.begin();
390  std::list<Vertex*>::iterator v=vList.begin();
391  while (i!=iList.end()){
392  addEdge(v1,*v,*t,*i);
393  i++;
394  t++;
395  v++;
396  }
397 }
398 
399 void TreePoseGraph3::recomputeAllTransformations(){
401  treeDepthVisit(tp,root);
402 }
403 
404 }; //namespace AISNavigation
root
const T & pitch() const
Defines the graph of 3D poses, with specific functionalities such as loading, saving, merging constraints, and etc.
A class (struct) to compute the parameterization of the vertex v.
Definition: posegraph3.hh:129
void perform(TreePoseGraph3::Vertex *v)
Definition: posegraph3.cpp:263
GLM_FUNC_DECL genType e()
static const float vertices[]
Definition: quad.cpp:39
#define LINESIZE
Definition: posegraph3.cpp:51
const T & roll() const
const T & yaw() const
Pose3< T > toPoseType() const
void perform(TreePoseGraph3::Vertex *v)
Definition: posegraph3.cpp:243
const T & x() const
const T & z() const
const T & y() const
unsigned int uint
Definition: posegraph2.cpp:53
DMatrix inv() const
IdPrinter(std::ostream &_os)
Definition: posegraph3.cpp:241
static DMatrix I(int)
A class (struct) for realizing the pose update of the individual nodes. Assumes the correct order of ...
Definition: posegraph2.cpp:218
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