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>
13 #include <mrpt/utils/CTextFileLinesParser.h>
14 #include <mrpt/math/lightweight_geom_data.h>
15 #include <mrpt/math/CArrayNumeric.h>
16 #include <mrpt/math/wrap2pi.h>
17 #include <mrpt/math/ops_matrices.h> // multiply_*()
18 #include <mrpt/math/matrix_serialization.h>
19 #include <mrpt/system/string_utils.h>
20 #include <mrpt/poses/CPose2D.h>
21 #include <mrpt/poses/CPose3D.h>
22 #include <mrpt/poses/CPose3DQuat.h>
23 #include <mrpt/poses/CPosePDFGaussian.h>
24 #include <mrpt/poses/CPosePDFGaussianInf.h>
25 #include <mrpt/poses/CPose3DPDFGaussian.h>
26 #include <mrpt/poses/CPose3DPDFGaussianInf.h>
27 #include <mrpt/graphs/TNodeAnnotations.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  {
98  CPosePDFGaussianInf p;
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  {
104  CPose3DPDFGaussianInf p;
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  {
110  CPosePDFGaussianInf p;
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  {
117  CPose3DPDFGaussianInf p;
118  p.mean = edge;
119  p.cov_inv.unit(6,1.0);
120  write_EDGE_line(edgeIDs,p,f);
121  }
122 
123 
124  static void save_graph_of_poses_to_ostream(
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  // =================================================================
149  static void save_graph_of_poses_to_text_file(
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())
155  THROW_EXCEPTION_FMT(
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  // =================================================================
163  static void save_graph_of_poses_to_binary_file(const graph_t *g, mrpt::utils::CStream &out)
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  // =================================================================
178  static void read_graph_of_poses_from_binary_file(graph_t *g, mrpt::utils::CStream &in)
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  // =================================================================
204  static void load_graph_of_poses_from_text_stream(
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 
224  mrpt::utils::CTextFileLinesParser filParser(f);
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())
244  THROW_EXCEPTION(format(
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))
253  THROW_EXCEPTION(format(
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())
286  THROW_EXCEPTION(format(
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))
296  THROW_EXCEPTION(format(
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())
302  THROW_EXCEPTION(format(
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  {
309  const map<TNodeID, TNodeID>::const_iterator itEq =
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)
327  THROW_EXCEPTION(format(
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))
339  THROW_EXCEPTION(format(
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())
345  THROW_EXCEPTION(format(
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  {
352  const map<TNodeID, TNodeID>::const_iterator itEq =
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>::
361  constraint_t::type_value(CPose3D(
362  p3D)); // Auto converted to CPose2D if needed
363  }
364  }
365  else if (strCmpI(key, "VERTEX_SE3:QUAT"))
366  {
367  if (!graph_is_3D)
368  THROW_EXCEPTION(format(
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))
378  THROW_EXCEPTION(format(
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())
384  THROW_EXCEPTION(format(
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  {
391  const map<TNodeID, TNodeID>::const_iterator itEq =
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>::
400  constraint_t::type_value(
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))
417  THROW_EXCEPTION(format(
418  "Line %u: Error parsing EDGE2 line: '%s'", lineNum,
419  lin.c_str()));
420 
421  // EQUIV? Replace ID by new one.
422  {
423  const map<TNodeID, TNodeID>::const_iterator itEq =
424  lstEquivs.find(to_id);
425  if (itEq != lstEquivs.end()) to_id = itEq->second;
426  }
427  {
428  const map<TNodeID, TNodeID>::const_iterator itEq =
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)))
442  THROW_EXCEPTION(format(
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;
453  TPosePDFHelper<CPOSE>::copyFrom2D(
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)
462  THROW_EXCEPTION(format(
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))
470  THROW_EXCEPTION(format(
471  "Line %u: Error parsing EDGE3 line: '%s'", lineNum,
472  lin.c_str()));
473 
474  // EQUIV? Replace ID by new one.
475  {
476  const map<TNodeID, TNodeID>::const_iterator itEq =
477  lstEquivs.find(to_id);
478  if (itEq != lstEquivs.end()) to_id = itEq->second;
479  }
480  {
481  const map<TNodeID, TNodeID>::const_iterator itEq =
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))
495  THROW_EXCEPTION(format(
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;
536  TPosePDFHelper<CPOSE>::copyFrom3D(
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)
545  THROW_EXCEPTION(format(
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))
555  THROW_EXCEPTION(format(
556  "Line %u: Error parsing EDGE_SE3:QUAT line: '%s'",
557  lineNum, lin.c_str()));
558 
559  // EQUIV? Replace ID by new one.
560  {
561  const map<TNodeID, TNodeID>::const_iterator itEq =
562  lstEquivs.find(to_id);
563  if (itEq != lstEquivs.end()) to_id = itEq->second;
564  }
565  {
566  const map<TNodeID, TNodeID>::const_iterator itEq =
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))
578  THROW_EXCEPTION(format(
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;
620  TPosePDFHelper<CPOSE>::copyFrom3D(
621  newEdge,
622  CPose3DPDFGaussianInf(
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))
635  THROW_EXCEPTION(format(
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 
655  static void load_graph_of_poses_from_text_file(
656  graph_t* g, const std::string& fil)
657  {
658  std::ifstream f(fil);
659  if (!f.is_open())
660  THROW_EXCEPTION_FMT(
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:
720  typedef CDijkstra<graph_t,typename graph_t::maps_implementation_t> dijkstra_t;
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) {
765  for (typename graph_t::global_poses_t::const_iterator
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) {
785  for (typename graph_t::global_poses_t::iterator
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
798  ASSERTMSG_(res, mrpt::format(
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]);
821  mrpt::math::CMatrixDouble33 COV_INV(mrpt::math::UNINITIALIZED_MATRIX);
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]);
829  mrpt::math::CMatrixDouble66 COV_INV(mrpt::math::UNINITIALIZED_MATRIX);
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()))+
858  square(mrpt::math::wrapToPi(p1.pitch()-p2.pitch()))+
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,
869  const typename mrpt::graphs::CDirectedGraph<typename graph_t::constraint_t>::edges_map_t::const_iterator &itEdge,
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  //
914  mrpt::math::CArrayDouble<constraint_t::type_value::static_size> err;
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
ASSERT_EQUAL_
#define ASSERT_EQUAL_(__A, __B)
Definition: maps/CColouredPointsMap.h:281
mrpt::graphs::CDirectedGraph
A directed graph with the argument of the template specifying the type of the annotations in the edge...
Definition: CDirectedGraph.h:55
mrpt::graphs::detail::graph_ops::auxMaha2Dist
static double auxMaha2Dist(VEC &err, const CPosePDFGaussian &p)
Definition: CNetworkOfPoses_impl.h:819
CTextFileLinesParser.h
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
CPose3DQuat.h
min
#define min(a, b)
Definition: rplidar_driver.cpp:43
static_size
Definition: eigen_plugins.h:17
mrpt::graphs::detail::graph_ops::write_EDGE_line
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPosePDFGaussianInf &edge, std::ostream &f)
Definition: CNetworkOfPoses_impl.h:74
mrpt::utils::TTypeName
A template to obtain the type of its argument as a string at compile time.
Definition: TTypeName.h:47
mrpt::graphs::detail::TNodeAnnotations
Abstract class from which NodeAnnotations related classes can be implemented.
Definition: TNodeAnnotations.h:18
MRPT_OVERRIDE
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition: maps/CColouredPointsMap.h:58
mrpt::utils::CTextFileLinesParser::getNextLine
bool getNextLine(std::string &out_str)
Reads from the file and return the next (non-comment) line, as a std::string.
Definition: CTextFileLinesParser.h:83
mrpt::math::TPose3D::roll
double roll
Roll coordinate (rotation angle over X coordinate).
Definition: lightweight_geom_data.h:411
mrpt::graphs::detail::graph_ops::auxMaha2Dist
static double auxMaha2Dist(VEC &err, const mrpt::poses::CPose2D &p)
Definition: CNetworkOfPoses_impl.h:834
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: maps/CColouredPointsMap.h:154
mrpt::graphs
Abstract graph and tree data structures, plus generic graph algorithms.
Definition: CAStarAlgorithm.h:19
THROW_EXCEPTION_FMT
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: maps/CColouredPointsMap.h:163
n
GLenum GLsizei n
Definition: glext.h:4618
iterator
Scalar * iterator
Definition: eigen_plugins.h:23
CPose3DPDFGaussianInf.h
mrpt::poses::CPose3DPDFGaussianInf::cov_inv
mrpt::math::CMatrixDouble66 cov_inv
The inverse of the 6x6 covariance matrix.
Definition: CPose3DPDFGaussianInf.h:54
mrpt::poses::CPose3D::pitch
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:392
mrpt::poses::CPose3D::yaw
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:391
ops_matrices.h
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
mrpt::math::multiply_HCHt_scalar
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
std
STL namespace.
CPose3D.h
const_iterator
const Scalar * const_iterator
Definition: eigen_plugins.h:24
mrpt::math::TPose3D::yaw
double yaw
Yaw coordinate (rotation angle over Z axis).
Definition: lightweight_geom_data.h:409
mrpt::graphs::detail::graph_ops::auxMaha2Dist
static double auxMaha2Dist(VEC &err, const CPosePDFGaussianInf &p)
Definition: CNetworkOfPoses_impl.h:809
mrpt::graphs::detail::graph_ops::read_graph_of_poses_from_binary_file
static void read_graph_of_poses_from_binary_file(graph_t *g, mrpt::utils::CStream &in)
Definition: CNetworkOfPoses_impl.h:178
s
GLdouble s
Definition: glext.h:3602
mrpt::graphs::detail::graph_ops::auxEuclid2Dist
static double auxEuclid2Dist(const mrpt::poses::CPose2D &p1, const mrpt::poses::CPose2D &p2)
Definition: CNetworkOfPoses_impl.h:846
mrpt::graphs::detail::graph_ops::auxEuclid2Dist
static double auxEuclid2Dist(const mrpt::poses::CPose3D &p1, const mrpt::poses::CPose3D &p2)
Definition: CNetworkOfPoses_impl.h:852
CArrayNumeric.h
mrpt::graphs::detail::graph_ops::graph_of_poses_dijkstra_init
static void graph_of_poses_dijkstra_init(graph_t *g)
Definition: CNetworkOfPoses_impl.h:714
mrpt::math::square
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
mrpt::graphs::CNetworkOfPoses
A directed graph of pose constraints, with edges being the relative poses between pairs of nodes iden...
Definition: CNetworkOfPoses.h:104
string_utils.h
mrpt::utils::CTextFileLinesParser::rewind
void rewind()
Reset the read pointer to the beginning of the file.
Definition: CTextFileLinesParser.h:73
mrpt::graphs::detail::TPosePDFHelper< CPose2D >::copyFrom2D
static void copyFrom2D(CPose2D &p, const CPosePDFGaussianInf &pdf)
Definition: CNetworkOfPoses_impl.h:48
mrpt::graphs::detail::graph_ops::write_EDGE_line
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPose3DPDFGaussian &edge, std::ostream &f)
Definition: CNetworkOfPoses_impl.h:102
mrpt::utils::CStream
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
mrpt::math::wrapToPiInPlace
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:61
mrpt::graphs::detail::graph_ops::save_graph_of_poses_to_ostream
static void save_graph_of_poses_to_ostream(const graph_t *g, std::ostream &f)
Definition: CNetworkOfPoses_impl.h:124
mrpt::math
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
mrpt::graphs::detail::graph_ops::save_graph_of_poses_to_binary_file
static void save_graph_of_poses_to_binary_file(const graph_t *g, mrpt::utils::CStream &out)
Definition: CNetworkOfPoses_impl.h:163
mrpt::poses::CPosePDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
Definition: CPosePDFGaussian.h:32
mrpt::mrpt::utils::TNodeID
uint64_t TNodeID
The type for node IDs in graphs of different types.
Definition: maps/CColouredPointsMap.h:45
MRPT_END
#define MRPT_END
Definition: maps/CColouredPointsMap.h:370
mrpt::graphs::detail::graph_ops::write_EDGE_line
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPosePDFGaussian &edge, std::ostream &f)
Definition: CNetworkOfPoses_impl.h:96
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Definition: maps/CColouredPointsMap.h:307
mrpt::graphs::detail::TPosePDFHelper::copyFrom2D
static void copyFrom2D(POSE_PDF &p, const CPosePDFGaussianInf &pdf)
Definition: CNetworkOfPoses_impl.h:43
c
const GLubyte * c
Definition: glext.h:5590
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: maps/CColouredPointsMap.h:217
TNodeAnnotations.h
mrpt::graphs::detail::TPosePDFHelper< CPose3D >::copyFrom2D
static void copyFrom2D(CPose3D &p, const CPosePDFGaussianInf &pdf)
Definition: CNetworkOfPoses_impl.h:53
mrpt::math::TPose3DQuat::z
double z
Translation in x,y,z.
Definition: lightweight_geom_data.h:485
g
GLubyte g
Definition: glext.h:5575
mrpt::format
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
mrpt::mrpt::utils::TPairNodeIDs
std::pair< TNodeID, TNodeID > TPairNodeIDs
A pair of node IDs.
Definition: maps/CColouredPointsMap.h:46
mrpt::graphs::detail::graph_ops::auxMaha2Dist
static double auxMaha2Dist(VEC &err, const CPose3DPDFGaussian &p)
Definition: CNetworkOfPoses_impl.h:825
version
int version
Definition: mrpt_jpeglib.h:898
mrpt::poses::CPose3DQuat
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
mrpt::utils::CTextFileLinesParser
A class for parsing text files, returning each non-empty and non-comment line, along its line number...
Definition: CTextFileLinesParser.h:27
mrpt::mrpt::format
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
mrpt::math::TPose3DQuat::qz
double qz
Unit quaternion part, qr,qx,qy,qz.
Definition: lightweight_geom_data.h:486
string
GLsizei const GLchar ** string
Definition: glext.h:3919
mrpt::math::wrapToPi
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
mrpt::poses::CPose3D::roll
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:393
mrpt::graphs::detail::graph_ops::load_graph_of_poses_from_text_stream
static void load_graph_of_poses_from_text_stream(graph_t *g, std::istream &f, const std::string &fil=std::string("(none)"))
Definition: CNetworkOfPoses_impl.h:204
mrpt::math::TPose3D::pitch
double pitch
Pitch coordinate (rotation angle over Y axis).
Definition: lightweight_geom_data.h:410
mrpt::math::TPose3DQuat
Lightweight 3D pose (three spatial coordinates, plus a quaternion ).
Definition: lightweight_geom_data.h:483
mrpt::graphs::detail::graph_ops::graph_of_poses_collapse_dup_edges
static size_t graph_of_poses_collapse_dup_edges(graph_t *g)
Definition: CNetworkOfPoses_impl.h:672
CPose3DPDFGaussian.h
mrpt::graphs::detail::graph_ops::save_graph_of_poses_to_text_file
static void save_graph_of_poses_to_text_file(const graph_t *g, const std::string &fil)
Definition: CNetworkOfPoses_impl.h:149
mrpt::math::TPose2D::y
double y
X,Y coordinates.
Definition: lightweight_geom_data.h:150
MRPT_START
#define MRPT_START
Definition: maps/CColouredPointsMap.h:366
CPosePDFGaussian.h
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
mrpt::poses::CPosePDFGaussianInf
A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...
Definition: CPosePDFGaussianInf.h:35
r
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
lightweight_geom_data.h
mrpt::graphs::detail::graph_ops::write_VERTEX_line
static void write_VERTEX_line(const TNodeID id, const mrpt::poses::CPose2D &p, std::ostream &f)
Definition: CNetworkOfPoses_impl.h:61
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
CPosePDFGaussianInf.h
matrix_serialization.h
This file implements matrix/vector text and binary serialization.
mrpt::graphs::detail::graph_ops::graph_edge_sqerror
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)
Definition: CNetworkOfPoses_impl.h:867
mrpt::poses::CPose2D::phi
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
id
GLuint id
Definition: glext.h:3770
mrpt::graphs::detail::TPosePDFHelper< CPose3D >::copyFrom3D
static void copyFrom3D(CPose3D &p, const CPose3DPDFGaussianInf &pdf)
Definition: CNetworkOfPoses_impl.h:54
mrpt::graphs::detail::graph_ops::write_EDGE_line
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const mrpt::poses::CPose3D &edge, std::ostream &f)
Definition: CNetworkOfPoses_impl.h:115
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: lightweight_geom_data.h:406
mrpt::poses::CPose3DPDFGaussianInf
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
Definition: CPose3DPDFGaussianInf.h:38
in
GLuint in
Definition: glext.h:6301
mrpt::math::TPose2D
Lightweight 2D pose.
Definition: lightweight_geom_data.h:148
mrpt::math::CArrayDouble
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:74
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
Definition: CPose3DPDFGaussian.h:37
mrpt::utils::CTextFileLinesParser::getCurrentLineNumber
size_t getCurrentLineNumber() const
Return the line number of the last line returned with getNextLine.
Definition: CTextFileLinesParser.h:125
mrpt::graphs::detail::graph_ops::load_graph_of_poses_from_text_file
static void load_graph_of_poses_from_text_file(graph_t *g, const std::string &fil)
Definition: CNetworkOfPoses_impl.h:655
mrpt::graphs::detail::graph_ops::write_EDGE_line
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const mrpt::poses::CPose2D &edge, std::ostream &f)
Definition: CNetworkOfPoses_impl.h:108
mrpt::graphs::CNetworkOfPoses::nodes
global_poses_t nodes
The nodes (vertices) of the graph, with their estimated "global" (with respect to root) position...
Definition: CNetworkOfPoses.h:173
mrpt::graphs::detail::graph_ops::auxMaha2Dist
static double auxMaha2Dist(VEC &err, const CPose3DPDFGaussianInf &p)
Definition: CNetworkOfPoses_impl.h:813
mrpt::graphs::CDijkstra
The Dijkstra algorithm for finding the shortest path between a given source node in a (weighted) dire...
Definition: dijkstra.h:103
mrpt::graphs::detail::graph_ops::write_EDGE_line
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPose3DPDFGaussianInf &edge, std::ostream &f)
Definition: CNetworkOfPoses_impl.h:82
res
GLuint res
Definition: glext.h:6298
mrpt::graphs::detail::TPosePDFHelper< CPose2D >::copyFrom3D
static void copyFrom3D(CPose2D &p, const CPose3DPDFGaussianInf &pdf)
Definition: CNetworkOfPoses_impl.h:49
mrpt::graphs::detail::TPosePDFHelper::copyFrom3D
static void copyFrom3D(POSE_PDF &p, const CPose3DPDFGaussianInf &pdf)
Definition: CNetworkOfPoses_impl.h:44
mrpt::system::strCmpI
bool BASE_IMPEXP strCmpI(const std::string &s1, const std::string &s2)
Return true if the two strings are equal (case insensitive)
Definition: string_utils.cpp:286
uint32_t
unsigned __int32 uint32_t
Definition: rptypes.h:49
mrpt::graphs::detail::graph_ops::write_VERTEX_line
static void write_VERTEX_line(const TNodeID id, const mrpt::poses::CPose3D &p, std::ostream &f)
Definition: CNetworkOfPoses_impl.h:66
CPose2D.h
ASSERTMSG_
#define ASSERTMSG_(f, __ERROR_MSG)
Definition: maps/CColouredPointsMap.h:277
p
GLfloat GLfloat p
Definition: glext.h:5587
mrpt::math::TPose3D::z
double z
X,Y,Z, coords.
Definition: lightweight_geom_data.h:408
mrpt::math::TPose2D::phi
double phi
Orientation (rads)
Definition: lightweight_geom_data.h:151
mrpt::poses::CPosePDFGaussianInf::cov_inv
mrpt::math::CMatrixDouble33 cov_inv
The inverse of the 3x3 covariance matrix (the "information" matrix)
Definition: CPosePDFGaussianInf.h:51
mrpt::graphs::detail::graph_ops::auxMaha2Dist
static double auxMaha2Dist(VEC &err, const mrpt::poses::CPose3D &p)
Definition: CNetworkOfPoses_impl.h:838


Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019