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


Page generated by Doxygen 1.8.14 for MRPT 1.5.5 Git: e06b63dbf Fri Dec 1 14:41:11 2017 +0100 at lun oct 28 01:31:35 CET 2019