MRPT  1.9.9
CNetworkOfPoses_impl.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
12 #include <mrpt/graphs/dijkstra.h>
14 #include <mrpt/math/CVectorFixed.h>
15 #include <mrpt/math/TPose2D.h>
16 #include <mrpt/math/TPose3D.h>
17 #include <mrpt/math/TPose3DQuat.h>
19 #include <mrpt/math/ops_matrices.h> // multiply_*()
20 #include <mrpt/math/wrap2pi.h>
21 #include <mrpt/poses/CPose2D.h>
22 #include <mrpt/poses/CPose3D.h>
25 #include <mrpt/poses/CPose3DQuat.h>
29 
30 namespace mrpt::graphs::detail
31 {
32 using namespace std;
33 using namespace mrpt;
34 using namespace mrpt::poses;
35 using namespace mrpt::graphs;
36 
37 template <class POSE_PDF>
39 {
40  static inline void copyFrom2D(POSE_PDF& p, const CPosePDFGaussianInf& pdf)
41  {
42  p.copyFrom(pdf);
43  }
44  static inline void copyFrom3D(POSE_PDF& p, const CPose3DPDFGaussianInf& pdf)
45  {
46  p.copyFrom(pdf);
47  }
48 };
49 template <>
51 {
52  static inline void copyFrom2D(CPose2D& p, const CPosePDFGaussianInf& pdf)
53  {
54  p = pdf.mean;
55  }
56  static inline void copyFrom3D(CPose2D& p, const CPose3DPDFGaussianInf& pdf)
57  {
58  p = CPose2D(pdf.mean);
59  }
60 };
61 template <>
63 {
64  static inline void copyFrom2D(CPose3D& p, const CPosePDFGaussianInf& pdf)
65  {
66  p = CPose3D(pdf.mean);
67  }
68  static inline void copyFrom3D(CPose3D& p, const CPose3DPDFGaussianInf& pdf)
69  {
70  p = pdf.mean;
71  }
72 };
73 
74 /// a helper struct with static template functions \sa CNetworkOfPoses
75 template <class graph_t>
76 struct graph_ops
77 {
78  static void write_VERTEX_line(
79  const TNodeID id, const mrpt::poses::CPose2D& p, std::ostream& f)
80  {
81  // VERTEX2 id x y phi
82  f << "VERTEX_SE2 " << id << " " << p.x() << " " << p.y() << " "
83  << p.phi();
84  }
85  static void write_VERTEX_line(
86  const TNodeID id, const mrpt::poses::CPose3D& p, std::ostream& f)
87  {
88  // VERTEX3 id x y z roll pitch yaw
89  // **CAUTION** In the TORO graph format angles are in the RPY order vs.
90  // MRPT's YPR.
91  f << "VERTEX3 " << id << " " << p.x() << " " << p.y() << " " << p.z()
92  << " " << p.roll() << " " << p.pitch() << " " << p.yaw();
93  }
94 
95  static void write_EDGE_line(
96  const TPairNodeIDs& edgeIDs, const CPosePDFGaussianInf& edge,
97  std::ostream& f)
98  {
99  // G2O: EDGE_SE2 from_id to_id Ax Ay Aphi i_xx i_xy i_xp i_yy i_yp
100  // inf_pp
101  f << "EDGE_SE2 " << edgeIDs.first << " " << edgeIDs.second << " "
102  << edge.mean.x() << " " << edge.mean.y() << " " << edge.mean.phi()
103  << " " << edge.cov_inv(0, 0) << " " << edge.cov_inv(0, 1) << " "
104  << edge.cov_inv(0, 2) << " " << edge.cov_inv(1, 1) << " "
105  << edge.cov_inv(1, 2) << " " << edge.cov_inv(2, 2) << endl;
106  }
107  static void write_EDGE_line(
108  const TPairNodeIDs& edgeIDs, const CPose3DPDFGaussianInf& edge,
109  std::ostream& f)
110  {
111  // EDGE3 from_id to_id Ax Ay Az Aroll Apitch Ayaw inf_11 inf_12 ..
112  // inf_16 inf_22 .. inf_66
113  // **CAUTION** In the TORO graph format angles are in the RPY order vs.
114  // MRPT's YPR.
115  // **CAUTION** TORO docs say "from_id" "to_id" in the opposite order,
116  // but it seems from the data that this is the correct expected format.
117  f << "EDGE3 " << edgeIDs.first << " " << edgeIDs.second << " "
118  << edge.mean.x() << " " << edge.mean.y() << " " << edge.mean.z()
119  << " " << edge.mean.roll() << " " << edge.mean.pitch() << " "
120  << edge.mean.yaw() << " " << edge.cov_inv(0, 0) << " "
121  << edge.cov_inv(0, 1) << " " << edge.cov_inv(0, 2) << " "
122  << edge.cov_inv(0, 5) << " " << edge.cov_inv(0, 4) << " "
123  << edge.cov_inv(0, 3) << " " << edge.cov_inv(1, 1) << " "
124  << edge.cov_inv(1, 2) << " " << edge.cov_inv(1, 5) << " "
125  << edge.cov_inv(1, 4) << " " << edge.cov_inv(1, 3) << " "
126  << edge.cov_inv(2, 2) << " " << edge.cov_inv(2, 5) << " "
127  << edge.cov_inv(2, 4) << " " << edge.cov_inv(2, 3) << " "
128  << edge.cov_inv(5, 5) << " " << edge.cov_inv(5, 4) << " "
129  << edge.cov_inv(5, 3) << " " << edge.cov_inv(4, 4) << " "
130  << edge.cov_inv(4, 3) << " " << edge.cov_inv(3, 3) << endl;
131  }
132  static void write_EDGE_line(
133  const TPairNodeIDs& edgeIDs, const CPosePDFGaussian& edge,
134  std::ostream& f)
135  {
137  p.copyFrom(edge);
138  write_EDGE_line(edgeIDs, p, f);
139  }
140  static void write_EDGE_line(
141  const TPairNodeIDs& edgeIDs, const CPose3DPDFGaussian& edge,
142  std::ostream& f)
143  {
145  p.copyFrom(edge);
146  write_EDGE_line(edgeIDs, p, f);
147  }
148  static void write_EDGE_line(
149  const TPairNodeIDs& edgeIDs, const mrpt::poses::CPose2D& edge,
150  std::ostream& f)
151  {
153  p.mean = edge;
154  p.cov_inv.setIdentity();
155  write_EDGE_line(edgeIDs, p, f);
156  }
157  static void write_EDGE_line(
158  const TPairNodeIDs& edgeIDs, const mrpt::poses::CPose3D& edge,
159  std::ostream& f)
160  {
162  p.mean = edge;
163  p.cov_inv.setIdentity();
164  write_EDGE_line(edgeIDs, p, f);
165  }
166 
168  const graph_t* g, std::ostream& f)
169  {
170  // 1st: Nodes
171  for (const auto& n : g->nodes)
172  {
173  write_VERTEX_line(n.first, n.second, f);
174  // Node annotations:
175  const auto sAnnot = n.second.retAnnotsAsString();
176  if (!sAnnot.empty()) f << " | " << sAnnot;
177  f << endl;
178  // Root?
179  if (n.first == g->root) f << "FIX " << n.first << endl;
180  }
181 
182  // 2nd: Edges:
183  for (const auto& e : *g)
184  if (e.first.first != e.first.second) // ignore EQUIV edges
185  write_EDGE_line(e.first, e.second, f);
186 
187  } // end save_graph
188 
189  // =================================================================
190  // save_graph_of_poses_to_text_file
191  // =================================================================
193  const graph_t* g, const std::string& fil)
194  {
195  std::ofstream f;
196  f.open(fil.c_str());
197  if (!f.is_open())
199  "Error opening file '%s' for writing", fil.c_str());
200  save_graph_of_poses_to_ostream(g, f);
201  }
202 
203  // =================================================================
204  // save_graph_of_poses_to_binary_file
205  // =================================================================
207  const graph_t* g, mrpt::serialization::CArchive& out)
208  {
209  // Store class name:
210  const std::string sClassName =
212  out << sClassName;
213 
214  // Store serialization version & object data:
215  const uint32_t version = 0;
216  out << version;
217  out << g->nodes << g->edges << g->root;
218  }
219 
220  // =================================================================
221  // read_graph_of_poses_from_binary_file
222  // =================================================================
225  {
226  // Compare class name:
227  const std::string sClassName =
229  std::string sStoredClassName;
230  in >> sStoredClassName;
231  ASSERT_EQUAL_(sStoredClassName, sClassName);
232 
233  // Check serialization version:
234  uint32_t stored_version;
235  in >> stored_version;
236 
237  g->clear();
238  switch (stored_version)
239  {
240  case 0:
241  in >> g->nodes >> g->edges >> g->root;
242  break;
243  default:
245  }
246  }
247 
248  // =================================================================
249  // load_graph_of_poses_from_text_file
250  // =================================================================
252  graph_t* g, std::istream& f,
253  const std::string& fil = std::string("(none)"))
254  {
255  using mrpt::system::strCmpI;
256  using namespace mrpt::math;
257 
258  using CPOSE = typename graph_t::constraint_t;
259 
260  set<string> alreadyWarnedUnknowns; // for unknown line types, show a
261  // warning to cerr just once.
262 
263  // First, empty the graph:
264  g->clear();
265 
266  // Determine if it's a 2D or 3D graph, just to raise an error if loading
267  // a 3D graph in a 2D one, since
268  // it would be an unintentional loss of information:
269  const bool graph_is_3D = CPOSE::is_3D();
270 
271  mrpt::io::CTextFileLinesParser filParser(f);
272 
273  // -------------------------------------------
274  // 1st PASS: Read EQUIV entries only
275  // since processing them AFTER loading the data
276  // is much much slower.
277  // -------------------------------------------
278  map<TNodeID, TNodeID> lstEquivs; // List of EQUIV entries: NODEID ->
279  // NEWNODEID. NEWNODEID will be always
280  // the lowest ID number.
281 
282  // Read & process lines each at once until EOF:
283  istringstream s;
284  while (filParser.getNextLine(s))
285  {
286  const unsigned int lineNum = filParser.getCurrentLineNumber();
287  const string lin = s.str();
288 
289  string key;
290  if (!(s >> key) || key.empty())
292  "Line %u: Can't read string for entry type in: '%s'",
293  lineNum, lin.c_str()));
294 
295  if (mrpt::system::strCmpI(key, "EQUIV"))
296  {
297  // Process these ones at the end, for now store in a list:
298  TNodeID id1, id2;
299  if (!(s >> id1 >> id2))
301  "Line %u: Can't read id1 & id2 in EQUIV line: '%s'",
302  lineNum, lin.c_str()));
303  lstEquivs[std::max(id1, id2)] = std::min(id1, id2);
304  }
305  } // end 1st pass
306 
307  // -------------------------------------------
308  // 2nd PASS: Read all other entries
309  // -------------------------------------------
310  filParser.rewind();
311 
312  // Read & process lines each at once until EOF:
313  while (filParser.getNextLine(s))
314  {
315  const unsigned int lineNum = filParser.getCurrentLineNumber();
316  const string lin = s.str();
317 
318  // Recognized strings:
319  // VERTEX2 id x y phi
320  // =(VERTEX_SE2)
321  // EDGE2 from_id to_id Ax Ay Aphi inf_xx inf_xy inf_yy inf_pp
322  // inf_xp inf_yp
323  // =(EDGE or EDGE_SE2 or ODOMETRY)
324  // VERTEX3 id x y z roll pitch yaw
325  // VERTEX_SE3:QUAT id x y z qx qy qz qw
326  // EDGE3 from_id to_id Ax Ay Az Aroll Apitch Ayaw inf_11 inf_12 ..
327  // inf_16 inf_22 .. inf_66
328  // EDGE_SE3:QUAT from_id to_id Ax Ay Az qx qy qz qw inf_11 inf_12
329  // .. inf_16 inf_22 .. inf_66
330  // EQUIV id1 id2
331  string key;
332  if (!(s >> key) || key.empty())
334  "Line %u: Can't read string for entry type in: '%s'",
335  lineNum, lin.c_str()));
336 
337  if (strCmpI(key, "VERTEX2") || strCmpI(key, "VERTEX") ||
338  strCmpI(key, "VERTEX_SE2"))
339  {
340  TNodeID id;
341  TPose2D p2D;
342  if (!(s >> id >> p2D.x >> p2D.y >> p2D.phi))
344  "Line %u: Error parsing VERTEX2 line: '%s'", lineNum,
345  lin.c_str()));
346 
347  // Make sure the node is new:
348  if (g->nodes.find(id) != g->nodes.end())
350  "Line %u: Error, duplicated verted ID %u in line: "
351  "'%s'",
352  lineNum, static_cast<unsigned int>(id), lin.c_str()));
353 
354  // EQUIV? Replace ID by new one.
355  {
356  const auto itEq = lstEquivs.find(id);
357  if (itEq != lstEquivs.end()) id = itEq->second;
358  }
359 
360  // Add to map: ID -> absolute pose:
361  if (g->nodes.find(id) == g->nodes.end())
362  {
363  using pose_t = typename CNetworkOfPoses<
364  CPOSE>::constraint_t::type_value;
365  pose_t& newNode = g->nodes[id];
366  newNode = pose_t(CPose2D(
367  p2D)); // Convert to mrpt::poses::CPose3D if needed
368  }
369  }
370  else if (strCmpI(key, "VERTEX3"))
371  {
372  if (!graph_is_3D)
374  "Line %u: Try to load VERTEX3 into a 2D graph: "
375  "'%s'",
376  lineNum, lin.c_str()));
377 
378  // VERTEX3 id x y z roll pitch yaw
379  TNodeID id;
380  TPose3D p3D;
381  // **CAUTION** In the TORO graph format angles are in the RPY
382  // order vs. MRPT's YPR.
383  if (!(s >> id >> p3D.x >> p3D.y >> p3D.z >> p3D.roll >>
384  p3D.pitch >> p3D.yaw))
386  "Line %u: Error parsing VERTEX3 line: '%s'", lineNum,
387  lin.c_str()));
388 
389  // Make sure the node is new:
390  if (g->nodes.find(id) != g->nodes.end())
392  "Line %u: Error, duplicated verted ID %u in line: "
393  "'%s'",
394  lineNum, static_cast<unsigned int>(id), lin.c_str()));
395 
396  // EQUIV? Replace ID by new one.
397  {
398  const auto itEq = lstEquivs.find(id);
399  if (itEq != lstEquivs.end()) id = itEq->second;
400  }
401 
402  // Add to map: ID -> absolute pose:
403  if (g->nodes.find(id) == g->nodes.end())
404  {
405  g->nodes[id] = typename CNetworkOfPoses<CPOSE>::
407  p3D)); // Auto converted to CPose2D if needed
408  }
409  }
410  else if (strCmpI(key, "VERTEX_SE3:QUAT"))
411  {
412  if (!graph_is_3D)
414  "Line %u: Try to load VERTEX_SE3:QUAT into a 2D "
415  "graph: '%s'",
416  lineNum, lin.c_str()));
417 
418  // VERTEX_SE3:QUAT id x y z qx qy qz qw
419  TNodeID id;
420  TPose3DQuat p3D;
421  if (!(s >> id >> p3D.x >> p3D.y >> p3D.z >> p3D.qx >> p3D.qy >>
422  p3D.qz >> p3D.qr))
424  "Line %u: Error parsing VERTEX_SE3:QUAT line: '%s'",
425  lineNum, lin.c_str()));
426 
427  // Make sure the node is new:
428  if (g->nodes.find(id) != g->nodes.end())
430  "Line %u: Error, duplicated verted ID %u in line: "
431  "'%s'",
432  lineNum, static_cast<unsigned int>(id), lin.c_str()));
433 
434  // EQUIV? Replace ID by new one.
435  {
436  const auto itEq = lstEquivs.find(id);
437  if (itEq != lstEquivs.end()) id = itEq->second;
438  }
439 
440  // Add to map: ID -> absolute pose:
441  if (g->nodes.find(id) == g->nodes.end())
442  {
443  g->nodes[id] = typename CNetworkOfPoses<CPOSE>::
445  CPose3D(CPose3DQuat(p3D))); // Auto converted to
446  // CPose2D if needed
447  }
448  }
449  else if (
450  strCmpI(key, "EDGE2") || strCmpI(key, "EDGE") ||
451  strCmpI(key, "ODOMETRY") || strCmpI(key, "EDGE_SE2"))
452  {
453  // EDGE_SE2 from_id to_id Ax Ay Aphi inf_xx i_xy i_xp i_yy i_yp
454  // i_pp Read values are:
455  // [ s00 s01 s02 ]
456  // [ - s11 s12 ]
457  // [ - - s22 ]
458  //
459  TNodeID to_id, from_id;
460  if (!(s >> from_id >> to_id))
462  "Line %u: Error parsing EDGE2 line: '%s'", lineNum,
463  lin.c_str()));
464 
465  // EQUIV? Replace ID by new one.
466  {
467  const auto itEq = lstEquivs.find(to_id);
468  if (itEq != lstEquivs.end()) to_id = itEq->second;
469  }
470  {
471  const auto itEq = lstEquivs.find(from_id);
472  if (itEq != lstEquivs.end()) from_id = itEq->second;
473  }
474 
475  if (from_id != to_id) // Don't load self-edges! (probably come
476  // from an EQUIV)
477  {
478  TPose2D Ap_mean;
479  mrpt::math::CMatrixDouble33 Ap_cov_inv;
480  if (!(s >> Ap_mean.x >> Ap_mean.y >> Ap_mean.phi >>
481  Ap_cov_inv(0, 0) >> Ap_cov_inv(0, 1) >>
482  Ap_cov_inv(0, 2) >> Ap_cov_inv(1, 1) >>
483  Ap_cov_inv(1, 2) >> Ap_cov_inv(2, 2)))
485  "Line %u: Error parsing EDGE2 line: '%s'", lineNum,
486  lin.c_str()));
487 
488  // Complete low triangular part of inf matrix:
489  Ap_cov_inv(1, 0) = Ap_cov_inv(0, 1);
490  Ap_cov_inv(2, 0) = Ap_cov_inv(0, 2);
491  Ap_cov_inv(2, 1) = Ap_cov_inv(1, 2);
492 
493  // Convert to 2D cov, 3D cov or 3D inv_cov as needed:
494  typename CNetworkOfPoses<CPOSE>::edge_t newEdge;
496  newEdge,
497  CPosePDFGaussianInf(CPose2D(Ap_mean), Ap_cov_inv));
498  g->insertEdge(from_id, to_id, newEdge);
499  }
500  }
501  else if (strCmpI(key, "EDGE3"))
502  {
503  if (!graph_is_3D)
505  "Line %u: Try to load EDGE3 into a 2D graph: '%s'",
506  lineNum, lin.c_str()));
507 
508  // EDGE3 from_id to_id Ax Ay Az Aroll Apitch Ayaw inf_11 inf_12
509  // .. inf_16 inf_22 .. inf_66
510  TNodeID to_id, from_id;
511  if (!(s >> from_id >> to_id))
513  "Line %u: Error parsing EDGE3 line: '%s'", lineNum,
514  lin.c_str()));
515 
516  // EQUIV? Replace ID by new one.
517  {
518  const auto itEq = lstEquivs.find(to_id);
519  if (itEq != lstEquivs.end()) to_id = itEq->second;
520  }
521  {
522  const auto itEq = lstEquivs.find(from_id);
523  if (itEq != lstEquivs.end()) from_id = itEq->second;
524  }
525 
526  if (from_id != to_id) // Don't load self-edges! (probably come
527  // from an EQUIV)
528  {
529  TPose3D Ap_mean;
530  mrpt::math::CMatrixDouble66 Ap_cov_inv;
531  // **CAUTION** In the TORO graph format angles are in the
532  // RPY order vs. MRPT's YPR.
533  if (!(s >> Ap_mean.x >> Ap_mean.y >> Ap_mean.z >>
534  Ap_mean.roll >> Ap_mean.pitch >> Ap_mean.yaw))
536  "Line %u: Error parsing EDGE3 line: '%s'", lineNum,
537  lin.c_str()));
538 
539  // **CAUTION** Indices are shuffled to the change YAW(3) <->
540  // ROLL(5) in the order of the data.
541  if (!(s >> Ap_cov_inv(0, 0) >> Ap_cov_inv(0, 1) >>
542  Ap_cov_inv(0, 2) >> Ap_cov_inv(0, 5) >>
543  Ap_cov_inv(0, 4) >> Ap_cov_inv(0, 3) >>
544  Ap_cov_inv(1, 1) >> Ap_cov_inv(1, 2) >>
545  Ap_cov_inv(1, 5) >> Ap_cov_inv(1, 4) >>
546  Ap_cov_inv(1, 3) >> Ap_cov_inv(2, 2) >>
547  Ap_cov_inv(2, 5) >> Ap_cov_inv(2, 4) >>
548  Ap_cov_inv(2, 3) >> Ap_cov_inv(5, 5) >>
549  Ap_cov_inv(5, 4) >> Ap_cov_inv(5, 3) >>
550  Ap_cov_inv(4, 4) >> Ap_cov_inv(4, 3) >>
551  Ap_cov_inv(3, 3)))
552  {
553  // Cov may be omitted in the file:
554  Ap_cov_inv.setIdentity();
555 
556  if (alreadyWarnedUnknowns.find("MISSING_3D") ==
557  alreadyWarnedUnknowns.end())
558  {
559  alreadyWarnedUnknowns.insert("MISSING_3D");
560  cerr << "[CNetworkOfPoses::loadFromTextFile] "
561  << fil << ":" << lineNum
562  << ": Warning: Information matrix missing, "
563  "assuming unity.\n";
564  }
565  }
566  else
567  {
568  // Complete low triangular part of inf matrix:
569  for (size_t r = 1; r < 6; r++)
570  for (size_t c = 0; c < r; c++)
571  Ap_cov_inv(r, c) = Ap_cov_inv(c, r);
572  }
573 
574  // Convert as needed:
575  typename CNetworkOfPoses<CPOSE>::edge_t newEdge;
577  newEdge,
578  CPose3DPDFGaussianInf(CPose3D(Ap_mean), Ap_cov_inv));
579  g->insertEdge(from_id, to_id, newEdge);
580  }
581  }
582  else if (strCmpI(key, "EDGE_SE3:QUAT"))
583  {
584  if (!graph_is_3D)
586  "Line %u: Try to load EDGE3 into a 2D graph: '%s'",
587  lineNum, lin.c_str()));
588 
589  // EDGE_SE3:QUAT from_id to_id Ax Ay Az qx qy qz qw inf_11
590  // inf_12 .. inf_16 inf_22 .. inf_66
591  // EDGE3 from_id to_id Ax Ay Az Aroll Apitch Ayaw inf_11 inf_12
592  // .. inf_16 inf_22 .. inf_66
593  TNodeID to_id, from_id;
594  if (!(s >> from_id >> to_id))
596  "Line %u: Error parsing EDGE_SE3:QUAT line: '%s'",
597  lineNum, lin.c_str()));
598 
599  // EQUIV? Replace ID by new one.
600  {
601  const auto itEq = lstEquivs.find(to_id);
602  if (itEq != lstEquivs.end()) to_id = itEq->second;
603  }
604  {
605  const auto itEq = lstEquivs.find(from_id);
606  if (itEq != lstEquivs.end()) from_id = itEq->second;
607  }
608 
609  if (from_id != to_id) // Don't load self-edges! (probably come
610  // from an EQUIV)
611  {
612  TPose3DQuat Ap_mean;
613  mrpt::math::CMatrixDouble66 Ap_cov_inv;
614  if (!(s >> Ap_mean.x >> Ap_mean.y >> Ap_mean.z >>
615  Ap_mean.qx >> Ap_mean.qy >> Ap_mean.qz >> Ap_mean.qr))
617  "Line %u: Error parsing EDGE_SE3:QUAT line: "
618  "'%s'",
619  lineNum, lin.c_str()));
620 
621  // **CAUTION** Indices are shuffled to the change YAW(3) <->
622  // ROLL(5) in the order of the data.
623  if (!(s >> Ap_cov_inv(0, 0) >> Ap_cov_inv(0, 1) >>
624  Ap_cov_inv(0, 2) >> Ap_cov_inv(0, 5) >>
625  Ap_cov_inv(0, 4) >> Ap_cov_inv(0, 3) >>
626  Ap_cov_inv(1, 1) >> Ap_cov_inv(1, 2) >>
627  Ap_cov_inv(1, 5) >> Ap_cov_inv(1, 4) >>
628  Ap_cov_inv(1, 3) >> Ap_cov_inv(2, 2) >>
629  Ap_cov_inv(2, 5) >> Ap_cov_inv(2, 4) >>
630  Ap_cov_inv(2, 3) >> Ap_cov_inv(5, 5) >>
631  Ap_cov_inv(5, 4) >> Ap_cov_inv(5, 3) >>
632  Ap_cov_inv(4, 4) >> Ap_cov_inv(4, 3) >>
633  Ap_cov_inv(3, 3)))
634  {
635  // Cov may be omitted in the file:
636  Ap_cov_inv.setIdentity();
637 
638  if (alreadyWarnedUnknowns.find("MISSING_3D") ==
639  alreadyWarnedUnknowns.end())
640  {
641  alreadyWarnedUnknowns.insert("MISSING_3D");
642  cerr << "[CNetworkOfPoses::loadFromTextFile] "
643  << fil << ":" << lineNum
644  << ": Warning: Information matrix missing, "
645  "assuming unity.\n";
646  }
647  }
648  else
649  {
650  // Complete low triangular part of inf matrix:
651  for (size_t r = 1; r < 6; r++)
652  for (size_t c = 0; c < r; c++)
653  Ap_cov_inv(r, c) = Ap_cov_inv(c, r);
654  }
655 
656  // Convert as needed:
657  typename CNetworkOfPoses<CPOSE>::edge_t newEdge;
659  newEdge,
661  CPose3D(CPose3DQuat(Ap_mean)), Ap_cov_inv));
662  g->insertEdge(from_id, to_id, newEdge);
663  }
664  }
665  else if (strCmpI(key, "EQUIV"))
666  {
667  // Already read in the 1st pass.
668  }
669  else if (strCmpI(key, "FIX"))
670  {
671  TNodeID id;
672  if (!(s >> id))
674  "Line %u: Can't read id in FIX line: '%s'", lineNum,
675  lin.c_str()));
676  g->root = id;
677  }
678  else
679  { // Unknown entry: Warn the user just once:
680  if (alreadyWarnedUnknowns.find(key) ==
681  alreadyWarnedUnknowns.end())
682  {
683  alreadyWarnedUnknowns.insert(key);
684  cerr << "[CNetworkOfPoses::loadFromTextFile] " << fil << ":"
685  << lineNum << ": Warning: unknown entry type: " << key
686  << endl;
687  }
688  }
689  } // end while
690 
691  } // end load_graph
692 
694  graph_t* g, const std::string& fil)
695  {
696  std::ifstream f(fil);
697  if (!f.is_open())
699  "Error opening file '%s' for reading", fil.c_str());
700  load_graph_of_poses_from_text_stream(g, f, fil);
701  }
702 
703  // --------------------------------------------------------------------------------
704  // Implements: collapseDuplicatedEdges
705  //
706  // Look for duplicated edges (even in opposite directions) between all pairs
707  // of nodes and fuse them.
708  // Upon return, only one edge remains between each pair of nodes with the
709  // mean
710  // & covariance (or information matrix) corresponding to the Bayesian
711  // fusion of all the Gaussians.
712  // --------------------------------------------------------------------------------
713  static size_t graph_of_poses_collapse_dup_edges(graph_t* g)
714  {
715  MRPT_START
716  using TEdgeIterator = typename graph_t::edges_map_t::iterator;
717 
718  // Data structure: (id1,id2) -> all edges between them
719  // (with id1 < id2)
720  using TListAllEdges =
721  map<pair<TNodeID, TNodeID>,
722  vector<TEdgeIterator>>; // For god's sake... when will ALL
723  // compilers support
724  // auto!! :'-(
725  TListAllEdges lstAllEdges;
726 
727  // Clasify all edges to identify duplicated ones:
728  for (auto itEd = g->edges.begin(); itEd != g->edges.end(); ++itEd)
729  {
730  // Build a pair <id1,id2> with id1 < id2:
731  const pair<TNodeID, TNodeID> arc_id = make_pair(
732  std::min(itEd->first.first, itEd->first.second),
733  std::max(itEd->first.first, itEd->first.second));
734  // get (or create the first time) the list of edges between them:
735  vector<TEdgeIterator>& lstEdges = lstAllEdges[arc_id];
736  // And add this one:
737  lstEdges.push_back(itEd);
738  }
739 
740  // Now, remove all but the first edge:
741  size_t nRemoved = 0;
742  for (auto it = lstAllEdges.begin(); it != lstAllEdges.end(); ++it)
743  {
744  const size_t N = it->second.size();
745  for (size_t i = 1; i < N; i++) // i=0 is NOT removed
746  g->edges.erase(it->second[i]);
747 
748  if (N >= 2) nRemoved += N - 1;
749  }
750 
751  return nRemoved;
752  MRPT_END
753  } // end of graph_of_poses_collapse_dup_edges
754 
755  // --------------------------------------------------------------------------------
756  // Implements: dijkstra_nodes_estimate
757  //
758  // Compute a simple estimation of the global coordinates of each node just
759  // from the information in all edges, sorted in a Dijkstra tree based on the
760  // current "root" node.
761  // Note that "global" coordinates are with respect to the node with the ID
762  // specified in \a root.
763  // --------------------------------------------------------------------------------
765  graph_t* g, std::map<TNodeID, size_t>* topological_distances = nullptr)
766  {
767  MRPT_START;
768  using namespace std;
769 
770  // Do Dijkstra shortest path from "root" to all other nodes:
771  using dijkstra_t =
773  using constraint_t = typename graph_t::constraint_t;
774 
775  // initialize corresponding dijkstra object from root.
776  dijkstra_t dijkstra(*g, g->root);
777 
778  // Get the tree representation of the graph and traverse it
779  // from its root toward the leafs:
780  typename dijkstra_t::tree_graph_t treeView;
781  dijkstra.getTreeGraph(treeView);
782 
783  // This visitor class performs the real job of
784  struct VisitorComputePoses : public dijkstra_t::tree_graph_t::Visitor
785  {
786  graph_t* m_g; // The original graph
787  std::map<TNodeID, size_t>* m_topo_dists{nullptr};
788 
789  VisitorComputePoses(graph_t* g) : m_g(g) {}
790  void OnVisitNode(
791  const TNodeID parent_id,
792  const typename dijkstra_t::tree_graph_t::Visitor::tree_t::
793  TEdgeInfo& edge_to_child,
794  const size_t depth_level) override
795  {
796  MRPT_UNUSED_PARAM(depth_level);
797  const TNodeID child_id = edge_to_child.id;
798 
799  // topological distance:
800  if (m_topo_dists) (*m_topo_dists)[child_id] = depth_level;
801 
802  // Compute the pose of "child_id" as parent_pose (+)
803  // edge_delta_pose,
804  // taking into account that that edge may be in reverse order
805  // and then have to invert the delta_pose:
806  if ((!edge_to_child.reverse &&
807  !m_g->edges_store_inverse_poses) ||
808  (edge_to_child.reverse && m_g->edges_store_inverse_poses))
809  { // pose_child = p_parent (+) p_delta
810  m_g->nodes[child_id].composeFrom(
811  m_g->nodes[parent_id],
812  edge_to_child.data->getPoseMean());
813  }
814  else
815  { // pose_child = p_parent (+) [(-)p_delta]
816  m_g->nodes[child_id].composeFrom(
817  m_g->nodes[parent_id],
818  -edge_to_child.data->getPoseMean());
819  }
820  }
821  };
822 
823  // Remove all global poses except for the root node, which is the
824  // origin:
825  //
826  // Keep track of the NODE_ANNOTATIONS for each node and put it after
827  // the global pose computation
828  bool empty_node_annots = g->nodes.begin()->second.is_node_annots_empty;
829  map<const TNodeID, TNodeAnnotations*> nodeID_to_annots;
830  if (!empty_node_annots)
831  {
832  for (auto poses_cit = g->nodes.begin(); poses_cit != g->nodes.end();
833  ++poses_cit)
834  {
835  nodeID_to_annots.insert(make_pair(
836  poses_cit->first, poses_cit->second.getCopyOfAnnots()));
837  }
838  }
839 
840  g->nodes.clear();
841  g->nodes[g->root] =
842  typename constraint_t::type_value(); // Typ: CPose2D() or CPose3D()
843 
844  // Run the visit thru all nodes in the tree:
845  VisitorComputePoses myVisitor(g);
846  myVisitor.m_topo_dists = topological_distances;
847  treeView.visitBreadthFirst(treeView.root, myVisitor);
848 
849  // The distance of "root" is zero (it's not visited in the loop above!):
850  if (topological_distances) (*topological_distances)[g->root] = 0;
851 
852  // Fill the NODE_ANNOTATIONS part again
853  if (!empty_node_annots)
854  {
855  for (auto poses_cit = g->nodes.begin(); poses_cit != g->nodes.end();
856  ++poses_cit)
857  {
858  TNodeAnnotations* node_annots =
859  nodeID_to_annots.at(poses_cit->first);
860  bool res = poses_cit->second.setAnnots(*node_annots);
861 
862  // free dynamically allocated mem
863  delete node_annots;
864  node_annots = nullptr;
865 
866  // make sure setting annotations was successful
867  ASSERTMSG_(
868  res, mrpt::format(
869  "Setting annotations for nodeID \"%lu\" was "
870  "unsuccessful",
871  static_cast<unsigned long>(poses_cit->first)));
872  }
873  }
874 
875  MRPT_END
876  } // end of graph_of_poses_dijkstra_init
877 
878  // Auxiliary funcs:
879  template <class VEC>
880  static inline double auxMaha2Dist(VEC& err, const CPosePDFGaussianInf& p)
881  {
882  math::wrapToPiInPlace(err[2]);
883  return mrpt::math::multiply_HtCH_scalar(err, p.cov_inv);
884  }
885  template <class VEC>
886  static inline double auxMaha2Dist(VEC& err, const CPose3DPDFGaussianInf& p)
887  {
888  math::wrapToPiInPlace(err[3]);
889  math::wrapToPiInPlace(err[4]);
890  math::wrapToPiInPlace(err[5]);
891  return mrpt::math::multiply_HtCH_scalar(err, p.cov_inv);
892  }
893  template <class VEC>
894  static inline double auxMaha2Dist(VEC& err, const CPosePDFGaussian& p)
895  {
896  math::wrapToPiInPlace(err[2]);
897  // err^t*cov_inv*err
898  return mrpt::math::multiply_HCHt_scalar(err, p.cov.inverse_LLt());
899  }
900  template <class VEC>
901  static inline double auxMaha2Dist(VEC& err, const CPose3DPDFGaussian& p)
902  {
903  math::wrapToPiInPlace(err[3]);
904  math::wrapToPiInPlace(err[4]);
905  math::wrapToPiInPlace(err[5]);
906  // err^t*cov_inv*err
907  return mrpt::math::multiply_HtCH_scalar(err, p.cov.inverse_LLt());
908  }
909  // These two are for simulating maha2 distances for non-PDF types: fallback
910  // to squared-norm:
911  template <class VEC>
912  static inline double auxMaha2Dist(VEC& err, const mrpt::poses::CPose2D& p)
913  {
914  math::wrapToPiInPlace(err[2]);
915  return square(err[0]) + square(err[1]) + square(err[2]);
916  }
917  template <class VEC>
918  static inline double auxMaha2Dist(VEC& err, const mrpt::poses::CPose3D& p)
919  {
920  math::wrapToPiInPlace(err[3]);
921  math::wrapToPiInPlace(err[4]);
922  math::wrapToPiInPlace(err[5]);
923  return square(err[0]) + square(err[1]) + square(err[2]) +
924  square(err[3]) + square(err[4]) + square(err[5]);
925  }
926 
927  static inline double auxEuclid2Dist(
928  const mrpt::poses::CPose2D& p1, const mrpt::poses::CPose2D& p2)
929  {
930  return square(p1.x() - p2.x()) + square(p1.y() - p2.y()) +
931  square(mrpt::math::wrapToPi(p1.phi() - p2.phi()));
932  }
933  static inline double auxEuclid2Dist(
934  const mrpt::poses::CPose3D& p1, const mrpt::poses::CPose3D& p2)
935  {
936  return square(p1.x() - p2.x()) + square(p1.y() - p2.y()) +
937  square(p1.z() - p2.z()) +
938  square(mrpt::math::wrapToPi(p1.yaw() - p2.yaw())) +
939  square(mrpt::math::wrapToPi(p1.pitch() - p2.pitch())) +
940  square(mrpt::math::wrapToPi(p1.roll() - p2.roll()));
941  }
942 
943  // --------------------------------------------------------------------------------
944  // Implements: detail::graph_edge_sqerror
945  //
946  // Compute the square error of a single edge, in comparison to the nodes
947  // global poses.
948  // --------------------------------------------------------------------------------
949  static double graph_edge_sqerror(
950  const graph_t* g,
951  const typename mrpt::graphs::CDirectedGraph<
952  typename graph_t::constraint_t>::edges_map_t::const_iterator&
953  itEdge,
954  bool ignoreCovariances)
955  {
956  MRPT_START
957 
958  // Get node IDs:
959  const TNodeID from_id = itEdge->first.first;
960  const TNodeID to_id = itEdge->first.second;
961 
962  // And their global poses as stored in "nodes"
963  auto itPoseFrom = g->nodes.find(from_id);
964  auto itPoseTo = g->nodes.find(to_id);
965  ASSERTMSG_(
966  itPoseFrom != g->nodes.end(),
967  format(
968  "Node %u doesn't have a global pose in 'nodes'.",
969  static_cast<unsigned int>(from_id)));
970  ASSERTMSG_(
971  itPoseTo != g->nodes.end(),
972  format(
973  "Node %u doesn't have a global pose in 'nodes'.",
974  static_cast<unsigned int>(to_id)));
975 
976  // The global poses:
977  using constraint_t = typename graph_t::constraint_t;
978 
979  const typename constraint_t::type_value& from_mean = itPoseFrom->second;
980  const typename constraint_t::type_value& to_mean = itPoseTo->second;
981 
982  // The delta_pose as stored in the edge:
983  const constraint_t& edge_delta_pose = itEdge->second;
984  const typename constraint_t::type_value& edge_delta_pose_mean =
985  edge_delta_pose.getPoseMean();
986 
987  if (ignoreCovariances)
988  { // Square Euclidean distance: Just use the mean values, ignore covs.
989  // from_plus_delta = from_mean (+) edge_delta_pose_mean
990  typename constraint_t::type_value from_plus_delta(
992  from_plus_delta.composeFrom(from_mean, edge_delta_pose_mean);
993 
994  // (auxMaha2Dist will also take into account the 2PI wrapping)
995  return auxEuclid2Dist(from_plus_delta, to_mean);
996  }
997  else
998  {
999  // Square Mahalanobis distance
1000  // from_plus_delta = from_mean (+) edge_delta_pose (as a Gaussian)
1001  constraint_t from_plus_delta = edge_delta_pose;
1002  from_plus_delta.changeCoordinatesReference(from_mean);
1003 
1004  // "from_plus_delta" is now a 3D or 6D Gaussian, to be compared to
1005  // "to_mean":
1006  // We want to compute the squared Mahalanobis distance:
1007  // err^t * INV_COV * err
1008  //
1010  constraint_t::type_value::static_size>
1011  err;
1012  for (size_t i = 0; i < constraint_t::type_value::static_size; i++)
1013  err[i] = from_plus_delta.getPoseMean()[i] - to_mean[i];
1014 
1015  // (auxMaha2Dist will also take into account the 2PI wrapping)
1016  return auxMaha2Dist(err, from_plus_delta);
1017  }
1018  MRPT_END
1019  }
1020 
1021 }; // end of graph_ops<graph_t>
1022 
1023 } // namespace mrpt::graphs::detail
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)
static void save_graph_of_poses_to_binary_file(const graph_t *g, mrpt::serialization::CArchive &out)
MAT_C::Scalar multiply_HtCH_scalar(const VECTOR_H &H, const MAT_C &C)
r (scalar) = H^t*C*H (H: column vector, C: symmetric matrix)
Definition: ops_matrices.h:54
#define MRPT_START
Definition: exceptions.h:241
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPosePDFGaussianInf &edge, std::ostream &f)
double x
X,Y coordinates.
Definition: TPose2D.h:30
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
Abstract class from which NodeAnnotations related classes can be implemented.
double roll
Roll coordinate (rotation angle over X coordinate).
Definition: TPose3D.h:37
static double auxMaha2Dist(VEC &err, const mrpt::poses::CPose2D &p)
Abstract graph and tree data structures, plus generic graph algorithms.
GLenum GLsizei n
Definition: glext.h:5136
double x
X,Y,Z, coords.
Definition: TPose3D.h:31
mrpt::math::CMatrixDouble66 cov_inv
The inverse of the 6x6 covariance matrix.
A class for parsing text files, returning each non-empty and non-comment line, along its line number...
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:548
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:542
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 (scalar) = H*C*H^t (H: row vector, C: symmetric matrix)
Definition: ops_matrices.h:63
STL namespace.
double yaw
Yaw coordinate (rotation angle over Z axis).
Definition: TPose3D.h:33
static double auxMaha2Dist(VEC &err, const CPosePDFGaussianInf &p)
static void read_graph_of_poses_from_binary_file(graph_t *g, mrpt::serialization::CArchive &in)
GLdouble s
Definition: glext.h:3682
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)
A directed graph of pose constraints, with edges being the relative poses between pairs of nodes iden...
static void copyFrom2D(CPose2D &p, const CPosePDFGaussianInf &pdf)
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPose3DPDFGaussian &edge, std::ostream &f)
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
void rewind()
Reset the read pointer to the beginning of the file.
size_t getCurrentLineNumber() const
Return the line number of the last line returned with getNextLine.
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:61
Internal functions for MRPT.
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.
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPosePDFGaussian &edge, std::ostream &f)
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
T square(const T x)
Inline function for the square of a number.
static void copyFrom2D(POSE_PDF &p, const CPosePDFGaussianInf &pdf)
const GLubyte * c
Definition: glext.h:6406
static void copyFrom2D(CPose3D &p, const CPosePDFGaussianInf &pdf)
double x
Translation in x,y,z.
Definition: TPose3DQuat.h:27
GLubyte g
Definition: glext.h:6372
static double auxMaha2Dist(VEC &err, const CPose3DPDFGaussian &p)
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
bool getNextLine(std::string &out_str)
Reads from the file and return the next (non-comment) line, as a std::string.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:45
GLsizei const GLchar ** string
Definition: glext.h:4116
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
double qr
Unit quaternion part, qr,qx,qy,qz.
Definition: TPose3DQuat.h:29
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:554
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).
Definition: TPose3D.h:35
Lightweight 3D pose (three spatial coordinates, plus a quaternion ).
Definition: TPose3DQuat.h:19
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)
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...
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
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:39
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:84
mrpt::vision::TStereoCalibResults out
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:86
GLuint id
Definition: glext.h:3920
#define MRPT_END
Definition: exceptions.h:245
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).
Definition: TPose3D.h:23
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
GLuint in
Definition: glext.h:7391
Lightweight 2D pose.
Definition: TPose2D.h:22
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
Definition: TNodeID.h:16
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
static void graph_of_poses_dijkstra_init(graph_t *g, std::map< TNodeID, size_t > *topological_distances=nullptr)
static void load_graph_of_poses_from_text_file(graph_t *g, const std::string &fil)
std::pair< TNodeID, TNodeID > TPairNodeIDs
A pair of node IDs.
Definition: TNodeID.h:18
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:97
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPose3DPDFGaussianInf &edge, std::ostream &f)
GLuint res
Definition: glext.h:7385
static void copyFrom3D(CPose2D &p, const CPose3DPDFGaussianInf &pdf)
static void copyFrom3D(POSE_PDF &p, const CPose3DPDFGaussianInf &pdf)
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
bool strCmpI(const std::string &s1, const std::string &s2)
Return true if the two strings are equal (case insensitive)
static void write_VERTEX_line(const TNodeID id, const mrpt::poses::CPose3D &p, std::ostream &f)
GLfloat GLfloat p
Definition: glext.h:6398
double phi
Orientation (rads)
Definition: TPose2D.h:32
mrpt::math::CMatrixDouble33 cov_inv
The inverse of the 3x3 covariance matrix (the "information" matrix)
static constexpr auto get()
Definition: TTypeName.h:71
static double auxMaha2Dist(VEC &err, const mrpt::poses::CPose3D &p)
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: e47402b84 Wed Oct 23 01:09:07 2019 +0200 at miƩ oct 23 01:10:13 CEST 2019