Main MRPT website > C++ reference for MRPT 1.3.2
CNetworkOfPoses_impl.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2015, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 #ifndef CONSTRAINED_POSE_NETWORK_IMPL_H
10 #define CONSTRAINED_POSE_NETWORK_IMPL_H
11 
12 #include <mrpt/graphs/dijkstra.h>
16 #include <mrpt/math/wrap2pi.h>
17 #include <mrpt/math/ops_matrices.h> // multiply_*()
20 #include <mrpt/poses/CPose2D.h>
21 #include <mrpt/poses/CPose3D.h>
22 #include <mrpt/poses/CPose3DQuat.h>
27 
28 namespace mrpt
29 {
30  namespace graphs
31  {
32  namespace detail
33  {
34  using namespace std;
35  using namespace mrpt;
36  using namespace mrpt::utils;
37  using namespace mrpt::poses;
38  using namespace mrpt::graphs;
39 
40  template <class POSE_PDF> struct TPosePDFHelper
41  {
42  static inline void copyFrom2D(POSE_PDF &p, const CPosePDFGaussianInf &pdf ) { p.copyFrom( pdf ); }
43  static inline void copyFrom3D(POSE_PDF &p, const CPose3DPDFGaussianInf &pdf ) { p.copyFrom( pdf ); }
44  };
45  template <> struct TPosePDFHelper<CPose2D>
46  {
47  static inline void copyFrom2D(CPose2D &p, const CPosePDFGaussianInf &pdf ) { p = pdf.mean; }
48  static inline void copyFrom3D(CPose2D &p, const CPose3DPDFGaussianInf &pdf ) { p = CPose2D(pdf.mean); }
49  };
50  template <> struct TPosePDFHelper<CPose3D>
51  {
52  static inline void copyFrom2D(CPose3D &p, const CPosePDFGaussianInf &pdf ) { p = pdf.mean; }
53  static inline void copyFrom3D(CPose3D &p, const CPose3DPDFGaussianInf &pdf ) { p = pdf.mean; }
54  };
55 
56  /// a helper struct with static template functions \sa CNetworkOfPoses
57  template <class graph_t>
58  struct graph_ops
59  {
60  static void write_VERTEX_line(const TNodeID id, const mrpt::poses::CPose2D &p, std::ofstream &f)
61  {
62  // VERTEX2 id x y phi
63  f << "VERTEX2 " << id << " " << p.x() << " " << p.y() << " " << p.phi() << endl;
64  }
65  static void write_VERTEX_line(const TNodeID id, const mrpt::poses::CPose3D &p, std::ofstream &f)
66  {
67  // VERTEX3 id x y z roll pitch yaw
68  // **CAUTION** In the TORO graph format angles are in the RPY order vs. MRPT's YPR.
69  f << "VERTEX3 " << id << " " << p.x() << " " << p.y() << " " << p.z()<< " " << p.roll()<< " " << p.pitch()<< " " << p.yaw() << endl;
70  }
71 
72 
73  static void write_EDGE_line( const TPairNodeIDs &edgeIDs,const CPosePDFGaussianInf & edge, std::ofstream &f)
74  {
75  // EDGE2 from_id to_id Ax Ay Aphi inf_xx inf_xy inf_yy inf_pp inf_xp inf_yp
76  // **CAUTION** TORO docs say "from_id" "to_id" in the opposite order, but it seems from the data that this is the correct expected format.
77  f << "EDGE2 " << edgeIDs.first << " " << edgeIDs.second << " " <<
78  edge.mean.x()<<" "<<edge.mean.y()<<" "<<edge.mean.phi()<<" "<<
79  edge.cov_inv(0,0)<<" "<<edge.cov_inv(0,1)<<" "<<edge.cov_inv(1,1)<<" "<<
80  edge.cov_inv(2,2)<<" "<<edge.cov_inv(0,2)<<" "<<edge.cov_inv(1,2) << endl;
81  }
82  static void write_EDGE_line( const TPairNodeIDs &edgeIDs,const CPose3DPDFGaussianInf & edge, std::ofstream &f)
83  {
84  // EDGE3 from_id to_id Ax Ay Az Aroll Apitch Ayaw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
85  // **CAUTION** In the TORO graph format angles are in the RPY order vs. MRPT's YPR.
86  // **CAUTION** TORO docs say "from_id" "to_id" in the opposite order, but it seems from the data that this is the correct expected format.
87  f << "EDGE3 " << edgeIDs.first << " " << edgeIDs.second << " " <<
88  edge.mean.x()<<" "<<edge.mean.y()<<" "<<edge.mean.z()<<" "<<
89  edge.mean.roll()<<" "<<edge.mean.pitch()<<" "<<edge.mean.yaw()<<" "<<
90  edge.cov_inv(0,0)<<" "<<edge.cov_inv(0,1)<<" "<<edge.cov_inv(0,2)<<" "<<edge.cov_inv(0,5)<<" "<<edge.cov_inv(0,4)<<" "<<edge.cov_inv(0,3)<<" "<<
91  edge.cov_inv(1,1)<<" "<<edge.cov_inv(1,2)<<" "<<edge.cov_inv(1,5)<<" "<<edge.cov_inv(1,4)<<" "<<edge.cov_inv(1,3)<<" "<<
92  edge.cov_inv(2,2)<<" "<<edge.cov_inv(2,5)<<" "<<edge.cov_inv(2,4)<<" "<<edge.cov_inv(2,3)<<" "<<
93  edge.cov_inv(5,5)<<" "<<edge.cov_inv(5,4)<<" "<<edge.cov_inv(5,3)<<" "<<
94  edge.cov_inv(4,4)<<" "<<edge.cov_inv(4,3)<<" "<<
95  edge.cov_inv(3,3) << endl;
96  }
97  static void write_EDGE_line( const TPairNodeIDs &edgeIDs,const CPosePDFGaussian & edge, std::ofstream &f)
98  {
100  p.copyFrom(edge);
101  write_EDGE_line(edgeIDs,p,f);
102  }
103  static void write_EDGE_line( const TPairNodeIDs &edgeIDs,const CPose3DPDFGaussian & edge, std::ofstream &f)
104  {
106  p.copyFrom(edge);
107  write_EDGE_line(edgeIDs,p,f);
108  }
109  static void write_EDGE_line( const TPairNodeIDs &edgeIDs,const mrpt::poses::CPose2D & edge, std::ofstream &f)
110  {
112  p.mean = edge;
113  p.cov_inv.unit(3,1.0);
114  write_EDGE_line(edgeIDs,p,f);
115  }
116  static void write_EDGE_line( const TPairNodeIDs &edgeIDs,const mrpt::poses::CPose3D & edge, std::ofstream &f)
117  {
119  p.mean = edge;
120  p.cov_inv.unit(6,1.0);
121  write_EDGE_line(edgeIDs,p,f);
122  }
123 
124 
125  // =================================================================
126  // save_graph_of_poses_to_text_file
127  // =================================================================
128  static void save_graph_of_poses_to_text_file(const graph_t *g, const std::string &fil)
129  {
130  std::ofstream f;
131  f.open(fil.c_str());
132  if (!f.is_open())
133  THROW_EXCEPTION_CUSTOM_MSG1("Error opening file '%s' for writing",fil.c_str());
134 
135  // 1st: Nodes
136  for (typename graph_t::global_poses_t::const_iterator itNod = g->nodes.begin();itNod!=g->nodes.end();++itNod)
137  write_VERTEX_line(itNod->first, itNod->second, f);
138 
139  // 2nd: Edges:
140  for (typename graph_t::const_iterator it=g->begin();it!=g->end();++it)
141  if (it->first.first!=it->first.second) // Ignore self-edges, typically from importing files with EQUIV's
142  write_EDGE_line( it->first, it->second, f);
143 
144  } // end save_graph
145 
146  // =================================================================
147  // save_graph_of_poses_to_binary_file
148  // =================================================================
149  static void save_graph_of_poses_to_binary_file(const graph_t *g, mrpt::utils::CStream &out)
150  {
151  // Store class name:
152  const std::string sClassName = TTypeName<graph_t>::get();
153  out << sClassName;
154 
155  // Store serialization version & object data:
156  const uint32_t version = 0;
157  out << version;
158  out << g->nodes << g->edges << g->root;
159  }
160 
161  // =================================================================
162  // read_graph_of_poses_from_binary_file
163  // =================================================================
165  {
166  // Compare class name:
167  const std::string sClassName = TTypeName<graph_t>::get();
168  std::string sStoredClassName;
169  in >> sStoredClassName;
170  ASSERT_EQUAL_(sStoredClassName,sClassName)
171 
172  // Check serialization version:
173  uint32_t stored_version;
174  in >> stored_version;
175 
176  g->clear();
177  switch (stored_version)
178  {
179  case 0:
180  in >> g->nodes >> g->edges >> g->root;
181  break;
182  default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(stored_version)
183  }
184 
185  }
186 
187  // =================================================================
188  // load_graph_of_poses_from_text_file
189  // =================================================================
190  static void load_graph_of_poses_from_text_file(graph_t *g, const std::string &fil)
191  {
192  using mrpt::system::strCmpI;
193  using namespace mrpt::math;
194 
195  typedef typename graph_t::constraint_t CPOSE;
196 
197  set<string> alreadyWarnedUnknowns; // for unknown line types, show a warning to cerr just once.
198 
199  // First, empty the graph:
200  g->clear();
201 
202  // Determine if it's a 2D or 3D graph, just to raise an error if loading a 3D graph in a 2D one, since
203  // it would be an unintentional loss of information:
204  const bool graph_is_3D = CPOSE::is_3D();
205 
206  CTextFileLinesParser filParser(fil); // raises an exception on error
207 
208  // -------------------------------------------
209  // 1st PASS: Read EQUIV entries only
210  // since processing them AFTER loading the data
211  // is much much slower.
212  // -------------------------------------------
213  map<TNodeID,TNodeID> lstEquivs; // List of EQUIV entries: NODEID -> NEWNODEID. NEWNODEID will be always the lowest ID number.
214 
215  // Read & process lines each at once until EOF:
216  istringstream s;
217  while (filParser.getNextLine(s))
218  {
219  const unsigned int lineNum = filParser.getCurrentLineNumber();
220  const string lin = s.str();
221 
222  string key;
223  if ( !(s >> key) || key.empty() )
224  THROW_EXCEPTION(format("Line %u: Can't read string for entry type in: '%s'", lineNum, lin.c_str() ) );
225 
226  if ( mrpt::system::strCmpI(key,"EQUIV") )
227  {
228  // Process these ones at the end, for now store in a list:
229  TNodeID id1,id2;
230  if (!(s>> id1 >> id2))
231  THROW_EXCEPTION(format("Line %u: Can't read id1 & id2 in EQUIV line: '%s'", lineNum, lin.c_str() ) );
232  lstEquivs[std::max(id1,id2)] = std::min(id1,id2);
233  }
234  } // end 1st pass
235 
236  // -------------------------------------------
237  // 2nd PASS: Read all other entries
238  // -------------------------------------------
239  filParser.rewind();
240 
241  // Read & process lines each at once until EOF:
242  while (filParser.getNextLine(s))
243  {
244  const unsigned int lineNum = filParser.getCurrentLineNumber();
245  const string lin = s.str();
246 
247  // Recognized strings:
248  // VERTEX2 id x y phi
249  // =(VERTEX_SE2)
250  // EDGE2 from_id to_id Ax Ay Aphi inf_xx inf_xy inf_yy inf_pp inf_xp inf_yp
251  // =(EDGE or EDGE_SE2 or ODOMETRY)
252  // VERTEX3 id x y z roll pitch yaw
253  // VERTEX_SE3:QUAT id x y z qx qy qz qw
254  // EDGE3 from_id to_id Ax Ay Az Aroll Apitch Ayaw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
255  // EDGE_SE3:QUAT from_id to_id Ax Ay Az qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
256  // EQUIV id1 id2
257  string key;
258  if ( !(s >> key) || key.empty() )
259  THROW_EXCEPTION(format("Line %u: Can't read string for entry type in: '%s'", lineNum, lin.c_str() ) );
260 
261  if ( strCmpI(key,"VERTEX2") || strCmpI(key,"VERTEX") || strCmpI(key,"VERTEX_SE2") )
262  {
263  TNodeID id;
264  TPose2D p2D;
265  if (!(s>> id >> p2D.x >> p2D.y >> p2D.phi))
266  THROW_EXCEPTION(format("Line %u: Error parsing VERTEX2 line: '%s'", lineNum, lin.c_str() ) );
267 
268  // Make sure the node is new:
269  if (g->nodes.find(id)!=g->nodes.end())
270  THROW_EXCEPTION(format("Line %u: Error, duplicated verted ID %u in line: '%s'", lineNum, static_cast<unsigned int>(id), lin.c_str() ) );
271 
272  // EQUIV? Replace ID by new one.
273  {
274  const map<TNodeID,TNodeID>::const_iterator itEq = lstEquivs.find(id);
275  if (itEq!=lstEquivs.end()) id = itEq->second;
276  }
277 
278  // Add to map: ID -> absolute pose:
279  if (g->nodes.find(id)==g->nodes.end())
280  {
281  typename CNetworkOfPoses<CPOSE>::constraint_t::type_value & newNode = g->nodes[id];
282  newNode = CPose2D(p2D); // Auto converted to mrpt::poses::CPose3D if needed
283  }
284  }
285  else if ( strCmpI(key,"VERTEX3") )
286  {
287  if (!graph_is_3D)
288  THROW_EXCEPTION(format("Line %u: Try to load VERTEX3 into a 2D graph: '%s'", lineNum, lin.c_str() ) );
289 
290  // VERTEX3 id x y z roll pitch yaw
291  TNodeID id;
292  TPose3D p3D;
293  // **CAUTION** In the TORO graph format angles are in the RPY order vs. MRPT's YPR.
294  if (!(s>> id >> p3D.x >> p3D.y >> p3D.z >> p3D.roll >> p3D.pitch >> p3D.yaw ))
295  THROW_EXCEPTION(format("Line %u: Error parsing VERTEX3 line: '%s'", lineNum, lin.c_str() ) );
296 
297  // Make sure the node is new:
298  if (g->nodes.find(id)!=g->nodes.end())
299  THROW_EXCEPTION(format("Line %u: Error, duplicated verted ID %u in line: '%s'", lineNum, static_cast<unsigned int>(id), lin.c_str() ) );
300 
301  // EQUIV? Replace ID by new one.
302  {
303  const map<TNodeID,TNodeID>::const_iterator itEq = lstEquivs.find(id);
304  if (itEq!=lstEquivs.end()) id = itEq->second;
305  }
306 
307  // Add to map: ID -> absolute pose:
308  if (g->nodes.find(id)==g->nodes.end())
309  {
310  g->nodes[id] = typename CNetworkOfPoses<CPOSE>::constraint_t::type_value( CPose3D(p3D) ); // Auto converted to CPose2D if needed
311  }
312  }
313  else if ( strCmpI(key,"VERTEX_SE3:QUAT") )
314  {
315  if (!graph_is_3D)
316  THROW_EXCEPTION(format("Line %u: Try to load VERTEX_SE3:QUAT into a 2D graph: '%s'", lineNum, lin.c_str() ) );
317 
318  // VERTEX_SE3:QUAT id x y z qx qy qz qw
319  TNodeID id;
320  TPose3DQuat p3D;
321  if (!(s>> id >> p3D.x >> p3D.y >> p3D.z >> p3D.qx >> p3D.qy >> p3D.qz >> p3D.qr ))
322  THROW_EXCEPTION(format("Line %u: Error parsing VERTEX_SE3:QUAT line: '%s'", lineNum, lin.c_str() ) );
323 
324  // Make sure the node is new:
325  if (g->nodes.find(id)!=g->nodes.end())
326  THROW_EXCEPTION(format("Line %u: Error, duplicated verted ID %u in line: '%s'", lineNum, static_cast<unsigned int>(id), lin.c_str() ) );
327 
328  // EQUIV? Replace ID by new one.
329  {
330  const map<TNodeID,TNodeID>::const_iterator itEq = lstEquivs.find(id);
331  if (itEq!=lstEquivs.end()) id = itEq->second;
332  }
333 
334  // Add to map: ID -> absolute pose:
335  if (g->nodes.find(id)==g->nodes.end())
336  {
337  g->nodes[id] = typename CNetworkOfPoses<CPOSE>::constraint_t::type_value( CPose3D(CPose3DQuat(p3D)) ); // Auto converted to CPose2D if needed
338  }
339  }
340  else if ( strCmpI(key,"EDGE2") || strCmpI(key,"EDGE") || strCmpI(key,"ODOMETRY") || strCmpI(key,"EDGE_SE2") )
341  {
342  // EDGE2 from_id to_id Ax Ay Aphi inf_xx inf_xy inf_yy inf_pp inf_xp inf_yp
343  // s00 s01 s11 s22 s02 s12
344  // Read values are:
345  // [ s00 s01 s02 ]
346  // [ - s11 s12 ]
347  // [ - - s22 ]
348  //
349  TNodeID to_id, from_id;
350  if (!(s>> from_id >> to_id ))
351  THROW_EXCEPTION(format("Line %u: Error parsing EDGE2 line: '%s'", lineNum, lin.c_str() ) );
352 
353  // EQUIV? Replace ID by new one.
354  {
355  const map<TNodeID,TNodeID>::const_iterator itEq = lstEquivs.find(to_id);
356  if (itEq!=lstEquivs.end()) to_id = itEq->second;
357  }
358  {
359  const map<TNodeID,TNodeID>::const_iterator itEq = lstEquivs.find(from_id);
360  if (itEq!=lstEquivs.end()) from_id = itEq->second;
361  }
362 
363  if (from_id!=to_id) // Don't load self-edges! (probably come from an EQUIV)
364  {
365  TPose2D Ap_mean;
366  mrpt::math::CMatrixDouble33 Ap_cov_inv;
367  if (!(s>>
368  Ap_mean.x >> Ap_mean.y >> Ap_mean.phi >>
369  Ap_cov_inv(0,0) >> Ap_cov_inv(0,1) >> Ap_cov_inv(1,1) >>
370  Ap_cov_inv(2,2) >> Ap_cov_inv(0,2) >> Ap_cov_inv(1,2) ))
371  THROW_EXCEPTION(format("Line %u: Error parsing EDGE2 line: '%s'", lineNum, lin.c_str() ) );
372 
373  // Complete low triangular part of inf matrix:
374  Ap_cov_inv(1,0) = Ap_cov_inv(0,1);
375  Ap_cov_inv(2,0) = Ap_cov_inv(0,2);
376  Ap_cov_inv(2,1) = Ap_cov_inv(1,2);
377 
378  // Convert to 2D cov, 3D cov or 3D inv_cov as needed:
379  typename CNetworkOfPoses<CPOSE>::edge_t newEdge;
380  TPosePDFHelper<CPOSE>::copyFrom2D(newEdge, CPosePDFGaussianInf( CPose2D(Ap_mean), Ap_cov_inv ) );
381  g->insertEdge(from_id, to_id, newEdge);
382  }
383  }
384  else if ( strCmpI(key,"EDGE3") )
385  {
386  if (!graph_is_3D)
387  THROW_EXCEPTION(format("Line %u: Try to load EDGE3 into a 2D graph: '%s'", lineNum, lin.c_str() ) );
388 
389  // EDGE3 from_id to_id Ax Ay Az Aroll Apitch Ayaw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
390  TNodeID to_id, from_id;
391  if (!(s>> from_id >> to_id ))
392  THROW_EXCEPTION(format("Line %u: Error parsing EDGE3 line: '%s'", lineNum, lin.c_str() ) );
393 
394  // EQUIV? Replace ID by new one.
395  {
396  const map<TNodeID,TNodeID>::const_iterator itEq = lstEquivs.find(to_id);
397  if (itEq!=lstEquivs.end()) to_id = itEq->second;
398  }
399  {
400  const map<TNodeID,TNodeID>::const_iterator itEq = lstEquivs.find(from_id);
401  if (itEq!=lstEquivs.end()) from_id = itEq->second;
402  }
403 
404  if (from_id!=to_id) // Don't load self-edges! (probably come from an EQUIV)
405  {
406  TPose3D Ap_mean;
407  mrpt::math::CMatrixDouble66 Ap_cov_inv;
408  // **CAUTION** In the TORO graph format angles are in the RPY order vs. MRPT's YPR.
409  if (!(s>> Ap_mean.x >> Ap_mean.y >> Ap_mean.z >> Ap_mean.roll >> Ap_mean.pitch >> Ap_mean.yaw ))
410  THROW_EXCEPTION(format("Line %u: Error parsing EDGE3 line: '%s'", lineNum, lin.c_str() ) );
411 
412  // **CAUTION** Indices are shuffled to the change YAW(3) <-> ROLL(5) in the order of the data.
413  if (!(s>>
414  Ap_cov_inv(0,0) >> Ap_cov_inv(0,1) >> Ap_cov_inv(0,2) >> Ap_cov_inv(0,5) >> Ap_cov_inv(0,4) >> Ap_cov_inv(0,3) >>
415  Ap_cov_inv(1,1) >> Ap_cov_inv(1,2) >> Ap_cov_inv(1,5) >> Ap_cov_inv(1,4) >> Ap_cov_inv(1,3) >>
416  Ap_cov_inv(2,2) >> Ap_cov_inv(2,5) >> Ap_cov_inv(2,4) >> Ap_cov_inv(2,3) >>
417  Ap_cov_inv(5,5) >> Ap_cov_inv(5,4) >> Ap_cov_inv(5,3) >>
418  Ap_cov_inv(4,4) >> Ap_cov_inv(4,3) >>
419  Ap_cov_inv(3,3) ))
420  {
421  // Cov may be omitted in the file:
422  Ap_cov_inv.unit(6,1.0);
423 
424  if (alreadyWarnedUnknowns.find("MISSING_3D")==alreadyWarnedUnknowns.end())
425  {
426  alreadyWarnedUnknowns.insert("MISSING_3D");
427  cerr << "[CNetworkOfPoses::loadFromTextFile] " << fil << ":" << lineNum << ": Warning: Information matrix missing, assuming unity.\n";
428  }
429  }
430  else
431  {
432  // Complete low triangular part of inf matrix:
433  for (size_t r=1;r<6;r++)
434  for (size_t c=0;c<r;c++)
435  Ap_cov_inv(r,c) = Ap_cov_inv(c,r);
436  }
437 
438  // Convert as needed:
439  typename CNetworkOfPoses<CPOSE>::edge_t newEdge;
440  TPosePDFHelper<CPOSE>::copyFrom3D(newEdge, CPose3DPDFGaussianInf( CPose3D(Ap_mean), Ap_cov_inv ) );
441  g->insertEdge(from_id, to_id, newEdge);
442  }
443  }
444  else if ( strCmpI(key,"EDGE_SE3:QUAT") )
445  {
446  if (!graph_is_3D)
447  THROW_EXCEPTION(format("Line %u: Try to load EDGE3 into a 2D graph: '%s'", lineNum, lin.c_str() ) );
448 
449  // EDGE_SE3:QUAT from_id to_id Ax Ay Az qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
450  // EDGE3 from_id to_id Ax Ay Az Aroll Apitch Ayaw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
451  TNodeID to_id, from_id;
452  if (!(s>> from_id >> to_id ))
453  THROW_EXCEPTION(format("Line %u: Error parsing EDGE_SE3:QUAT line: '%s'", lineNum, lin.c_str() ) );
454 
455  // EQUIV? Replace ID by new one.
456  {
457  const map<TNodeID,TNodeID>::const_iterator itEq = lstEquivs.find(to_id);
458  if (itEq!=lstEquivs.end()) to_id = itEq->second;
459  }
460  {
461  const map<TNodeID,TNodeID>::const_iterator itEq = lstEquivs.find(from_id);
462  if (itEq!=lstEquivs.end()) from_id = itEq->second;
463  }
464 
465  if (from_id!=to_id) // Don't load self-edges! (probably come from an EQUIV)
466  {
467  TPose3DQuat Ap_mean;
468  mrpt::math::CMatrixDouble66 Ap_cov_inv;
469  if (!(s>> Ap_mean.x >> Ap_mean.y >> Ap_mean.z >> Ap_mean.qx >> Ap_mean.qy >> Ap_mean.qz >> Ap_mean.qr ))
470  THROW_EXCEPTION(format("Line %u: Error parsing EDGE_SE3:QUAT line: '%s'", lineNum, lin.c_str() ) );
471 
472  // **CAUTION** Indices are shuffled to the change YAW(3) <-> ROLL(5) in the order of the data.
473  if (!(s>>
474  Ap_cov_inv(0,0) >> Ap_cov_inv(0,1) >> Ap_cov_inv(0,2) >> Ap_cov_inv(0,5) >> Ap_cov_inv(0,4) >> Ap_cov_inv(0,3) >>
475  Ap_cov_inv(1,1) >> Ap_cov_inv(1,2) >> Ap_cov_inv(1,5) >> Ap_cov_inv(1,4) >> Ap_cov_inv(1,3) >>
476  Ap_cov_inv(2,2) >> Ap_cov_inv(2,5) >> Ap_cov_inv(2,4) >> Ap_cov_inv(2,3) >>
477  Ap_cov_inv(5,5) >> Ap_cov_inv(5,4) >> Ap_cov_inv(5,3) >>
478  Ap_cov_inv(4,4) >> Ap_cov_inv(4,3) >>
479  Ap_cov_inv(3,3) ))
480  {
481  // Cov may be omitted in the file:
482  Ap_cov_inv.unit(6,1.0);
483 
484  if (alreadyWarnedUnknowns.find("MISSING_3D")==alreadyWarnedUnknowns.end())
485  {
486  alreadyWarnedUnknowns.insert("MISSING_3D");
487  cerr << "[CNetworkOfPoses::loadFromTextFile] " << fil << ":" << lineNum << ": Warning: Information matrix missing, assuming unity.\n";
488  }
489  }
490  else
491  {
492  // Complete low triangular part of inf matrix:
493  for (size_t r=1;r<6;r++)
494  for (size_t c=0;c<r;c++)
495  Ap_cov_inv(r,c) = Ap_cov_inv(c,r);
496  }
497 
498  // Convert as needed:
499  typename CNetworkOfPoses<CPOSE>::edge_t newEdge;
501  g->insertEdge(from_id, to_id, newEdge);
502  }
503  }
504  else if ( strCmpI(key,"EQUIV") )
505  {
506  // Already read in the 1st pass.
507  }
508  else
509  { // Unknown entry: Warn the user just once:
510  if (alreadyWarnedUnknowns.find(key)==alreadyWarnedUnknowns.end())
511  {
512  alreadyWarnedUnknowns.insert(key);
513  cerr << "[CNetworkOfPoses::loadFromTextFile] " << fil << ":" << lineNum << ": Warning: unknown entry type: " << key << endl;
514  }
515  }
516  } // end while
517 
518  } // end load_graph
519 
520 
521  // --------------------------------------------------------------------------------
522  // Implements: collapseDuplicatedEdges
523  //
524  // Look for duplicated edges (even in opposite directions) between all pairs of nodes and fuse them.
525  // Upon return, only one edge remains between each pair of nodes with the mean
526  // & covariance (or information matrix) corresponding to the Bayesian fusion of all the Gaussians.
527  // --------------------------------------------------------------------------------
528  static size_t graph_of_poses_collapse_dup_edges(graph_t *g)
529  {
530  MRPT_START
531  typedef typename graph_t::edges_map_t::iterator TEdgeIterator;
532 
533  // Data structure: (id1,id2) -> all edges between them
534  // (with id1 < id2)
535  typedef map<pair<TNodeID,TNodeID>, vector<TEdgeIterator> > TListAllEdges; // For god's sake... when will ALL compilers support auto!! :'-(
536  TListAllEdges lstAllEdges;
537 
538  // Clasify all edges to identify duplicated ones:
539  for (TEdgeIterator itEd=g->edges.begin();itEd!=g->edges.end();++itEd)
540  {
541  // Build a pair <id1,id2> with id1 < id2:
542  const pair<TNodeID,TNodeID> arc_id = make_pair( std::min(itEd->first.first,itEd->first.second),std::max(itEd->first.first,itEd->first.second) );
543  // get (or create the first time) the list of edges between them:
544  vector<TEdgeIterator> &lstEdges = lstAllEdges[arc_id];
545  // And add this one:
546  lstEdges.push_back(itEd);
547  }
548 
549  // Now, remove all but the first edge:
550  size_t nRemoved = 0;
551  for (typename TListAllEdges::const_iterator it=lstAllEdges.begin();it!=lstAllEdges.end();++it)
552  {
553  const size_t N = it->second.size();
554  for (size_t i=1;i<N;i++) // i=0 is NOT removed
555  g->edges.erase( it->second[i] );
556 
557  if (N>=2) nRemoved+=N-1;
558  }
559 
560  return nRemoved;
561  MRPT_END
562  } // end of graph_of_poses_collapse_dup_edges
563 
564  // --------------------------------------------------------------------------------
565  // Implements: dijkstra_nodes_estimate
566  //
567  // Compute a simple estimation of the global coordinates of each node just from the information in all edges, sorted in a Dijkstra tree based on the current "root" node.
568  // Note that "global" coordinates are with respect to the node with the ID specified in \a root.
569  // --------------------------------------------------------------------------------
570  static void graph_of_poses_dijkstra_init(graph_t *g)
571  {
572  MRPT_START
573 
574  // Do Dijkstra shortest path from "root" to all other nodes:
576  typedef typename graph_t::constraint_t constraint_t;
577 
578  dijkstra_t dijkstra(*g, g->root);
579 
580  // Get the tree representation of the graph and traverse it
581  // from its root toward the leafs:
582  typename dijkstra_t::tree_graph_t treeView;
583  dijkstra.getTreeGraph(treeView);
584 
585  // This visitor class performs the real job of
586  struct VisitorComputePoses : public dijkstra_t::tree_graph_t::Visitor
587  {
588  graph_t * m_g; // The original graph
589 
590  VisitorComputePoses(graph_t *g) : m_g(g) { }
591  virtual void OnVisitNode( const TNodeID parent_id, const typename dijkstra_t::tree_graph_t::Visitor::tree_t::TEdgeInfo &edge_to_child, const size_t depth_level )
592  {
593  MRPT_UNUSED_PARAM(depth_level);
594  const TNodeID child_id = edge_to_child.id;
595 
596  // Compute the pose of "child_id" as parent_pose (+) edge_delta_pose,
597  // taking into account that that edge may be in reverse order and then have to invert the delta_pose:
598  if ( (!edge_to_child.reverse && !m_g->edges_store_inverse_poses) ||
599  ( edge_to_child.reverse && m_g->edges_store_inverse_poses)
600  )
601  { // pose_child = p_parent (+) p_delta
602  m_g->nodes[child_id].composeFrom( m_g->nodes[parent_id], edge_to_child.data->getPoseMean() );
603  }
604  else
605  { // pose_child = p_parent (+) [(-)p_delta]
606  m_g->nodes[child_id].composeFrom( m_g->nodes[parent_id], - edge_to_child.data->getPoseMean() );
607  }
608  }
609  };
610 
611  // Remove all global poses but for the root node, which is the origin:
612  g->nodes.clear();
613  g->nodes[g->root] = typename constraint_t::type_value(); // Typ: CPose2D() or CPose3D()
614 
615  // Run the visit thru all nodes in the tree:
616  VisitorComputePoses myVisitor(g);
617  treeView.visitBreadthFirst(treeView.root, myVisitor);
618 
619  MRPT_END
620  }
621 
622 
623  // Auxiliary funcs:
624  template <class VEC> static inline double auxMaha2Dist(VEC &err,const CPosePDFGaussianInf &p) {
625  math::wrapToPiInPlace(err[2]);
626  return mrpt::math::multiply_HCHt_scalar(err,p.cov_inv); // err^t*cov_inv*err
627  }
628  template <class VEC> static inline double auxMaha2Dist(VEC &err,const CPose3DPDFGaussianInf &p) {
629  math::wrapToPiInPlace(err[3]);
630  math::wrapToPiInPlace(err[4]);
631  math::wrapToPiInPlace(err[5]);
632  return mrpt::math::multiply_HCHt_scalar(err,p.cov_inv); // err^t*cov_inv*err
633  }
634  template <class VEC> static inline double auxMaha2Dist(VEC &err,const CPosePDFGaussian &p) {
635  math::wrapToPiInPlace(err[2]);
637  p.cov.inv(COV_INV);
638  return mrpt::math::multiply_HCHt_scalar(err,COV_INV); // err^t*cov_inv*err
639  }
640  template <class VEC> static inline double auxMaha2Dist(VEC &err,const CPose3DPDFGaussian &p) {
641  math::wrapToPiInPlace(err[3]);
642  math::wrapToPiInPlace(err[4]);
643  math::wrapToPiInPlace(err[5]);
645  p.cov.inv(COV_INV);
646  return mrpt::math::multiply_HCHt_scalar(err,COV_INV); // err^t*cov_inv*err
647  }
648  // These two are for simulating maha2 distances for non-PDF types: fallback to squared-norm:
649  template <class VEC> static inline double auxMaha2Dist(VEC &err,const mrpt::poses::CPose2D &p) {
650  math::wrapToPiInPlace(err[2]);
651  return square(err[0])+square(err[1])+square(err[2]);
652  }
653  template <class VEC> static inline double auxMaha2Dist(VEC &err,const mrpt::poses::CPose3D &p) {
654  math::wrapToPiInPlace(err[3]);
655  math::wrapToPiInPlace(err[4]);
656  math::wrapToPiInPlace(err[5]);
657  return square(err[0])+square(err[1])+square(err[2])+square(err[3])+square(err[4])+square(err[5]);
658  }
659 
660 
661  static inline double auxEuclid2Dist(const mrpt::poses::CPose2D &p1,const mrpt::poses::CPose2D &p2) {
662  return
663  square(p1.x()-p2.x())+
664  square(p1.y()-p2.y())+
665  square( mrpt::math::wrapToPi(p1.phi()-p2.phi() ) );
666  }
667  static inline double auxEuclid2Dist(const mrpt::poses::CPose3D &p1,const mrpt::poses::CPose3D &p2) {
668  return
669  square(p1.x()-p2.x())+
670  square(p1.y()-p2.y())+
671  square(p1.z()-p2.z())+
672  square( mrpt::math::wrapToPi(p1.yaw()-p2.yaw() ) )+
673  square( mrpt::math::wrapToPi(p1.pitch()-p2.pitch() ) )+
674  square( mrpt::math::wrapToPi(p1.roll()-p2.roll() ) );
675  }
676 
677  // --------------------------------------------------------------------------------
678  // Implements: detail::graph_edge_sqerror
679  //
680  // Compute the square error of a single edge, in comparison to the nodes global poses.
681  // --------------------------------------------------------------------------------
682  static double graph_edge_sqerror(
683  const graph_t *g,
685  bool ignoreCovariances )
686  {
687  MRPT_START
688 
689  // Get node IDs:
690  const TNodeID from_id = itEdge->first.first;
691  const TNodeID to_id = itEdge->first.second;
692 
693  // And their global poses as stored in "nodes"
694  typename graph_t::global_poses_t::const_iterator itPoseFrom = g->nodes.find(from_id);
695  typename graph_t::global_poses_t::const_iterator itPoseTo = g->nodes.find(to_id);
696  ASSERTMSG_(itPoseFrom!=g->nodes.end(), format("Node %u doesn't have a global pose in 'nodes'.", static_cast<unsigned int>(from_id)))
697  ASSERTMSG_(itPoseTo!=g->nodes.end(), format("Node %u doesn't have a global pose in 'nodes'.", static_cast<unsigned int>(to_id)))
698 
699  // The global poses:
700  typedef typename graph_t::constraint_t constraint_t;
701 
702  const typename constraint_t::type_value &from_mean = itPoseFrom->second;
703  const typename constraint_t::type_value &to_mean = itPoseTo->second;
704 
705  // The delta_pose as stored in the edge:
706  const constraint_t &edge_delta_pose = itEdge->second;
707  const typename constraint_t::type_value &edge_delta_pose_mean = edge_delta_pose.getPoseMean();
708 
709  if (ignoreCovariances)
710  { // Square Euclidean distance: Just use the mean values, ignore covs.
711  // from_plus_delta = from_mean (+) edge_delta_pose_mean
712  typename constraint_t::type_value from_plus_delta(UNINITIALIZED_POSE);
713  from_plus_delta.composeFrom(from_mean, edge_delta_pose_mean);
714 
715  // (auxMaha2Dist will also take into account the 2PI wrapping)
716  return auxEuclid2Dist(from_plus_delta,to_mean);
717  }
718  else
719  {
720  // Square Mahalanobis distance
721  // from_plus_delta = from_mean (+) edge_delta_pose (as a Gaussian)
722  constraint_t from_plus_delta = edge_delta_pose;
723  from_plus_delta.changeCoordinatesReference(from_mean);
724 
725  // "from_plus_delta" is now a 3D or 6D Gaussian, to be compared to "to_mean":
726  // We want to compute the squared Mahalanobis distance:
727  // err^t * INV_COV * err
728  //
730  for (size_t i=0;i<constraint_t::type_value::static_size;i++)
731  err[i] = from_plus_delta.getPoseMean()[i] - to_mean[i];
732 
733  // (auxMaha2Dist will also take into account the 2PI wrapping)
734  return auxMaha2Dist(err,from_plus_delta);
735  }
736  MRPT_END
737  }
738 
739  }; // end of graph_ops<graph_t>
740 
741  }// end NS
742  }// end NS
743 } // end NS
744 
745 #endif
#define ASSERT_EQUAL_(__A, __B)
A directed graph with the argument of the template specifying the type of the annotations in the edge...
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:393
static double auxMaha2Dist(VEC &err, const CPosePDFGaussian &p)
size_t getCurrentLineNumber() const
Return the line number of the last line returned with getNextLine.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
A template to obtain the type of its argument as a string at compile time.
Definition: TTypeName.h:47
bool getNextLine(std::string &out_str)
Reads from the file and return the next (non-comment) line, as a std::string.
void copyFrom(const CPosePDF &o)
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
double roll
Roll coordinate (rotation angle over X coordinate).
static double auxMaha2Dist(VEC &err, const mrpt::poses::CPose2D &p)
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
#define THROW_EXCEPTION(msg)
Abstract graph and tree data structures, plus generic graph algorithms.
Scalar * iterator
Definition: eigen_plugins.h:23
mrpt::math::CMatrixDouble66 cov_inv
The inverse of the 6x6 covariance matrix.
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
MAT_C::Scalar multiply_HCHt_scalar(const VECTOR_H &H, const MAT_C &C)
r (a scalar) = H * C * H^t (with a vector H and a symmetric matrix C)
Definition: ops_matrices.h:62
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPose3DPDFGaussianInf &edge, std::ofstream &f)
STL namespace.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
double yaw
Yaw coordinate (rotation angle over Z axis).
static double auxMaha2Dist(VEC &err, const CPosePDFGaussianInf &p)
static void read_graph_of_poses_from_binary_file(graph_t *g, mrpt::utils::CStream &in)
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPose3DPDFGaussian &edge, std::ofstream &f)
static double auxEuclid2Dist(const mrpt::poses::CPose2D &p1, const mrpt::poses::CPose2D &p2)
static double auxEuclid2Dist(const mrpt::poses::CPose3D &p1, const mrpt::poses::CPose3D &p2)
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
static void graph_of_poses_dijkstra_init(graph_t *g)
A directed graph of pose constraints, with edges being the relative pose between pairs of nodes inden...
void rewind()
Reset the read pointer to the beginning of the file.
static void copyFrom2D(CPose2D &p, const CPosePDFGaussianInf &pdf)
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:391
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPosePDFGaussian &edge, std::ofstream &f)
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:61
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
Definition: CArray.h:18
static void save_graph_of_poses_to_binary_file(const graph_t *g, mrpt::utils::CStream &out)
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
uint64_t TNodeID
The type for node IDs in graphs of different types.
#define MRPT_END
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const mrpt::poses::CPose3D &edge, std::ofstream &f)
static void copyFrom2D(POSE_PDF &p, const CPosePDFGaussianInf &pdf)
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
static void copyFrom2D(CPose3D &p, const CPosePDFGaussianInf &pdf)
double z
Translation in x,y,z.
std::pair< TNodeID, TNodeID > TPairNodeIDs
A pair of node IDs.
bool BASE_IMPEXP strCmpI(const std::string &s1, const std::string &s2)
Return true if the two strings are equal (case insensitive)
static double auxMaha2Dist(VEC &err, const CPose3DPDFGaussian &p)
A class for parsing text files, returning each non-empty and non-comment line, along its line number...
double qz
Unit quaternion part, qr,qx,qy,qz.
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
class BASE_IMPEXP CPose3DQuat
Definition: CPose3D.h:23
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
double pitch
Pitch coordinate (rotation angle over Y axis).
Lightweight 3D pose (three spatial coordinates, plus a quaternion ).
static size_t graph_of_poses_collapse_dup_edges(graph_t *g)
static void save_graph_of_poses_to_text_file(const graph_t *g, const std::string &fil)
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
double y
X,Y coordinates.
#define MRPT_START
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:113
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...
A class used to store a 2D pose.
Definition: CPose2D.h:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
This file implements matrix/vector text and binary serialization.
static double graph_edge_sqerror(const graph_t *g, const typename mrpt::graphs::CDirectedGraph< typename graph_t::constraint_t >::edges_map_t::const_iterator &itEdge, bool ignoreCovariances)
a helper struct with static template functions
static void copyFrom3D(CPose3D &p, const CPose3DPDFGaussianInf &pdf)
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:392
Lightweight 2D pose.
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:74
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
static void load_graph_of_poses_from_text_file(graph_t *g, const std::string &fil)
void copyFrom(const CPose3DPDF &o)
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
global_poses_t nodes
The nodes (vertices) of the graph, with their estimated "global" (with respect to root) position...
static double auxMaha2Dist(VEC &err, const CPose3DPDFGaussianInf &p)
The Dijkstra algorithm for finding the shortest path between a given source node in a (weighted) dire...
Definition: dijkstra.h:37
static void copyFrom3D(CPose2D &p, const CPose3DPDFGaussianInf &pdf)
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const mrpt::poses::CPose2D &edge, std::ofstream &f)
static void copyFrom3D(POSE_PDF &p, const CPose3DPDFGaussianInf &pdf)
static void write_VERTEX_line(const TNodeID id, const mrpt::poses::CPose2D &p, std::ofstream &f)
#define ASSERTMSG_(f, __ERROR_MSG)
#define THROW_EXCEPTION_CUSTOM_MSG1(msg, param1)
double z
X,Y,Z, coords.
double phi
Orientation (rads)
mrpt::math::CMatrixDouble33 cov_inv
The inverse of the 3x3 covariance matrix (the "information" matrix)
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPosePDFGaussianInf &edge, std::ofstream &f)
static void write_VERTEX_line(const TNodeID id, const mrpt::poses::CPose3D &p, std::ofstream &f)
static double auxMaha2Dist(VEC &err, const mrpt::poses::CPose3D &p)



Page generated by Doxygen 1.8.11 for MRPT 1.3.2 SVN:Unversioned directory at Sun May 1 08:45:24 UTC 2016