Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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>
28 
29 namespace mrpt
30 {
31  namespace graphs
32  {
33  namespace detail
34  {
35  using namespace std;
36  using namespace mrpt;
37  using namespace mrpt::utils;
38  using namespace mrpt::poses;
39  using namespace mrpt::graphs;
40 
41  template <class POSE_PDF> struct TPosePDFHelper
42  {
43  static inline void copyFrom2D(POSE_PDF &p, const CPosePDFGaussianInf &pdf) { p.copyFrom(pdf); }
44  static inline void copyFrom3D(POSE_PDF &p, const CPose3DPDFGaussianInf &pdf) { p.copyFrom(pdf); }
45  };
46  template <> struct TPosePDFHelper<CPose2D>
47  {
48  static inline void copyFrom2D(CPose2D &p, const CPosePDFGaussianInf &pdf) { p = pdf.mean; }
49  static inline void copyFrom3D(CPose2D &p, const CPose3DPDFGaussianInf &pdf) { p = CPose2D(pdf.mean); }
50  };
51  template <> struct TPosePDFHelper<CPose3D>
52  {
53  static inline void copyFrom2D(CPose3D &p, const CPosePDFGaussianInf &pdf) { p = CPose3D(pdf.mean); }
54  static inline void copyFrom3D(CPose3D &p, const CPose3DPDFGaussianInf &pdf) { p = pdf.mean; }
55  };
56 
57  /// a helper struct with static template functions \sa CNetworkOfPoses
58  template <class graph_t>
59  struct graph_ops
60  {
61  static void write_VERTEX_line(const TNodeID id, const mrpt::poses::CPose2D &p, std::ostream &f)
62  {
63  // VERTEX2 id x y phi
64  f << "VERTEX_SE2 " << id << " " << p.x() << " " << p.y() << " " << p.phi();
65  }
66  static void write_VERTEX_line(const TNodeID id, const mrpt::poses::CPose3D &p, std::ostream &f)
67  {
68  // VERTEX3 id x y z roll pitch yaw
69  // **CAUTION** In the TORO graph format angles are in the RPY order vs. MRPT's YPR.
70  f << "VERTEX3 " << id << " " << p.x() << " " << p.y() << " " << p.z()<< " " << p.roll()<< " " << p.pitch()<< " " << p.yaw();
71  }
72 
73 
74  static void write_EDGE_line(const TPairNodeIDs &edgeIDs,const CPosePDFGaussianInf & edge, std::ostream &f)
75  {
76  // G2O: EDGE_SE2 from_id to_id Ax Ay Aphi inf_xx inf_xy inf_yy inf_pp inf_xp inf_yp
77  f << "EDGE_SE2 " << 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(0,2)<<" "<<
80  edge.cov_inv(1,1)<<" "<<edge.cov_inv(1,2)<<" "<<edge.cov_inv(2,2) << std::endl;
81  }
82  static void write_EDGE_line(const TPairNodeIDs &edgeIDs,const CPose3DPDFGaussianInf & edge, std::ostream &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  f << "EDGE3 " << edgeIDs.first << " " << edgeIDs.second << " " <<
87  edge.mean.x()<<" "<<edge.mean.y()<<" "<<edge.mean.z()<<" "<<
88  edge.mean.roll()<<" "<<edge.mean.pitch()<<" "<<edge.mean.yaw()<<" "<<
89  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)<<" "<<
90  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)<<" "<<
91  edge.cov_inv(2,2)<<" "<<edge.cov_inv(2,5)<<" "<<edge.cov_inv(2,4)<<" "<<edge.cov_inv(2,3)<<" "<<
92  edge.cov_inv(5,5)<<" "<<edge.cov_inv(5,4)<<" "<<edge.cov_inv(5,3)<<" "<<
93  edge.cov_inv(4,4)<<" "<<edge.cov_inv(4,3)<<" "<<
94  edge.cov_inv(3,3) << std::endl;
95  }
96  static void write_EDGE_line(const TPairNodeIDs &edgeIDs,const CPosePDFGaussian & edge, std::ostream &f)
97  {
99  p.copyFrom(edge);
100  write_EDGE_line(edgeIDs,p,f);
101  }
102  static void write_EDGE_line(const TPairNodeIDs &edgeIDs,const CPose3DPDFGaussian & edge, std::ostream &f)
103  {
105  p.copyFrom(edge);
106  write_EDGE_line(edgeIDs,p,f);
107  }
108  static void write_EDGE_line(const TPairNodeIDs &edgeIDs,const mrpt::poses::CPose2D & edge, std::ostream &f)
109  {
111  p.mean = edge;
112  p.cov_inv.unit(3,1.0);
113  write_EDGE_line(edgeIDs,p,f);
114  }
115  static void write_EDGE_line(const TPairNodeIDs &edgeIDs,const mrpt::poses::CPose3D & edge, std::ostream &f)
116  {
118  p.mean = edge;
119  p.cov_inv.unit(6,1.0);
120  write_EDGE_line(edgeIDs,p,f);
121  }
122 
123 
125  const graph_t* g, std::ostream& f)
126  {
127  // 1st: Nodes
128  for (const auto& n : g->nodes)
129  {
130  write_VERTEX_line(n.first, n.second, f);
131  // Node annotations:
132  const auto sAnnot = n.second.retAnnotsAsString();
133  if (!sAnnot.empty()) f << " | " << sAnnot;
134  f << endl;
135  // Root?
136  if (n.first == g->root) f << "FIX " << n.first << endl;
137  }
138 
139  // 2nd: Edges:
140  for (const auto& e : *g)
141  if (e.first.first != e.first.second) // ignore EQUIV edges
142  write_EDGE_line(e.first, e.second, f);
143 
144  }
145 
146  // =================================================================
147  // save_graph_of_poses_to_text_file
148  // =================================================================
150  const graph_t* g, const std::string& fil)
151  {
152  std::ofstream f;
153  f.open(fil.c_str());
154  if (!f.is_open())
156  "Error opening file '%s' for writing", fil.c_str());
157  save_graph_of_poses_to_ostream(g, f);
158  }
159 
160  // =================================================================
161  // save_graph_of_poses_to_binary_file
162  // =================================================================
164  {
165  // Store class name:
166  const std::string sClassName = TTypeName<graph_t>::get();
167  out << sClassName;
168 
169  // Store serialization version & object data:
170  const uint32_t version = 0;
171  out << version;
172  out << g->nodes << g->edges << g->root;
173  }
174 
175  // =================================================================
176  // read_graph_of_poses_from_binary_file
177  // =================================================================
179  {
180  // Compare class name:
181  const std::string sClassName = TTypeName<graph_t>::get();
182  std::string sStoredClassName;
183  in >> sStoredClassName;
184  ASSERT_EQUAL_(sStoredClassName,sClassName)
185 
186  // Check serialization version:
187  uint32_t stored_version;
188  in >> stored_version;
189 
190  g->clear();
191  switch (stored_version)
192  {
193  case 0:
194  in >> g->nodes >> g->edges >> g->root;
195  break;
196  default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(stored_version)
197  }
198 
199  }
200 
201  // =================================================================
202  // load_graph_of_poses_from_text_file
203  // =================================================================
205  graph_t* g, std::istream& f,
206  const std::string& fil = std::string("(none)"))
207  {
208  using mrpt::system::strCmpI;
209  using namespace mrpt::math;
210 
211  using CPOSE = typename graph_t::constraint_t;
212 
213  set<string> alreadyWarnedUnknowns; // for unknown line types, show a
214  // warning to cerr just once.
215 
216  // First, empty the graph:
217  g->clear();
218 
219  // Determine if it's a 2D or 3D graph, just to raise an error if loading
220  // a 3D graph in a 2D one, since
221  // it would be an unintentional loss of information:
222  const bool graph_is_3D = CPOSE::is_3D();
223 
225 
226  // -------------------------------------------
227  // 1st PASS: Read EQUIV entries only
228  // since processing them AFTER loading the data
229  // is much much slower.
230  // -------------------------------------------
231  map<TNodeID, TNodeID> lstEquivs; // List of EQUIV entries: NODEID ->
232  // NEWNODEID. NEWNODEID will be always
233  // the lowest ID number.
234 
235  // Read & process lines each at once until EOF:
236  istringstream s;
237  while (filParser.getNextLine(s))
238  {
239  const unsigned int lineNum = filParser.getCurrentLineNumber();
240  const string lin = s.str();
241 
242  string key;
243  if (!(s >> key) || key.empty())
245  "Line %u: Can't read string for entry type in: '%s'",
246  lineNum, lin.c_str()));
247 
248  if (mrpt::system::strCmpI(key, "EQUIV"))
249  {
250  // Process these ones at the end, for now store in a list:
251  TNodeID id1, id2;
252  if (!(s >> id1 >> id2))
254  "Line %u: Can't read id1 & id2 in EQUIV line: '%s'",
255  lineNum, lin.c_str()));
256  lstEquivs[std::max(id1, id2)] = std::min(id1, id2);
257  }
258  } // end 1st pass
259 
260  // -------------------------------------------
261  // 2nd PASS: Read all other entries
262  // -------------------------------------------
263  filParser.rewind();
264 
265  // Read & process lines each at once until EOF:
266  while (filParser.getNextLine(s))
267  {
268  const unsigned int lineNum = filParser.getCurrentLineNumber();
269  const string lin = s.str();
270 
271  // Recognized strings:
272  // VERTEX2 id x y phi
273  // =(VERTEX_SE2)
274  // EDGE2 from_id to_id Ax Ay Aphi inf_xx inf_xy inf_yy inf_pp
275  // inf_xp inf_yp
276  // =(EDGE or EDGE_SE2 or ODOMETRY)
277  // VERTEX3 id x y z roll pitch yaw
278  // VERTEX_SE3:QUAT id x y z qx qy qz qw
279  // EDGE3 from_id to_id Ax Ay Az Aroll Apitch Ayaw inf_11 inf_12 ..
280  // inf_16 inf_22 .. inf_66
281  // EDGE_SE3:QUAT from_id to_id Ax Ay Az qx qy qz qw inf_11 inf_12
282  // .. inf_16 inf_22 .. inf_66
283  // EQUIV id1 id2
284  string key;
285  if (!(s >> key) || key.empty())
287  "Line %u: Can't read string for entry type in: '%s'",
288  lineNum, lin.c_str()));
289 
290  if (strCmpI(key, "VERTEX2") || strCmpI(key, "VERTEX") ||
291  strCmpI(key, "VERTEX_SE2"))
292  {
293  TNodeID id;
294  TPose2D p2D;
295  if (!(s >> id >> p2D.x >> p2D.y >> p2D.phi))
297  "Line %u: Error parsing VERTEX2 line: '%s'", lineNum,
298  lin.c_str()));
299 
300  // Make sure the node is new:
301  if (g->nodes.find(id) != g->nodes.end())
303  "Line %u: Error, duplicated verted ID %u in line: "
304  "'%s'",
305  lineNum, static_cast<unsigned int>(id), lin.c_str()));
306 
307  // EQUIV? Replace ID by new one.
308  {
310  lstEquivs.find(id);
311  if (itEq != lstEquivs.end()) id = itEq->second;
312  }
313 
314  // Add to map: ID -> absolute pose:
315  if (g->nodes.find(id) == g->nodes.end())
316  {
317  using pose_t = typename CNetworkOfPoses<
318  CPOSE>::constraint_t::type_value;
319  pose_t& newNode = g->nodes[id];
320  newNode = pose_t(CPose2D(
321  p2D)); // Convert to mrpt::poses::CPose3D if needed
322  }
323  }
324  else if (strCmpI(key, "VERTEX3"))
325  {
326  if (!graph_is_3D)
328  "Line %u: Try to load VERTEX3 into a 2D graph: "
329  "'%s'",
330  lineNum, lin.c_str()));
331 
332  // VERTEX3 id x y z roll pitch yaw
333  TNodeID id;
334  TPose3D p3D;
335  // **CAUTION** In the TORO graph format angles are in the RPY
336  // order vs. MRPT's YPR.
337  if (!(s >> id >> p3D.x >> p3D.y >> p3D.z >> p3D.roll >>
338  p3D.pitch >> p3D.yaw))
340  "Line %u: Error parsing VERTEX3 line: '%s'", lineNum,
341  lin.c_str()));
342 
343  // Make sure the node is new:
344  if (g->nodes.find(id) != g->nodes.end())
346  "Line %u: Error, duplicated verted ID %u in line: "
347  "'%s'",
348  lineNum, static_cast<unsigned int>(id), lin.c_str()));
349 
350  // EQUIV? Replace ID by new one.
351  {
353  lstEquivs.find(id);
354  if (itEq != lstEquivs.end()) id = itEq->second;
355  }
356 
357  // Add to map: ID -> absolute pose:
358  if (g->nodes.find(id) == g->nodes.end())
359  {
360  g->nodes[id] = typename CNetworkOfPoses<CPOSE>::
362  p3D)); // Auto converted to CPose2D if needed
363  }
364  }
365  else if (strCmpI(key, "VERTEX_SE3:QUAT"))
366  {
367  if (!graph_is_3D)
369  "Line %u: Try to load VERTEX_SE3:QUAT into a 2D "
370  "graph: '%s'",
371  lineNum, lin.c_str()));
372 
373  // VERTEX_SE3:QUAT id x y z qx qy qz qw
374  TNodeID id;
375  TPose3DQuat p3D;
376  if (!(s >> id >> p3D.x >> p3D.y >> p3D.z >> p3D.qx >> p3D.qy >>
377  p3D.qz >> p3D.qr))
379  "Line %u: Error parsing VERTEX_SE3:QUAT line: '%s'",
380  lineNum, lin.c_str()));
381 
382  // Make sure the node is new:
383  if (g->nodes.find(id) != g->nodes.end())
385  "Line %u: Error, duplicated verted ID %u in line: "
386  "'%s'",
387  lineNum, static_cast<unsigned int>(id), lin.c_str()));
388 
389  // EQUIV? Replace ID by new one.
390  {
392  lstEquivs.find(id);
393  if (itEq != lstEquivs.end()) id = itEq->second;
394  }
395 
396  // Add to map: ID -> absolute pose:
397  if (g->nodes.find(id) == g->nodes.end())
398  {
399  g->nodes[id] = typename CNetworkOfPoses<CPOSE>::
401  CPose3D(CPose3DQuat(p3D))); // Auto converted to
402  // CPose2D if needed
403  }
404  }
405  else if (
406  strCmpI(key, "EDGE2") || strCmpI(key, "EDGE") ||
407  strCmpI(key, "ODOMETRY") || strCmpI(key, "EDGE_SE2"))
408  {
409  // EDGE_SE2 from_id to_id Ax Ay Aphi inf_xx i_xy i_xp i_yy i_yp
410  // i_pp Read values are:
411  // [ s00 s01 s02 ]
412  // [ - s11 s12 ]
413  // [ - - s22 ]
414  //
415  TNodeID to_id, from_id;
416  if (!(s >> from_id >> to_id))
418  "Line %u: Error parsing EDGE2 line: '%s'", lineNum,
419  lin.c_str()));
420 
421  // EQUIV? Replace ID by new one.
422  {
424  lstEquivs.find(to_id);
425  if (itEq != lstEquivs.end()) to_id = itEq->second;
426  }
427  {
429  lstEquivs.find(from_id);
430  if (itEq != lstEquivs.end()) from_id = itEq->second;
431  }
432 
433  if (from_id != to_id) // Don't load self-edges! (probably come
434  // from an EQUIV)
435  {
436  TPose2D Ap_mean;
437  mrpt::math::CMatrixDouble33 Ap_cov_inv;
438  if (!(s >> Ap_mean.x >> Ap_mean.y >> Ap_mean.phi >>
439  Ap_cov_inv(0, 0) >> Ap_cov_inv(0, 1) >>
440  Ap_cov_inv(0, 2) >> Ap_cov_inv(1, 1) >>
441  Ap_cov_inv(1, 2) >> Ap_cov_inv(2, 2)))
443  "Line %u: Error parsing EDGE2 line: '%s'", lineNum,
444  lin.c_str()));
445 
446  // Complete low triangular part of inf matrix:
447  Ap_cov_inv(1, 0) = Ap_cov_inv(0, 1);
448  Ap_cov_inv(2, 0) = Ap_cov_inv(0, 2);
449  Ap_cov_inv(2, 1) = Ap_cov_inv(1, 2);
450 
451  // Convert to 2D cov, 3D cov or 3D inv_cov as needed:
452  typename CNetworkOfPoses<CPOSE>::edge_t newEdge;
454  newEdge,
455  CPosePDFGaussianInf(CPose2D(Ap_mean), Ap_cov_inv));
456  g->insertEdge(from_id, to_id, newEdge);
457  }
458  }
459  else if (strCmpI(key, "EDGE3"))
460  {
461  if (!graph_is_3D)
463  "Line %u: Try to load EDGE3 into a 2D graph: '%s'",
464  lineNum, lin.c_str()));
465 
466  // EDGE3 from_id to_id Ax Ay Az Aroll Apitch Ayaw inf_11 inf_12
467  // .. inf_16 inf_22 .. inf_66
468  TNodeID to_id, from_id;
469  if (!(s >> from_id >> to_id))
471  "Line %u: Error parsing EDGE3 line: '%s'", lineNum,
472  lin.c_str()));
473 
474  // EQUIV? Replace ID by new one.
475  {
477  lstEquivs.find(to_id);
478  if (itEq != lstEquivs.end()) to_id = itEq->second;
479  }
480  {
482  lstEquivs.find(from_id);
483  if (itEq != lstEquivs.end()) from_id = itEq->second;
484  }
485 
486  if (from_id != to_id) // Don't load self-edges! (probably come
487  // from an EQUIV)
488  {
489  TPose3D Ap_mean;
490  mrpt::math::CMatrixDouble66 Ap_cov_inv;
491  // **CAUTION** In the TORO graph format angles are in the
492  // RPY order vs. MRPT's YPR.
493  if (!(s >> Ap_mean.x >> Ap_mean.y >> Ap_mean.z >>
494  Ap_mean.roll >> Ap_mean.pitch >> Ap_mean.yaw))
496  "Line %u: Error parsing EDGE3 line: '%s'", lineNum,
497  lin.c_str()));
498 
499  // **CAUTION** Indices are shuffled to the change YAW(3) <->
500  // ROLL(5) in the order of the data.
501  if (!(s >> Ap_cov_inv(0, 0) >> Ap_cov_inv(0, 1) >>
502  Ap_cov_inv(0, 2) >> Ap_cov_inv(0, 5) >>
503  Ap_cov_inv(0, 4) >> Ap_cov_inv(0, 3) >>
504  Ap_cov_inv(1, 1) >> Ap_cov_inv(1, 2) >>
505  Ap_cov_inv(1, 5) >> Ap_cov_inv(1, 4) >>
506  Ap_cov_inv(1, 3) >> Ap_cov_inv(2, 2) >>
507  Ap_cov_inv(2, 5) >> Ap_cov_inv(2, 4) >>
508  Ap_cov_inv(2, 3) >> Ap_cov_inv(5, 5) >>
509  Ap_cov_inv(5, 4) >> Ap_cov_inv(5, 3) >>
510  Ap_cov_inv(4, 4) >> Ap_cov_inv(4, 3) >>
511  Ap_cov_inv(3, 3)))
512  {
513  // Cov may be omitted in the file:
514  Ap_cov_inv.unit(6, 1.0);
515 
516  if (alreadyWarnedUnknowns.find("MISSING_3D") ==
517  alreadyWarnedUnknowns.end())
518  {
519  alreadyWarnedUnknowns.insert("MISSING_3D");
520  cerr << "[CNetworkOfPoses::loadFromTextFile] "
521  << fil << ":" << lineNum
522  << ": Warning: Information matrix missing, "
523  "assuming unity.\n";
524  }
525  }
526  else
527  {
528  // Complete low triangular part of inf matrix:
529  for (size_t r = 1; r < 6; r++)
530  for (size_t c = 0; c < r; c++)
531  Ap_cov_inv(r, c) = Ap_cov_inv(c, r);
532  }
533 
534  // Convert as needed:
535  typename CNetworkOfPoses<CPOSE>::edge_t newEdge;
537  newEdge,
538  CPose3DPDFGaussianInf(CPose3D(Ap_mean), Ap_cov_inv));
539  g->insertEdge(from_id, to_id, newEdge);
540  }
541  }
542  else if (strCmpI(key, "EDGE_SE3:QUAT"))
543  {
544  if (!graph_is_3D)
546  "Line %u: Try to load EDGE3 into a 2D graph: '%s'",
547  lineNum, lin.c_str()));
548 
549  // EDGE_SE3:QUAT from_id to_id Ax Ay Az qx qy qz qw inf_11
550  // inf_12 .. inf_16 inf_22 .. inf_66
551  // EDGE3 from_id to_id Ax Ay Az Aroll Apitch Ayaw inf_11 inf_12
552  // .. inf_16 inf_22 .. inf_66
553  TNodeID to_id, from_id;
554  if (!(s >> from_id >> to_id))
556  "Line %u: Error parsing EDGE_SE3:QUAT line: '%s'",
557  lineNum, lin.c_str()));
558 
559  // EQUIV? Replace ID by new one.
560  {
562  lstEquivs.find(to_id);
563  if (itEq != lstEquivs.end()) to_id = itEq->second;
564  }
565  {
567  lstEquivs.find(from_id);
568  if (itEq != lstEquivs.end()) from_id = itEq->second;
569  }
570 
571  if (from_id != to_id) // Don't load self-edges! (probably come
572  // from an EQUIV)
573  {
574  TPose3DQuat Ap_mean;
575  mrpt::math::CMatrixDouble66 Ap_cov_inv;
576  if (!(s >> Ap_mean.x >> Ap_mean.y >> Ap_mean.z >>
577  Ap_mean.qx >> Ap_mean.qy >> Ap_mean.qz >> Ap_mean.qr))
579  "Line %u: Error parsing EDGE_SE3:QUAT line: "
580  "'%s'",
581  lineNum, lin.c_str()));
582 
583  // **CAUTION** Indices are shuffled to the change YAW(3) <->
584  // ROLL(5) in the order of the data.
585  if (!(s >> Ap_cov_inv(0, 0) >> Ap_cov_inv(0, 1) >>
586  Ap_cov_inv(0, 2) >> Ap_cov_inv(0, 5) >>
587  Ap_cov_inv(0, 4) >> Ap_cov_inv(0, 3) >>
588  Ap_cov_inv(1, 1) >> Ap_cov_inv(1, 2) >>
589  Ap_cov_inv(1, 5) >> Ap_cov_inv(1, 4) >>
590  Ap_cov_inv(1, 3) >> Ap_cov_inv(2, 2) >>
591  Ap_cov_inv(2, 5) >> Ap_cov_inv(2, 4) >>
592  Ap_cov_inv(2, 3) >> Ap_cov_inv(5, 5) >>
593  Ap_cov_inv(5, 4) >> Ap_cov_inv(5, 3) >>
594  Ap_cov_inv(4, 4) >> Ap_cov_inv(4, 3) >>
595  Ap_cov_inv(3, 3)))
596  {
597  // Cov may be omitted in the file:
598  Ap_cov_inv.unit(6, 1.0);
599 
600  if (alreadyWarnedUnknowns.find("MISSING_3D") ==
601  alreadyWarnedUnknowns.end())
602  {
603  alreadyWarnedUnknowns.insert("MISSING_3D");
604  cerr << "[CNetworkOfPoses::loadFromTextFile] "
605  << fil << ":" << lineNum
606  << ": Warning: Information matrix missing, "
607  "assuming unity.\n";
608  }
609  }
610  else
611  {
612  // Complete low triangular part of inf matrix:
613  for (size_t r = 1; r < 6; r++)
614  for (size_t c = 0; c < r; c++)
615  Ap_cov_inv(r, c) = Ap_cov_inv(c, r);
616  }
617 
618  // Convert as needed:
619  typename CNetworkOfPoses<CPOSE>::edge_t newEdge;
621  newEdge,
623  CPose3D(CPose3DQuat(Ap_mean)), Ap_cov_inv));
624  g->insertEdge(from_id, to_id, newEdge);
625  }
626  }
627  else if (strCmpI(key, "EQUIV"))
628  {
629  // Already read in the 1st pass.
630  }
631  else if (strCmpI(key, "FIX"))
632  {
633  TNodeID id;
634  if (!(s >> id))
636  "Line %u: Can't read id in FIX line: '%s'",
637  lineNum, lin.c_str()));
638  g->root = id;
639  }
640  else
641  { // Unknown entry: Warn the user just once:
642  if (alreadyWarnedUnknowns.find(key) ==
643  alreadyWarnedUnknowns.end())
644  {
645  alreadyWarnedUnknowns.insert(key);
646  cerr << "[CNetworkOfPoses::loadFromTextFile] " << fil << ":"
647  << lineNum << ": Warning: unknown entry type: " << key
648  << endl;
649  }
650  }
651  } // end while
652 
653  } // end load_graph
654 
656  graph_t* g, const std::string& fil)
657  {
658  std::ifstream f(fil);
659  if (!f.is_open())
661  "Error opening file '%s' for reading", fil.c_str());
662  load_graph_of_poses_from_text_stream(g, f, fil);
663  }
664 
665  // --------------------------------------------------------------------------------
666  // Implements: collapseDuplicatedEdges
667  //
668  // Look for duplicated edges (even in opposite directions) between all pairs of nodes and fuse them.
669  // Upon return, only one edge remains between each pair of nodes with the mean
670  // & covariance (or information matrix) corresponding to the Bayesian fusion of all the Gaussians.
671  // --------------------------------------------------------------------------------
672  static size_t graph_of_poses_collapse_dup_edges(graph_t *g)
673  {
674  MRPT_START
675  typedef typename graph_t::edges_map_t::iterator TEdgeIterator;
676 
677  // Data structure: (id1,id2) -> all edges between them
678  // (with id1 < id2)
679  typedef map<pair<TNodeID,TNodeID>, vector<TEdgeIterator> > TListAllEdges; // For god's sake... when will ALL compilers support auto!! :'-(
680  TListAllEdges lstAllEdges;
681 
682  // Clasify all edges to identify duplicated ones:
683  for (TEdgeIterator itEd=g->edges.begin();itEd!=g->edges.end();++itEd)
684  {
685  // Build a pair <id1,id2> with id1 < id2:
686  const pair<TNodeID,TNodeID> arc_id = make_pair(std::min(itEd->first.first,itEd->first.second),std::max(itEd->first.first,itEd->first.second));
687  // get (or create the first time) the list of edges between them:
688  vector<TEdgeIterator> &lstEdges = lstAllEdges[arc_id];
689  // And add this one:
690  lstEdges.push_back(itEd);
691  }
692 
693  // Now, remove all but the first edge:
694  size_t nRemoved = 0;
695  for (typename TListAllEdges::const_iterator it=lstAllEdges.begin();it!=lstAllEdges.end();++it)
696  {
697  const size_t N = it->second.size();
698  for (size_t i=1;i<N;i++) // i=0 is NOT removed
699  g->edges.erase(it->second[i]);
700 
701  if (N>=2) nRemoved+=N-1;
702  }
703 
704  return nRemoved;
705  MRPT_END
706  } // end of graph_of_poses_collapse_dup_edges
707 
708  // --------------------------------------------------------------------------------
709  // Implements: dijkstra_nodes_estimate
710  //
711  // 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.
712  // Note that "global" coordinates are with respect to the node with the ID specified in \a root.
713  // --------------------------------------------------------------------------------
714  static void graph_of_poses_dijkstra_init(graph_t *g)
715  {
716  MRPT_START;
717  using namespace std;
718 
719  // Do Dijkstra shortest path from "root" to all other nodes:
721  typedef typename graph_t::constraint_t constraint_t;
722 
723  // initialize corresponding dijkstra object from root.
724  dijkstra_t dijkstra(*g, g->root);
725 
726  // Get the tree representation of the graph and traverse it
727  // from its root toward the leafs:
728  typename dijkstra_t::tree_graph_t treeView;
729  dijkstra.getTreeGraph(treeView);
730 
731  // This visitor class performs the real job of
732  struct VisitorComputePoses : public dijkstra_t::tree_graph_t::Visitor
733  {
734  graph_t * m_g; // The original graph
735 
736  VisitorComputePoses(graph_t *g) : m_g(g) { }
737  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) MRPT_OVERRIDE
738  {
739  MRPT_UNUSED_PARAM(depth_level);
740  const TNodeID child_id = edge_to_child.id;
741 
742  // Compute the pose of "child_id" as parent_pose (+) edge_delta_pose,
743  // taking into account that that edge may be in reverse order
744  // and then have to invert the delta_pose:
745  if ((!edge_to_child.reverse && !m_g->edges_store_inverse_poses) ||
746  (edge_to_child.reverse && m_g->edges_store_inverse_poses)
747  )
748  { // pose_child = p_parent (+) p_delta
749  m_g->nodes[child_id].composeFrom(m_g->nodes[parent_id], edge_to_child.data->getPoseMean());
750  }
751  else
752  { // pose_child = p_parent (+) [(-)p_delta]
753  m_g->nodes[child_id].composeFrom(m_g->nodes[parent_id], - edge_to_child.data->getPoseMean());
754  }
755  }
756  };
757 
758  // Remove all global poses except for the root node, which is the origin:
759  //
760  // Keep track of the NODE_ANNOTATIONS for each node and put it after
761  // the global pose computation
762  bool empty_node_annots = g->nodes.begin()->second.is_node_annots_empty;
763  map<const TNodeID, TNodeAnnotations*> nodeID_to_annots;
764  if (!empty_node_annots) {
766  poses_cit = g->nodes.begin();
767  poses_cit != g->nodes.end();
768  ++poses_cit) {
769 
770  nodeID_to_annots.insert(
771  make_pair(
772  poses_cit->first, poses_cit->second.getCopyOfAnnots()));
773  }
774  }
775 
776  g->nodes.clear();
777  g->nodes[g->root] = typename constraint_t::type_value(); // Typ: CPose2D() or CPose3D()
778 
779  // Run the visit thru all nodes in the tree:
780  VisitorComputePoses myVisitor(g);
781  treeView.visitBreadthFirst(treeView.root, myVisitor);
782 
783  // Fill the NODE_ANNOTATIONS part again
784  if (!empty_node_annots) {
786  poses_cit = g->nodes.begin();
787  poses_cit != g->nodes.end();
788  ++poses_cit) {
789 
790  TNodeAnnotations* node_annots = nodeID_to_annots.at(poses_cit->first);
791  bool res = poses_cit->second.setAnnots(*node_annots);
792 
793  // free dynamically allocated mem
794  delete node_annots;
795  node_annots = NULL;
796 
797  // make sure setting annotations was successful
799  "Setting annotations for nodeID \"%lu\" was unsuccessful",
800  static_cast<unsigned long>(poses_cit->first)));
801  }
802  }
803 
804  MRPT_END
805  } // end of graph_of_poses_dijkstra_init
806 
807 
808  // Auxiliary funcs:
809  template <class VEC> static inline double auxMaha2Dist(VEC &err,const CPosePDFGaussianInf &p) {
810  math::wrapToPiInPlace(err[2]);
811  return mrpt::math::multiply_HCHt_scalar(err,p.cov_inv); // err^t*cov_inv*err
812  }
813  template <class VEC> static inline double auxMaha2Dist(VEC &err,const CPose3DPDFGaussianInf &p) {
814  math::wrapToPiInPlace(err[3]);
815  math::wrapToPiInPlace(err[4]);
816  math::wrapToPiInPlace(err[5]);
817  return mrpt::math::multiply_HCHt_scalar(err,p.cov_inv); // err^t*cov_inv*err
818  }
819  template <class VEC> static inline double auxMaha2Dist(VEC &err,const CPosePDFGaussian &p) {
820  math::wrapToPiInPlace(err[2]);
822  p.cov.inv(COV_INV);
823  return mrpt::math::multiply_HCHt_scalar(err,COV_INV); // err^t*cov_inv*err
824  }
825  template <class VEC> static inline double auxMaha2Dist(VEC &err,const CPose3DPDFGaussian &p) {
826  math::wrapToPiInPlace(err[3]);
827  math::wrapToPiInPlace(err[4]);
828  math::wrapToPiInPlace(err[5]);
830  p.cov.inv(COV_INV);
831  return mrpt::math::multiply_HCHt_scalar(err,COV_INV); // err^t*cov_inv*err
832  }
833  // These two are for simulating maha2 distances for non-PDF types: fallback to squared-norm:
834  template <class VEC> static inline double auxMaha2Dist(VEC &err,const mrpt::poses::CPose2D &p) {
835  math::wrapToPiInPlace(err[2]);
836  return square(err[0])+square(err[1])+square(err[2]);
837  }
838  template <class VEC> static inline double auxMaha2Dist(VEC &err,const mrpt::poses::CPose3D &p) {
839  math::wrapToPiInPlace(err[3]);
840  math::wrapToPiInPlace(err[4]);
841  math::wrapToPiInPlace(err[5]);
842  return square(err[0])+square(err[1])+square(err[2])+square(err[3])+square(err[4])+square(err[5]);
843  }
844 
845 
846  static inline double auxEuclid2Dist(const mrpt::poses::CPose2D &p1,const mrpt::poses::CPose2D &p2) {
847  return
848  square(p1.x()-p2.x())+
849  square(p1.y()-p2.y())+
850  square(mrpt::math::wrapToPi(p1.phi()-p2.phi()));
851  }
852  static inline double auxEuclid2Dist(const mrpt::poses::CPose3D &p1,const mrpt::poses::CPose3D &p2) {
853  return
854  square(p1.x()-p2.x())+
855  square(p1.y()-p2.y())+
856  square(p1.z()-p2.z())+
857  square(mrpt::math::wrapToPi(p1.yaw()-p2.yaw()))+
859  square(mrpt::math::wrapToPi(p1.roll()-p2.roll()));
860  }
861 
862  // --------------------------------------------------------------------------------
863  // Implements: detail::graph_edge_sqerror
864  //
865  // Compute the square error of a single edge, in comparison to the nodes global poses.
866  // --------------------------------------------------------------------------------
867  static double graph_edge_sqerror(
868  const graph_t *g,
870  bool ignoreCovariances)
871  {
872  MRPT_START
873 
874  // Get node IDs:
875  const TNodeID from_id = itEdge->first.first;
876  const TNodeID to_id = itEdge->first.second;
877 
878  // And their global poses as stored in "nodes"
879  typename graph_t::global_poses_t::const_iterator itPoseFrom = g->nodes.find(from_id);
880  typename graph_t::global_poses_t::const_iterator itPoseTo = g->nodes.find(to_id);
881  ASSERTMSG_(itPoseFrom!=g->nodes.end(), format("Node %u doesn't have a global pose in 'nodes'.", static_cast<unsigned int>(from_id)))
882  ASSERTMSG_(itPoseTo!=g->nodes.end(), format("Node %u doesn't have a global pose in 'nodes'.", static_cast<unsigned int>(to_id)))
883 
884  // The global poses:
885  typedef typename graph_t::constraint_t constraint_t;
886 
887  const typename constraint_t::type_value &from_mean = itPoseFrom->second;
888  const typename constraint_t::type_value &to_mean = itPoseTo->second;
889 
890  // The delta_pose as stored in the edge:
891  const constraint_t &edge_delta_pose = itEdge->second;
892  const typename constraint_t::type_value &edge_delta_pose_mean = edge_delta_pose.getPoseMean();
893 
894  if (ignoreCovariances)
895  { // Square Euclidean distance: Just use the mean values, ignore covs.
896  // from_plus_delta = from_mean (+) edge_delta_pose_mean
897  typename constraint_t::type_value from_plus_delta(UNINITIALIZED_POSE);
898  from_plus_delta.composeFrom(from_mean, edge_delta_pose_mean);
899 
900  // (auxMaha2Dist will also take into account the 2PI wrapping)
901  return auxEuclid2Dist(from_plus_delta,to_mean);
902  }
903  else
904  {
905  // Square Mahalanobis distance
906  // from_plus_delta = from_mean (+) edge_delta_pose (as a Gaussian)
907  constraint_t from_plus_delta = edge_delta_pose;
908  from_plus_delta.changeCoordinatesReference(from_mean);
909 
910  // "from_plus_delta" is now a 3D or 6D Gaussian, to be compared to "to_mean":
911  // We want to compute the squared Mahalanobis distance:
912  // err^t * INV_COV * err
913  //
915  for (size_t i=0;i<constraint_t::type_value::static_size;i++)
916  err[i] = from_plus_delta.getPoseMean()[i] - to_mean[i];
917 
918  // (auxMaha2Dist will also take into account the 2PI wrapping)
919  return auxMaha2Dist(err,from_plus_delta);
920  }
921  MRPT_END
922  }
923 
924  }; // end of graph_ops<graph_t>
925 
926  }// end NS
927  }// end NS
928 } // end NS
929 
930 #endif
#define ASSERT_EQUAL_(__A, __B)
A directed graph with the argument of the template specifying the type of the annotations in the edge...
static double auxMaha2Dist(VEC &err, const CPosePDFGaussian &p)
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
#define min(a, b)
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPosePDFGaussianInf &edge, std::ostream &f)
A template to obtain the type of its argument as a string at compile time.
Definition: TTypeName.h:47
Abstract class from which NodeAnnotations related classes can be implemented.
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
bool getNextLine(std::string &out_str)
Reads from the file and return the next (non-comment) line, as a std::string.
double roll
Roll coordinate (rotation angle over X coordinate).
static double auxMaha2Dist(VEC &err, const mrpt::poses::CPose2D &p)
#define THROW_EXCEPTION(msg)
Abstract graph and tree data structures, plus generic graph algorithms.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
GLenum GLsizei n
Definition: glext.h:4618
Scalar * iterator
Definition: eigen_plugins.h:23
mrpt::math::CMatrixDouble66 cov_inv
The inverse of the 6x6 covariance matrix.
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:392
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:391
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
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)
GLdouble s
Definition: glext.h:3602
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)
static void graph_of_poses_dijkstra_init(graph_t *g)
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
A directed graph of pose constraints, with edges being the relative poses between pairs of nodes iden...
void rewind()
Reset the read pointer to the beginning of the file.
static void copyFrom2D(CPose2D &p, const CPosePDFGaussianInf &pdf)
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPose3DPDFGaussian &edge, std::ostream &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
static void save_graph_of_poses_to_ostream(const graph_t *g, std::ostream &f)
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
static void save_graph_of_poses_to_binary_file(const graph_t *g, mrpt::utils::CStream &out)
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
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPosePDFGaussian &edge, std::ostream &f)
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
static void copyFrom2D(POSE_PDF &p, const CPosePDFGaussianInf &pdf)
const GLubyte * c
Definition: glext.h:5590
#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.
GLubyte g
Definition: glext.h:5575
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
std::pair< TNodeID, TNodeID > TPairNodeIDs
A pair of node IDs.
static double auxMaha2Dist(VEC &err, const CPose3DPDFGaussian &p)
int version
Definition: mrpt_jpeglib.h:898
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:41
A class for parsing text files, returning each non-empty and non-comment line, along its line number...
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
double qz
Unit quaternion part, qr,qx,qy,qz.
GLsizei const GLchar ** string
Definition: glext.h:3919
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:393
static void load_graph_of_poses_from_text_stream(graph_t *g, std::istream &f, const std::string &fil=std::string("(none)"))
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)
double y
X,Y coordinates.
#define MRPT_START
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...
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
static void write_VERTEX_line(const TNodeID id, const mrpt::poses::CPose2D &p, std::ostream &f)
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
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)
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
GLuint id
Definition: glext.h:3770
static void copyFrom3D(CPose3D &p, const CPose3DPDFGaussianInf &pdf)
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const mrpt::poses::CPose3D &edge, std::ostream &f)
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...
GLuint in
Definition: glext.h:6301
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 ...
size_t getCurrentLineNumber() const
Return the line number of the last line returned with getNextLine.
static void load_graph_of_poses_from_text_file(graph_t *g, const std::string &fil)
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const mrpt::poses::CPose2D &edge, std::ostream &f)
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:103
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPose3DPDFGaussianInf &edge, std::ostream &f)
GLuint res
Definition: glext.h:6298
static void copyFrom3D(CPose2D &p, const CPose3DPDFGaussianInf &pdf)
static void copyFrom3D(POSE_PDF &p, const CPose3DPDFGaussianInf &pdf)
bool BASE_IMPEXP strCmpI(const std::string &s1, const std::string &s2)
Return true if the two strings are equal (case insensitive)
unsigned __int32 uint32_t
Definition: rptypes.h:49
static void write_VERTEX_line(const TNodeID id, const mrpt::poses::CPose3D &p, std::ostream &f)
#define ASSERTMSG_(f, __ERROR_MSG)
GLfloat GLfloat p
Definition: glext.h:5587
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 double auxMaha2Dist(VEC &err, const mrpt::poses::CPose3D &p)



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 8277875f6 Mon Jun 11 02:47:32 2018 +0200 at lun oct 28 01:50:49 CET 2019