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


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019