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


Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST