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>
16 #include <mrpt/math/wrap2pi.h>
17 #include <mrpt/math/ops_matrices.h> // multiply_*()
20 #include <mrpt/poses/CPose2D.h>
21 #include <mrpt/poses/CPose3D.h>
22 #include <mrpt/poses/CPose3DQuat.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  {
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  {
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  {
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  {
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
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  // =================================================================
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  // =================================================================
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;
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:
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) {
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) {
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
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]);
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]);
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()))+
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,
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  //
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
#define ASSERT_EQUAL_(__A, __B)
A directed graph with the argument of the template specifying the type of the annotations in the edge...
static double auxMaha2Dist(VEC &err, const CPosePDFGaussian &p)
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
#define min(a, b)
A template to obtain the type of its argument as a string at compile time.
Definition: TTypeName.h:47
Abstract class from which NodeAnnotations related classes can be implemented.
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
bool getNextLine(std::string &out_str)
Reads from the file and return the next (non-comment) line, as a std::string.
double roll
Roll coordinate (rotation angle over X coordinate).
static double auxMaha2Dist(VEC &err, const mrpt::poses::CPose2D &p)
#define THROW_EXCEPTION(msg)
Abstract graph and tree data structures, plus generic graph algorithms.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Scalar * iterator
Definition: eigen_plugins.h:23
mrpt::math::CMatrixDouble66 cov_inv
The inverse of the 6x6 covariance matrix.
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:392
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:391
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
MAT_C::Scalar multiply_HCHt_scalar(const VECTOR_H &H, const MAT_C &C)
r (a scalar) = H * C * H^t (with a vector H and a symmetric matrix C)
Definition: ops_matrices.h:62
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPose3DPDFGaussianInf &edge, std::ofstream &f)
STL namespace.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
double yaw
Yaw coordinate (rotation angle over Z axis).
static double auxMaha2Dist(VEC &err, const CPosePDFGaussianInf &p)
static void read_graph_of_poses_from_binary_file(graph_t *g, mrpt::utils::CStream &in)
GLdouble s
Definition: glext.h:3602
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPose3DPDFGaussian &edge, std::ofstream &f)
static double auxEuclid2Dist(const mrpt::poses::CPose2D &p1, const mrpt::poses::CPose2D &p2)
static double auxEuclid2Dist(const mrpt::poses::CPose3D &p1, const mrpt::poses::CPose3D &p2)
static void graph_of_poses_dijkstra_init(graph_t *g)
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
A directed graph of pose constraints, with edges being the relative poses between pairs of nodes iden...
void rewind()
Reset the read pointer to the beginning of the file.
static void copyFrom2D(CPose2D &p, const CPosePDFGaussianInf &pdf)
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPosePDFGaussian &edge, std::ofstream &f)
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:61
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
static void save_graph_of_poses_to_binary_file(const graph_t *g, mrpt::utils::CStream &out)
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
uint64_t TNodeID
The type for node IDs in graphs of different types.
#define MRPT_END
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const mrpt::poses::CPose3D &edge, std::ofstream &f)
static void copyFrom2D(POSE_PDF &p, const CPosePDFGaussianInf &pdf)
const GLubyte * c
Definition: glext.h:5590
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
static void copyFrom2D(CPose3D &p, const CPosePDFGaussianInf &pdf)
double z
Translation in x,y,z.
GLubyte g
Definition: glext.h:5575
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
std::pair< TNodeID, TNodeID > TPairNodeIDs
A pair of node IDs.
static double auxMaha2Dist(VEC &err, const CPose3DPDFGaussian &p)
int version
Definition: mrpt_jpeglib.h:898
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
A class for parsing text files, returning each non-empty and non-comment line, along its line number...
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
double qz
Unit quaternion part, qr,qx,qy,qz.
GLsizei const GLchar ** string
Definition: glext.h:3919
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:393
double pitch
Pitch coordinate (rotation angle over Y axis).
Lightweight 3D pose (three spatial coordinates, plus a quaternion ).
static size_t graph_of_poses_collapse_dup_edges(graph_t *g)
static void save_graph_of_poses_to_text_file(const graph_t *g, const std::string &fil)
double y
X,Y coordinates.
#define MRPT_START
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
This file implements matrix/vector text and binary serialization.
static double graph_edge_sqerror(const graph_t *g, const typename mrpt::graphs::CDirectedGraph< typename graph_t::constraint_t >::edges_map_t::const_iterator &itEdge, bool ignoreCovariances)
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
GLuint id
Definition: glext.h:3770
static void copyFrom3D(CPose3D &p, const CPose3DPDFGaussianInf &pdf)
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
GLuint in
Definition: glext.h:6301
Lightweight 2D pose.
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:74
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
size_t getCurrentLineNumber() const
Return the line number of the last line returned with getNextLine.
static void load_graph_of_poses_from_text_file(graph_t *g, const std::string &fil)
global_poses_t nodes
The nodes (vertices) of the graph, with their estimated "global" (with respect to root) position...
static double auxMaha2Dist(VEC &err, const CPose3DPDFGaussianInf &p)
The Dijkstra algorithm for finding the shortest path between a given source node in a (weighted) dire...
Definition: dijkstra.h:103
GLuint res
Definition: glext.h:6298
static void copyFrom3D(CPose2D &p, const CPose3DPDFGaussianInf &pdf)
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const mrpt::poses::CPose2D &edge, std::ofstream &f)
static void copyFrom3D(POSE_PDF &p, const CPose3DPDFGaussianInf &pdf)
bool BASE_IMPEXP strCmpI(const std::string &s1, const std::string &s2)
Return true if the two strings are equal (case insensitive)
unsigned __int32 uint32_t
Definition: rptypes.h:49
static void write_VERTEX_line(const TNodeID id, const mrpt::poses::CPose2D &p, std::ofstream &f)
#define ASSERTMSG_(f, __ERROR_MSG)
GLfloat GLfloat p
Definition: glext.h:5587
double z
X,Y,Z, coords.
double phi
Orientation (rads)
mrpt::math::CMatrixDouble33 cov_inv
The inverse of the 3x3 covariance matrix (the "information" matrix)
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPosePDFGaussianInf &edge, std::ofstream &f)
static void write_VERTEX_line(const TNodeID id, const mrpt::poses::CPose3D &p, std::ofstream &f)
static double auxMaha2Dist(VEC &err, const mrpt::poses::CPose3D &p)



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